diff --git a/README-ZED WITH OPT INSTALL ONLY FOLLOW THIS IF USING ZED.txt b/README-ZED WITH OPT INSTALL ONLY FOLLOW THIS IF USING ZED.txt new file mode 100644 index 0000000..bdd0fbb --- /dev/null +++ b/README-ZED WITH OPT INSTALL ONLY FOLLOW THIS IF USING ZED.txt @@ -0,0 +1,119 @@ +Built for Ubuntu 14.04 with Ros Indigo + +---------------------------------INSTALLING OPT WITH ZED-------------------------------------------------------- + +First Install Cuda 7.5 Toolkit: + +Using below link: +https://developer.nvidia.com/cuda-75-downloads-archive + +For Options Select: +Linux +x86_64 +Ubuntu +14.04 +deb (local) + +download cuda-repo-ubuntu1404-7-5-local package + +After downloaded: +In terminal go to folder containing cuda package +$ sudo dpkg -i cuda-repo-ubuntu1404-7-5-local_7.5-18_amd64.deb +$ sudo apt-get update +$ sudo apt-get install cuda-7-5 + +After install, reboot computer +Verify Nvidia installed: +$ nvidia-settings + +and make sure there is an nvidia driver number + +Next make sure enviroment path is set: +$ export CUDA_HOME=/usr/local/cuda-7.5 +$ export LD_LIBRARY_PATH=${CUDA_HOME}/lib64 + +$ PATH=${CUDA_HOME}/bin:${PATH} +$ export PATH + +Finally verify install: +$ cuda-install-samples-7.5.sh ~ +$ cd ~/NVIDIA_CUDA-7.5_Samples +$ cd 1_Utilities/deviceQuery +$ make +$ ./deviceQuery + +The terminal then should output the cuda version info + +Next it is necessary to install ROS/OpenCV 2.0 (included in ROS): +from directory where OPT is copied currently +$ cd open_ptrack/scripts +$ chmod +x *.sh +$ ./ros_install.sh +$ source /opt/ros/indigo/setup.bash +$ ./ros_configure.sh + +OpenCV 3.1 is required for ZED to work (however it cannot conflict with OpenCV 2!! Therefore we will create a separate install path): +$ cd +$ sudo apt-get update + +$ sudo apt-get install libopencv-dev build-essential checkinstall cmake pkg-config yasm libtiff4-dev libjpeg-dev libjasper-dev libavcodec-dev libavformat-dev libswscale-dev libdc1394-22-dev libxine-dev libgstreamer0.10-dev libgstreamer-plugins-base0.10-dev libv4l-dev python-dev python-numpy libtbb-dev libqt4-dev libgtk2.0-dev libfaac-dev libmp3lame-dev libopencore-amrnb-dev libopencore-amrwb-dev libtheora-dev libvorbis-dev libxvidcore-dev x264 v4l-utils + +$ sudo add-apt-repository ppa:joyard-nicolas/ffmpeg +$ sudo apt-get update +$ sudo apt-get install ffmpeg +$ sudo apt-get install frei0r-plugins + +$ sudo mkdir /usr/local/opencv3 + +$ mkdir OpenCV +$ cd OpenCV +$ git clone https://github.com/opencv/opencv.git +$ cd opencv +$ git checkout tags/3.1.0 +$ mkdir release +$ cd release +$ cmake -D CMAKE_BUILD_TYPE=RELEASE -D CMAKE_INSTALL_PREFIX=/usr/local/opencv3 -D WITH_TBB=ON -D BUILD_NEW_PYTHON_SUPPORT=ON -D WITH_V4L=ON -D INSTALL_C_EXAMPLES=ON -D INSTALL_PYTHON_EXAMPLES=ON -D BUILD_EXAMPLES=ON -D WITH_QT=ON -D WITH_OPENGL=ON -D ENABLE_FAST_MATH=1 -D CUDA_FAST_MATH=1 -D WITH_CUBLAS=1 .. + +Note adjust the -j7 to the number of available cores on computer +$ make -j7 +$ sudo make install +This process WILL TAKE FOREVER... + +$ printf '# OpenCV\nPKG_CONFIG_PATH=$PKG_CONFIG_PATH:/usr/local/opencv3/lib/pkgconfig\nexport PKG_CONFIG_PATH\n' >> ~/.bashrc +$ source ~/.bashrc + +Now Time to Install ZED Camera Drivers: +Download ZED v1.1.0 SDK from: https://www.stereolabs.com/developers/release/archives/ +It is very important v1.1.0 is downloaded and not any other version + +First make sure ZED Camera is plugged into a POWERED USB 3.0 Slot +$ ./ZED_SDK_Linux_x86_64_v1.1.0.run +Press q to exit license agreement and accept all defaults + +Next you need to get the proper ZED ROS Wrapper: +$ cd +$ cd workspace/ros/catkin/src +$ git clone https://github.com/chanbrown007/zed_ros_wrapper_OPT_update.git + +Finally Time To Install OPT: +$ cd +$ cd open_ptrack/scripts +$ ./openptrack_install.sh +Note sometimes you have to run ./openptrack_install.sh twice for it to fully install +If there is a persistent error that appears to be linked with OpenCV it sometimes is then necessary to install OpenCV 2 in /usr/local (following same procedure as before) and make sure to add package configure (/usr/local/lib/pkgconfig) to path + +It is suggested to delete openp_track folder in home and then: +$ ln -s ~/workspace/ros/catkin/src/open_ptrack ~/open_ptrack + +------------------RUNNING OPT WITH ZED----------------------------------------------------- +To Run make Sure ZED IS PLUGGED IN: +$ roslaunch tracking detection_and_tracking_zed.launch + +In a 2nd terminal, tracking can be fine tuned using: +$ rosrun rqt_reconfigure rqt_reconfigure + +ALSO ZED Camera Settings Can Be Adjusted in workspace/ros/catkin/src/zed_ros_wrapper_OPT_update/launch/zed.launch: +Resolution Parameter: 0 = 2K, 1 = 1080HD, 2 = 720HD, 3 = VGA +Quality Parameter (Disparity Map Quality): 0 = none, 1 = performance, 2 = medium, 3 = high +Frame Rate: 2K has max frame rate of 15fps, 1080 - 30fps and 720 - 60fps +--------------------------------------------------------------------------------------------- diff --git a/detection/CMakeLists.txt b/detection/CMakeLists.txt index b030e6a..c5c43a7 100644 --- a/detection/CMakeLists.txt +++ b/detection/CMakeLists.txt @@ -11,11 +11,12 @@ if(VTK_FOUND) include (${VTK_USE_FILE}) endif() -find_package(OpenCV REQUIRED) +find_package(OpenCV 2.4 REQUIRED) include_directories(${OpenCV_INCLUDE_DIRS}) link_directories(${OpenCV_LIB_DIR}) add_definitions(${OpenCV_DEFINITIONS}) + find_package(Eigen REQUIRED) include_directories(${Eigen_INCLUDE_DIRS} include ${catkin_INCLUDE_DIRS}) diff --git a/detection/apps/ground_based_people_detector_node.cpp b/detection/apps/ground_based_people_detector_node.cpp index 3b8c58f..5c73cb0 100644 --- a/detection/apps/ground_based_people_detector_node.cpp +++ b/detection/apps/ground_based_people_detector_node.cpp @@ -74,6 +74,9 @@ #include #include +#include +#include + using namespace opt_msgs; using namespace sensor_msgs; @@ -115,6 +118,11 @@ bool background_subtraction; // Threshold on the ratio of valid points needed for ground estimation double valid_points_threshold; +//ros::Publisher extractedRGB_cloud; +//ros::Publisher beforeVoxelFilter_cloud; +//ros::Publisher preProcessed_cloud; +//ros::Publisher groundRemoval_cloud; + void cloud_cb (const PointCloudT::ConstPtr& callback_cloud) { @@ -143,6 +151,51 @@ updateBackgroundCallback (const std_msgs::String::ConstPtr & msg) } } +/* +//Publish Intermediate Point Cloud Calls +void +publishExtractRGBCloud(PointCloudT::Ptr& cloud) +{ + sensor_msgs::PointCloud2 output; + pcl::toROSMsg(*cloud, output); // Convert the point cloud to a ROS message + extractedRGB_cloud.publish(output); +} + +void +publishpreProcessedCloud(PointCloudT::Ptr& cloud) +{ + sensor_msgs::PointCloud2 output; + pcl::toROSMsg(*cloud, output); // Convert the point cloud to a ROS message + preProcessed_cloud.publish(output); +} + + +void +publishBeforeVoxelFilterCloud(PointCloudT::Ptr& cloud) +{ + sensor_msgs::PointCloud2 output; + pcl::toROSMsg(*cloud, output); // Convert the point cloud to a ROS message + beforeVoxelFilter_cloud.publish(output); +} + +void +publishBeforeVoxelFilterDummyCloud(PointCloudT::Ptr& cloud) +{ + //sensor_msgs::PointCloud2 output; + //pcl::toROSMsg(*cloud, output); // Convert the point cloud to a ROS message + //beforeVoxelFilter_cloud.publish(output); +} + + +void +publishgroundRemovalCloud(PointCloudT::Ptr& cloud) +{ + sensor_msgs::PointCloud2 output; + pcl::toROSMsg(*cloud, output); // Convert the point cloud to a ROS message + groundRemoval_cloud.publish(output); +} +*/ + void computeBackgroundCloud (int frames, float voxel_size, std::string frame_id, ros::Rate rate, PointCloudT::Ptr& background_cloud) { @@ -155,7 +208,7 @@ computeBackgroundCloud (int frames, float voxel_size, std::string frame_id, ros: { // Point cloud pre-processing (downsampling and filtering): PointCloudT::Ptr cloud_filtered(new PointCloudT); - cloud_filtered = people_detector.preprocessCloud (cloud); + cloud_filtered = people_detector.preprocessCloud (cloud);//, &publishBeforeVoxelFilterDummyCloud); *background_cloud += *cloud_filtered; ros::spinOnce(); @@ -316,6 +369,11 @@ main (int argc, char** argv) ros::Publisher detection_pub; detection_pub= nh.advertise(output_topic, 3); + //extractedRGB_cloud = nh.advertise (pointcloud_topic + "/extractedRGB", 3); + //preProcessed_cloud = nh.advertise (pointcloud_topic + "/preProcessed", 3); + //groundRemoval_cloud = nh.advertise (pointcloud_topic + "/groundRemoval", 3); + //beforeVoxelFilter_cloud = nh.advertise (pointcloud_topic + "/beforeVoxelFilter", 3); + Rois output_rois_; open_ptrack::opt_utils::Conversions converter; @@ -407,10 +465,8 @@ main (int argc, char** argv) if (new_cloud_available_flag) { new_cloud_available_flag = false; - // Convert PCL cloud header to ROS header: std_msgs::Header cloud_header = pcl_conversions::fromPCL(cloud->header); - // If requested, update background: if (update_background) { @@ -427,9 +483,11 @@ main (int argc, char** argv) // Perform people detection on the new cloud: std::vector > clusters; // vector containing persons clusters + + people_detector.setInputCloud(cloud); people_detector.setGround(ground_coeffs); // set floor coefficients - people_detector.compute(clusters); // perform people detection + people_detector.compute(clusters);//, &publishExtractRGBCloud, &publishpreProcessedCloud , &publishgroundRemovalCloud, &publishBeforeVoxelFilterCloud); // perform people detection // If not lock_ground, update ground coefficients: if (not lock_ground) diff --git a/detection/apps/haardispada_node.cpp b/detection/apps/haardispada_node.cpp index 071ff1a..54f0036 100644 --- a/detection/apps/haardispada_node.cpp +++ b/detection/apps/haardispada_node.cpp @@ -60,8 +60,8 @@ POSSIBILITY OF SUCH DAMAGE. #include // Used to display OPENCV images -#include -#include +#include +#include // Dynamic reconfigure: #include diff --git a/detection/apps/haardispada_node_sr.cpp b/detection/apps/haardispada_node_sr.cpp index 97194e2..6901233 100644 --- a/detection/apps/haardispada_node_sr.cpp +++ b/detection/apps/haardispada_node_sr.cpp @@ -60,8 +60,8 @@ POSSIBILITY OF SUCH DAMAGE. #include // Used to display OPENCV images -#include -#include +#include +#include using namespace stereo_msgs; using namespace message_filters::sync_policies; diff --git a/detection/conf/ground_based_people_detector_zed.yaml b/detection/conf/ground_based_people_detector_zed.yaml new file mode 100644 index 0000000..2690f18 --- /dev/null +++ b/detection/conf/ground_based_people_detector_zed.yaml @@ -0,0 +1,52 @@ +####################### +## Ground estimation ## +####################### +# Ground estimation mode - 0:manual, 1:semi-auto, 2:auto with user validation, 3:fully automatic (default 0) +ground_estimation_mode: 3 +# Flag stating if the ground should be read from file, if present: +read_ground_from_file: false +# Flag that locks the ground plane update: +lock_ground: false +# Threshold on the ratio of valid points needed for ground estimation: +valid_points_threshold: 0.1 + +############################ +## Background subtraction ## +############################ +# Flag enabling/disabling background subtraction: +background_subtraction: false +# Resolution of the octree representing the background: +background_resolution: 0.3 +# Seconds to use to learn the background: +background_seconds: 3.0 + +############################################## +## Ground based people detection parameters ## +############################################## +# Minimum detection confidence: +ground_based_people_detection_min_confidence: -5 +# Minimum person height: +minimum_person_height: 0.5 +# Maximum person height: +maximum_person_height: 2.3 +# Kinect's best range is 5.0 m, greater is possible w/ larger error +max_distance: 10.0 +# Point cloud downsampling factor: 4 +sampling_factor: 4 +# Flag stating if classifiers based on RGB image should be used or not: +use_rgb: true +# Threshold on image luminance. If luminance is over this threhsold, classifiers on RGB image are also used: +minimum_luminance: 120 +# If true, sensor tilt angle wrt ground plane is compensated to improve people detection: +sensor_tilt_compensation: flase +# Minimum distance between two persons: +heads_minimum_distance: 0.3 +# Voxel size used to downsample the point cloud (lower: detection slower but more precise; higher: detection faster but less precise): +voxel_size: .06 +# Denoising flag. If true, a statistical filter is applied to the point cloud to remove noise: +apply_denoising: false +# MeanK for denoising (the higher it is, the stronger is the filtering): +mean_k_denoising: 5 +# Standard deviation for denoising (the lower it is, the stronger is the filtering): +std_dev_denoising: 0.3 + diff --git a/detection/conf/ground_zed_tracked_frame.txt b/detection/conf/ground_zed_tracked_frame.txt new file mode 100644 index 0000000..ccd6555 --- /dev/null +++ b/detection/conf/ground_zed_tracked_frame.txt @@ -0,0 +1,4 @@ +2.94239e-07 +4.57398e-41 +1.49029e-38 +0 \ No newline at end of file diff --git a/detection/include/open_ptrack/detection/ground_based_people_detection_app.h b/detection/include/open_ptrack/detection/ground_based_people_detection_app.h index f91c1b0..63c5620 100644 --- a/detection/include/open_ptrack/detection/ground_based_people_detection_app.h +++ b/detection/include/open_ptrack/detection/ground_based_people_detection_app.h @@ -317,7 +317,7 @@ namespace open_ptrack * \return The cloud after pre-processing. */ PointCloudPtr - preprocessCloud (PointCloudPtr& input_cloud); + preprocessCloud (PointCloudPtr& input_cloud);//, void (*publishBeforeVoxelFilterCloud)(PointCloudPtr&)); /** * \brief Perform people detection on the input data and return people clusters information. @@ -327,7 +327,7 @@ namespace open_ptrack * \return true if the compute operation is successful, false otherwise. */ bool - compute (std::vector >& clusters); + compute (std::vector >& clusters);//, void (*publishExtractRGB)(PointCloudPtr&), void (*publishPreProcessed)(PointCloudPtr&), void (*publishGroundRemoval)(PointCloudPtr&), void (*publishBeforeVoxelFilterCloud)(PointCloudPtr&)); protected: /** \brief sampling factor used to downsample the point cloud */ diff --git a/detection/include/open_ptrack/detection/impl/ground_based_people_detection_app.hpp b/detection/include/open_ptrack/detection/impl/ground_based_people_detection_app.hpp index 25b2f7b..e6e612f 100644 --- a/detection/include/open_ptrack/detection/impl/ground_based_people_detection_app.hpp +++ b/detection/include/open_ptrack/detection/impl/ground_based_people_detection_app.hpp @@ -370,14 +370,21 @@ open_ptrack::detection::GroundBasedPeopleDetectionApp::preprocessCloud ( PointCloudPtr cloud_filtered(new PointCloud); pcl::VoxelGrid voxel_grid_filter_object; if (apply_denoising_) + { voxel_grid_filter_object.setInputCloud(cloud_denoised); + } else { if (sampling_factor_ != 1) + { voxel_grid_filter_object.setInputCloud(cloud_downsampled); + } else + { voxel_grid_filter_object.setInputCloud(input_cloud); + } } + voxel_grid_filter_object.setLeafSize (voxel_size_, voxel_size_, voxel_size_); voxel_grid_filter_object.setFilterFieldName("z"); voxel_grid_filter_object.setFilterLimits(0.0, max_distance_); @@ -476,6 +483,7 @@ open_ptrack::detection::GroundBasedPeopleDetectionApp::compute (std::vec PCL_INFO ("No groundplane update!\n"); } } + // Background Subtraction (optional): if (background_subtraction_) @@ -490,12 +498,34 @@ open_ptrack::detection::GroundBasedPeopleDetectionApp::compute (std::vec } no_ground_cloud_ = foreground_cloud; } + + + std::vector myvector; + + int count = 0; + for(pcl::PointCloud::iterator it = no_ground_cloud_->begin(); it != no_ground_cloud_->end(); it++) + { + if (!std::isfinite (it->x) || !std::isfinite (it->y) || !std::isfinite (it->z)) + { + myvector.push_back(count); + } + count++; + } + + count = 0; + for (unsigned i=0; ierase(no_ground_cloud_->begin() + myvector[i] - count); + count++; + } + if (no_ground_cloud_->points.size() > 0) { // Euclidean Clustering: std::vector cluster_indices; typename pcl::search::KdTree::Ptr tree (new pcl::search::KdTree); + tree->setInputCloud(no_ground_cloud_); pcl::EuclideanClusterExtraction ec; ec.setClusterTolerance(2 * 0.06); @@ -504,7 +534,7 @@ open_ptrack::detection::GroundBasedPeopleDetectionApp::compute (std::vec ec.setSearchMethod(tree); ec.setInputCloud(no_ground_cloud_); ec.extract(cluster_indices); - + // Sensor tilt compensation to improve people detection: PointCloudPtr no_ground_cloud_rotated(new PointCloud); Eigen::VectorXf ground_coeffs_new; diff --git a/detection/launch/detector_depth_zed.launch b/detection/launch/detector_depth_zed.launch new file mode 100644 index 0000000..fd5fe84 --- /dev/null +++ b/detection/launch/detector_depth_zed.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/swissranger_camera/cfg/cpp/swissranger_camera/SwissRangerConfig.h b/swissranger_camera/cfg/cpp/swissranger_camera/SwissRangerConfig.h new file mode 100644 index 0000000..fa8d0ae --- /dev/null +++ b/swissranger_camera/cfg/cpp/swissranger_camera/SwissRangerConfig.h @@ -0,0 +1,531 @@ +//#line 2 "/opt/ros/indigo/share/dynamic_reconfigure/templates/ConfigType.h.template" +// ********************************************************* +// +// File autogenerated for the swissranger_camera package +// by the dynamic_reconfigure package. +// Please do not edit. +// +// ********************************************************/ + +#ifndef __swissranger_camera__SWISSRANGERCONFIG_H__ +#define __swissranger_camera__SWISSRANGERCONFIG_H__ + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace swissranger_camera +{ + class SwissRangerConfigStatics; + + class SwissRangerConfig + { + public: + class AbstractParamDescription : public dynamic_reconfigure::ParamDescription + { + public: + AbstractParamDescription(std::string n, std::string t, uint32_t l, + std::string d, std::string e) + { + name = n; + type = t; + level = l; + description = d; + edit_method = e; + } + + virtual void clamp(SwissRangerConfig &config, const SwissRangerConfig &max, const SwissRangerConfig &min) const = 0; + virtual void calcLevel(uint32_t &level, const SwissRangerConfig &config1, const SwissRangerConfig &config2) const = 0; + virtual void fromServer(const ros::NodeHandle &nh, SwissRangerConfig &config) const = 0; + virtual void toServer(const ros::NodeHandle &nh, const SwissRangerConfig &config) const = 0; + virtual bool fromMessage(const dynamic_reconfigure::Config &msg, SwissRangerConfig &config) const = 0; + virtual void toMessage(dynamic_reconfigure::Config &msg, const SwissRangerConfig &config) const = 0; + virtual void getValue(const SwissRangerConfig &config, boost::any &val) const = 0; + }; + + typedef boost::shared_ptr AbstractParamDescriptionPtr; + typedef boost::shared_ptr AbstractParamDescriptionConstPtr; + + template + class ParamDescription : public AbstractParamDescription + { + public: + ParamDescription(std::string name, std::string type, uint32_t level, + std::string description, std::string edit_method, T SwissRangerConfig::* f) : + AbstractParamDescription(name, type, level, description, edit_method), + field(f) + {} + + T (SwissRangerConfig::* field); + + virtual void clamp(SwissRangerConfig &config, const SwissRangerConfig &max, const SwissRangerConfig &min) const + { + if (config.*field > max.*field) + config.*field = max.*field; + + if (config.*field < min.*field) + config.*field = min.*field; + } + + virtual void calcLevel(uint32_t &comb_level, const SwissRangerConfig &config1, const SwissRangerConfig &config2) const + { + if (config1.*field != config2.*field) + comb_level |= level; + } + + virtual void fromServer(const ros::NodeHandle &nh, SwissRangerConfig &config) const + { + nh.getParam(name, config.*field); + } + + virtual void toServer(const ros::NodeHandle &nh, const SwissRangerConfig &config) const + { + nh.setParam(name, config.*field); + } + + virtual bool fromMessage(const dynamic_reconfigure::Config &msg, SwissRangerConfig &config) const + { + return dynamic_reconfigure::ConfigTools::getParameter(msg, name, config.*field); + } + + virtual void toMessage(dynamic_reconfigure::Config &msg, const SwissRangerConfig &config) const + { + dynamic_reconfigure::ConfigTools::appendParameter(msg, name, config.*field); + } + + virtual void getValue(const SwissRangerConfig &config, boost::any &val) const + { + val = config.*field; + } + }; + + class AbstractGroupDescription : public dynamic_reconfigure::Group + { + public: + AbstractGroupDescription(std::string n, std::string t, int p, int i, bool s) + { + name = n; + type = t; + parent = p; + state = s; + id = i; + } + + std::vector abstract_parameters; + bool state; + + virtual void toMessage(dynamic_reconfigure::Config &msg, const boost::any &config) const = 0; + virtual bool fromMessage(const dynamic_reconfigure::Config &msg, boost::any &config) const =0; + virtual void updateParams(boost::any &cfg, SwissRangerConfig &top) const= 0; + virtual void setInitialState(boost::any &cfg) const = 0; + + + void convertParams() + { + for(std::vector::const_iterator i = abstract_parameters.begin(); i != abstract_parameters.end(); ++i) + { + parameters.push_back(dynamic_reconfigure::ParamDescription(**i)); + } + } + }; + + typedef boost::shared_ptr AbstractGroupDescriptionPtr; + typedef boost::shared_ptr AbstractGroupDescriptionConstPtr; + + template + class GroupDescription : public AbstractGroupDescription + { + public: + GroupDescription(std::string name, std::string type, int parent, int id, bool s, T PT::* f) : AbstractGroupDescription(name, type, parent, id, s), field(f) + { + } + + GroupDescription(const GroupDescription& g): AbstractGroupDescription(g.name, g.type, g.parent, g.id, g.state), field(g.field), groups(g.groups) + { + parameters = g.parameters; + abstract_parameters = g.abstract_parameters; + } + + virtual bool fromMessage(const dynamic_reconfigure::Config &msg, boost::any &cfg) const + { + PT* config = boost::any_cast(cfg); + if(!dynamic_reconfigure::ConfigTools::getGroupState(msg, name, (*config).*field)) + return false; + + for(std::vector::const_iterator i = groups.begin(); i != groups.end(); ++i) + { + boost::any n = &((*config).*field); + if(!(*i)->fromMessage(msg, n)) + return false; + } + + return true; + } + + virtual void setInitialState(boost::any &cfg) const + { + PT* config = boost::any_cast(cfg); + T* group = &((*config).*field); + group->state = state; + + for(std::vector::const_iterator i = groups.begin(); i != groups.end(); ++i) + { + boost::any n = boost::any(&((*config).*field)); + (*i)->setInitialState(n); + } + + } + + virtual void updateParams(boost::any &cfg, SwissRangerConfig &top) const + { + PT* config = boost::any_cast(cfg); + + T* f = &((*config).*field); + f->setParams(top, abstract_parameters); + + for(std::vector::const_iterator i = groups.begin(); i != groups.end(); ++i) + { + boost::any n = &((*config).*field); + (*i)->updateParams(n, top); + } + } + + virtual void toMessage(dynamic_reconfigure::Config &msg, const boost::any &cfg) const + { + const PT config = boost::any_cast(cfg); + dynamic_reconfigure::ConfigTools::appendGroup(msg, name, id, parent, config.*field); + + for(std::vector::const_iterator i = groups.begin(); i != groups.end(); ++i) + { + (*i)->toMessage(msg, config.*field); + } + } + + T (PT::* field); + std::vector groups; + }; + +class DEFAULT +{ + public: + DEFAULT() + { + state = true; + name = "Default"; + } + + void setParams(SwissRangerConfig &config, const std::vector params) + { + for (std::vector::const_iterator _i = params.begin(); _i != params.end(); ++_i) + { + boost::any val; + (*_i)->getValue(config, val); + + if("frame_id"==(*_i)->name){frame_id = boost::any_cast(val);} + if("camera_info_url"==(*_i)->name){camera_info_url = boost::any_cast(val);} + if("auto_exposure"==(*_i)->name){auto_exposure = boost::any_cast(val);} + if("integration_time"==(*_i)->name){integration_time = boost::any_cast(val);} + if("amp_threshold"==(*_i)->name){amp_threshold = boost::any_cast(val);} + } + } + + std::string frame_id; +std::string camera_info_url; +int auto_exposure; +int integration_time; +int amp_threshold; + + bool state; + std::string name; + + +}groups; + + + +//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" + std::string frame_id; +//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" + std::string camera_info_url; +//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" + int auto_exposure; +//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" + int integration_time; +//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" + int amp_threshold; +//#line 218 "/opt/ros/indigo/share/dynamic_reconfigure/templates/ConfigType.h.template" + + bool __fromMessage__(dynamic_reconfigure::Config &msg) + { + const std::vector &__param_descriptions__ = __getParamDescriptions__(); + const std::vector &__group_descriptions__ = __getGroupDescriptions__(); + + int count = 0; + for (std::vector::const_iterator i = __param_descriptions__.begin(); i != __param_descriptions__.end(); ++i) + if ((*i)->fromMessage(msg, *this)) + count++; + + for (std::vector::const_iterator i = __group_descriptions__.begin(); i != __group_descriptions__.end(); i ++) + { + if ((*i)->id == 0) + { + boost::any n = boost::any(this); + (*i)->updateParams(n, *this); + (*i)->fromMessage(msg, n); + } + } + + if (count != dynamic_reconfigure::ConfigTools::size(msg)) + { + ROS_ERROR("SwissRangerConfig::__fromMessage__ called with an unexpected parameter."); + ROS_ERROR("Booleans:"); + for (unsigned int i = 0; i < msg.bools.size(); i++) + ROS_ERROR(" %s", msg.bools[i].name.c_str()); + ROS_ERROR("Integers:"); + for (unsigned int i = 0; i < msg.ints.size(); i++) + ROS_ERROR(" %s", msg.ints[i].name.c_str()); + ROS_ERROR("Doubles:"); + for (unsigned int i = 0; i < msg.doubles.size(); i++) + ROS_ERROR(" %s", msg.doubles[i].name.c_str()); + ROS_ERROR("Strings:"); + for (unsigned int i = 0; i < msg.strs.size(); i++) + ROS_ERROR(" %s", msg.strs[i].name.c_str()); + // @todo Check that there are no duplicates. Make this error more + // explicit. + return false; + } + return true; + } + + // This version of __toMessage__ is used during initialization of + // statics when __getParamDescriptions__ can't be called yet. + void __toMessage__(dynamic_reconfigure::Config &msg, const std::vector &__param_descriptions__, const std::vector &__group_descriptions__) const + { + dynamic_reconfigure::ConfigTools::clear(msg); + for (std::vector::const_iterator i = __param_descriptions__.begin(); i != __param_descriptions__.end(); ++i) + (*i)->toMessage(msg, *this); + + for (std::vector::const_iterator i = __group_descriptions__.begin(); i != __group_descriptions__.end(); ++i) + { + if((*i)->id == 0) + { + (*i)->toMessage(msg, *this); + } + } + } + + void __toMessage__(dynamic_reconfigure::Config &msg) const + { + const std::vector &__param_descriptions__ = __getParamDescriptions__(); + const std::vector &__group_descriptions__ = __getGroupDescriptions__(); + __toMessage__(msg, __param_descriptions__, __group_descriptions__); + } + + void __toServer__(const ros::NodeHandle &nh) const + { + const std::vector &__param_descriptions__ = __getParamDescriptions__(); + for (std::vector::const_iterator i = __param_descriptions__.begin(); i != __param_descriptions__.end(); ++i) + (*i)->toServer(nh, *this); + } + + void __fromServer__(const ros::NodeHandle &nh) + { + static bool setup=false; + + const std::vector &__param_descriptions__ = __getParamDescriptions__(); + for (std::vector::const_iterator i = __param_descriptions__.begin(); i != __param_descriptions__.end(); ++i) + (*i)->fromServer(nh, *this); + + const std::vector &__group_descriptions__ = __getGroupDescriptions__(); + for (std::vector::const_iterator i = __group_descriptions__.begin(); i != __group_descriptions__.end(); i++){ + if (!setup && (*i)->id == 0) { + setup = true; + boost::any n = boost::any(this); + (*i)->setInitialState(n); + } + } + } + + void __clamp__() + { + const std::vector &__param_descriptions__ = __getParamDescriptions__(); + const SwissRangerConfig &__max__ = __getMax__(); + const SwissRangerConfig &__min__ = __getMin__(); + for (std::vector::const_iterator i = __param_descriptions__.begin(); i != __param_descriptions__.end(); ++i) + (*i)->clamp(*this, __max__, __min__); + } + + uint32_t __level__(const SwissRangerConfig &config) const + { + const std::vector &__param_descriptions__ = __getParamDescriptions__(); + uint32_t level = 0; + for (std::vector::const_iterator i = __param_descriptions__.begin(); i != __param_descriptions__.end(); ++i) + (*i)->calcLevel(level, config, *this); + return level; + } + + static const dynamic_reconfigure::ConfigDescription &__getDescriptionMessage__(); + static const SwissRangerConfig &__getDefault__(); + static const SwissRangerConfig &__getMax__(); + static const SwissRangerConfig &__getMin__(); + static const std::vector &__getParamDescriptions__(); + static const std::vector &__getGroupDescriptions__(); + + private: + static const SwissRangerConfigStatics *__get_statics__(); + }; + + template <> // Max and min are ignored for strings. + inline void SwissRangerConfig::ParamDescription::clamp(SwissRangerConfig &config, const SwissRangerConfig &max, const SwissRangerConfig &min) const + { + return; + } + + class SwissRangerConfigStatics + { + friend class SwissRangerConfig; + + SwissRangerConfigStatics() + { +SwissRangerConfig::GroupDescription Default("Default", "", 0, 0, true, &SwissRangerConfig::groups); +//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" + __min__.frame_id = ""; +//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" + __max__.frame_id = ""; +//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" + __default__.frame_id = "camera"; +//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" + Default.abstract_parameters.push_back(SwissRangerConfig::AbstractParamDescriptionConstPtr(new SwissRangerConfig::ParamDescription("frame_id", "str", 3, "ROS tf frame of reference, resolved with tf_prefix unless absolute.", "", &SwissRangerConfig::frame_id))); +//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" + __param_descriptions__.push_back(SwissRangerConfig::AbstractParamDescriptionConstPtr(new SwissRangerConfig::ParamDescription("frame_id", "str", 3, "ROS tf frame of reference, resolved with tf_prefix unless absolute.", "", &SwissRangerConfig::frame_id))); +//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" + __min__.camera_info_url = ""; +//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" + __max__.camera_info_url = ""; +//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" + __default__.camera_info_url = ""; +//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" + Default.abstract_parameters.push_back(SwissRangerConfig::AbstractParamDescriptionConstPtr(new SwissRangerConfig::ParamDescription("camera_info_url", "str", 0, "Camera calibration URL for this video_mode (uncalibrated if null).", "", &SwissRangerConfig::camera_info_url))); +//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" + __param_descriptions__.push_back(SwissRangerConfig::AbstractParamDescriptionConstPtr(new SwissRangerConfig::ParamDescription("camera_info_url", "str", 0, "Camera calibration URL for this video_mode (uncalibrated if null).", "", &SwissRangerConfig::camera_info_url))); +//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" + __min__.auto_exposure = 0; +//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" + __max__.auto_exposure = 1; +//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" + __default__.auto_exposure = 1; +//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" + Default.abstract_parameters.push_back(SwissRangerConfig::AbstractParamDescriptionConstPtr(new SwissRangerConfig::ParamDescription("auto_exposure", "int", 0, "Auto exposure control state.", "{'enum_description': 'Feature control states', 'enum': [{'srcline': 54, 'description': 'Use fixed value', 'srcfile': '/home/remap/workspace/ros/catkin/src/open_ptrack/swissranger_camera/cfg/SwissRanger.cfg', 'cconsttype': 'const int', 'value': 0, 'ctype': 'int', 'type': 'int', 'name': 'Off'}, {'srcline': 55, 'description': 'Camera sets continuously', 'srcfile': '/home/remap/workspace/ros/catkin/src/open_ptrack/swissranger_camera/cfg/SwissRanger.cfg', 'cconsttype': 'const int', 'value': 1, 'ctype': 'int', 'type': 'int', 'name': 'Auto'}]}", &SwissRangerConfig::auto_exposure))); +//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" + __param_descriptions__.push_back(SwissRangerConfig::AbstractParamDescriptionConstPtr(new SwissRangerConfig::ParamDescription("auto_exposure", "int", 0, "Auto exposure control state.", "{'enum_description': 'Feature control states', 'enum': [{'srcline': 54, 'description': 'Use fixed value', 'srcfile': '/home/remap/workspace/ros/catkin/src/open_ptrack/swissranger_camera/cfg/SwissRanger.cfg', 'cconsttype': 'const int', 'value': 0, 'ctype': 'int', 'type': 'int', 'name': 'Off'}, {'srcline': 55, 'description': 'Camera sets continuously', 'srcfile': '/home/remap/workspace/ros/catkin/src/open_ptrack/swissranger_camera/cfg/SwissRanger.cfg', 'cconsttype': 'const int', 'value': 1, 'ctype': 'int', 'type': 'int', 'name': 'Auto'}]}", &SwissRangerConfig::auto_exposure))); +//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" + __min__.integration_time = 0; +//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" + __max__.integration_time = 255; +//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" + __default__.integration_time = 1; +//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" + Default.abstract_parameters.push_back(SwissRangerConfig::AbstractParamDescriptionConstPtr(new SwissRangerConfig::ParamDescription("integration_time", "int", 0, "integration_time control.", "", &SwissRangerConfig::integration_time))); +//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" + __param_descriptions__.push_back(SwissRangerConfig::AbstractParamDescriptionConstPtr(new SwissRangerConfig::ParamDescription("integration_time", "int", 0, "integration_time control.", "", &SwissRangerConfig::integration_time))); +//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" + __min__.amp_threshold = -1; +//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" + __max__.amp_threshold = 65535; +//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" + __default__.amp_threshold = -1; +//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" + Default.abstract_parameters.push_back(SwissRangerConfig::AbstractParamDescriptionConstPtr(new SwissRangerConfig::ParamDescription("amp_threshold", "int", 0, "amplitude_threshold control.", "", &SwissRangerConfig::amp_threshold))); +//#line 259 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" + __param_descriptions__.push_back(SwissRangerConfig::AbstractParamDescriptionConstPtr(new SwissRangerConfig::ParamDescription("amp_threshold", "int", 0, "amplitude_threshold control.", "", &SwissRangerConfig::amp_threshold))); +//#line 233 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" + Default.convertParams(); +//#line 233 "/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py" + __group_descriptions__.push_back(SwissRangerConfig::AbstractGroupDescriptionConstPtr(new SwissRangerConfig::GroupDescription(Default))); +//#line 353 "/opt/ros/indigo/share/dynamic_reconfigure/templates/ConfigType.h.template" + + for (std::vector::const_iterator i = __group_descriptions__.begin(); i != __group_descriptions__.end(); ++i) + { + __description_message__.groups.push_back(**i); + } + __max__.__toMessage__(__description_message__.max, __param_descriptions__, __group_descriptions__); + __min__.__toMessage__(__description_message__.min, __param_descriptions__, __group_descriptions__); + __default__.__toMessage__(__description_message__.dflt, __param_descriptions__, __group_descriptions__); + } + std::vector __param_descriptions__; + std::vector __group_descriptions__; + SwissRangerConfig __max__; + SwissRangerConfig __min__; + SwissRangerConfig __default__; + dynamic_reconfigure::ConfigDescription __description_message__; + + static const SwissRangerConfigStatics *get_instance() + { + // Split this off in a separate function because I know that + // instance will get initialized the first time get_instance is + // called, and I am guaranteeing that get_instance gets called at + // most once. + static SwissRangerConfigStatics instance; + return &instance; + } + }; + + inline const dynamic_reconfigure::ConfigDescription &SwissRangerConfig::__getDescriptionMessage__() + { + return __get_statics__()->__description_message__; + } + + inline const SwissRangerConfig &SwissRangerConfig::__getDefault__() + { + return __get_statics__()->__default__; + } + + inline const SwissRangerConfig &SwissRangerConfig::__getMax__() + { + return __get_statics__()->__max__; + } + + inline const SwissRangerConfig &SwissRangerConfig::__getMin__() + { + return __get_statics__()->__min__; + } + + inline const std::vector &SwissRangerConfig::__getParamDescriptions__() + { + return __get_statics__()->__param_descriptions__; + } + + inline const std::vector &SwissRangerConfig::__getGroupDescriptions__() + { + return __get_statics__()->__group_descriptions__; + } + + inline const SwissRangerConfigStatics *SwissRangerConfig::__get_statics__() + { + const static SwissRangerConfigStatics *statics; + + if (statics) // Common case + return statics; + + boost::mutex::scoped_lock lock(dynamic_reconfigure::__init_mutex__); + + if (statics) // In case we lost a race. + return statics; + + statics = SwissRangerConfigStatics::get_instance(); + + return statics; + } + +//#line 54 "/home/remap/workspace/ros/catkin/src/open_ptrack/swissranger_camera/cfg/SwissRanger.cfg" + const int SwissRanger_Off = 0; +//#line 55 "/home/remap/workspace/ros/catkin/src/open_ptrack/swissranger_camera/cfg/SwissRanger.cfg" + const int SwissRanger_Auto = 1; +} + +#endif // __SWISSRANGERRECONFIGURATOR_H__ diff --git a/swissranger_camera/src/swissranger_camera/__init__.py b/swissranger_camera/src/swissranger_camera/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/swissranger_camera/src/swissranger_camera/cfg/SwissRangerConfig.py b/swissranger_camera/src/swissranger_camera/cfg/SwissRangerConfig.py new file mode 100644 index 0000000..26d0aad --- /dev/null +++ b/swissranger_camera/src/swissranger_camera/cfg/SwissRangerConfig.py @@ -0,0 +1,38 @@ +## ********************************************************* +## +## File autogenerated for the swissranger_camera package +## by the dynamic_reconfigure package. +## Please do not edit. +## +## ********************************************************/ + +from dynamic_reconfigure.encoding import extract_params + +inf = float('inf') + +config_description = {'upper': 'DEFAULT', 'lower': 'groups', 'srcline': 233, 'name': 'Default', 'parent': 0, 'srcfile': '/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py', 'cstate': 'true', 'parentname': 'Default', 'class': 'DEFAULT', 'field': 'default', 'state': True, 'parentclass': '', 'groups': [], 'parameters': [{'srcline': 259, 'description': 'ROS tf frame of reference, resolved with tf_prefix unless absolute.', 'max': '', 'cconsttype': 'const char * const', 'ctype': 'std::string', 'srcfile': '/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py', 'name': 'frame_id', 'edit_method': '', 'default': 'camera', 'level': 3, 'min': '', 'type': 'str'}, {'srcline': 259, 'description': 'Camera calibration URL for this video_mode (uncalibrated if null).', 'max': '', 'cconsttype': 'const char * const', 'ctype': 'std::string', 'srcfile': '/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py', 'name': 'camera_info_url', 'edit_method': '', 'default': '', 'level': 0, 'min': '', 'type': 'str'}, {'srcline': 259, 'description': 'Auto exposure control state.', 'max': 1, 'cconsttype': 'const int', 'ctype': 'int', 'srcfile': '/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py', 'name': 'auto_exposure', 'edit_method': "{'enum_description': 'Feature control states', 'enum': [{'srcline': 54, 'description': 'Use fixed value', 'srcfile': '/home/remap/workspace/ros/catkin/src/open_ptrack/swissranger_camera/cfg/SwissRanger.cfg', 'cconsttype': 'const int', 'value': 0, 'ctype': 'int', 'type': 'int', 'name': 'Off'}, {'srcline': 55, 'description': 'Camera sets continuously', 'srcfile': '/home/remap/workspace/ros/catkin/src/open_ptrack/swissranger_camera/cfg/SwissRanger.cfg', 'cconsttype': 'const int', 'value': 1, 'ctype': 'int', 'type': 'int', 'name': 'Auto'}]}", 'default': 1, 'level': 0, 'min': 0, 'type': 'int'}, {'srcline': 259, 'description': 'integration_time control.', 'max': 255, 'cconsttype': 'const int', 'ctype': 'int', 'srcfile': '/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py', 'name': 'integration_time', 'edit_method': '', 'default': 1, 'level': 0, 'min': 0, 'type': 'int'}, {'srcline': 259, 'description': 'amplitude_threshold control.', 'max': 65535, 'cconsttype': 'const int', 'ctype': 'int', 'srcfile': '/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py', 'name': 'amp_threshold', 'edit_method': '', 'default': -1, 'level': 0, 'min': -1, 'type': 'int'}], 'type': '', 'id': 0} + +min = {} +max = {} +defaults = {} +level = {} +type = {} +all_level = 0 + +#def extract_params(config): +# params = [] +# params.extend(config['parameters']) +# for group in config['groups']: +# params.extend(extract_params(group)) +# return params + +for param in extract_params(config_description): + min[param['name']] = param['min'] + max[param['name']] = param['max'] + defaults[param['name']] = param['default'] + level[param['name']] = param['level'] + type[param['name']] = param['type'] + all_level = all_level | param['level'] + +SwissRanger_Off = 0 +SwissRanger_Auto = 1 diff --git a/swissranger_camera/src/swissranger_camera/cfg/__init__.py b/swissranger_camera/src/swissranger_camera/cfg/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/tracking/launch/detection_and_tracking_zed.launch b/tracking/launch/detection_and_tracking_zed.launch new file mode 100644 index 0000000..e5be647 --- /dev/null +++ b/tracking/launch/detection_and_tracking_zed.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + +