diff --git a/doc/jsk_pcl_ros/nodes/hinted_plane_detector.md b/doc/jsk_pcl_ros/nodes/hinted_plane_detector.md index 8d652ed5b1..f40cea8b2c 100644 --- a/doc/jsk_pcl_ros/nodes/hinted_plane_detector.md +++ b/doc/jsk_pcl_ros/nodes/hinted_plane_detector.md @@ -49,6 +49,10 @@ Algorithm is: ## Parameters ### Parameters for detecting hint plane +* `~synchronize` (Double, default: `True`) + + Enable time synchronization of input topics + * `~hint_outlier_threashold` (Double, default: `0.1`) Outlier threshold to detect hint plane using RANSAC diff --git a/jsk_pcl_ros/cfg/HintedPlaneDetector.cfg b/jsk_pcl_ros/cfg/HintedPlaneDetector.cfg index 58ec2b73f8..b986982e22 100755 --- a/jsk_pcl_ros/cfg/HintedPlaneDetector.cfg +++ b/jsk_pcl_ros/cfg/HintedPlaneDetector.cfg @@ -9,6 +9,7 @@ from math import pi gen = ParameterGenerator () +gen.add('synchronize', bool_t, 0, 'Enable synchronization of input topics', True) gen.add("hint_outlier_threashold", double_t, 0, "outlier threshold to detect hint plane", 0.1, 0.0, 1.0) gen.add("hint_max_iteration", int_t, 0, "max iteration to detect hint plane", 100, 1, 10000) gen.add("hint_min_size", int_t, 0, "minimum number of inliers included in hint plane", 100, 0, 1000) diff --git a/jsk_pcl_ros/include/jsk_pcl_ros/hinted_plane_detector.h b/jsk_pcl_ros/include/jsk_pcl_ros/hinted_plane_detector.h index d95ed1b2e6..4247c3e9d8 100644 --- a/jsk_pcl_ros/include/jsk_pcl_ros/hinted_plane_detector.h +++ b/jsk_pcl_ros/include/jsk_pcl_ros/hinted_plane_detector.h @@ -66,6 +66,7 @@ namespace jsk_pcl_ros { virtual void onInit(); virtual void subscribe(); virtual void unsubscribe(); + virtual void setHintCloud(const sensor_msgs::PointCloud2::ConstPtr& msg); virtual void detect( const sensor_msgs::PointCloud2::ConstPtr& cloud_msg, const sensor_msgs::PointCloud2::ConstPtr& hint_cloud_msg); @@ -124,10 +125,14 @@ namespace jsk_pcl_ros { ros::Publisher pub_euclidean_filtered_indices_; boost::shared_ptr > srv_; boost::mutex mutex_; + ros::Subscriber sub_cloud_single_; + ros::Subscriber sub_hint_cloud_single_; + sensor_msgs::PointCloud2::ConstPtr hint_cloud_; //////////////////////////////////////////////////////// // parameters //////////////////////////////////////////////////////// + bool synchronize_; double hint_outlier_threashold_; int hint_max_iteration_; int hint_min_size_; diff --git a/jsk_pcl_ros/src/hinted_plane_detector_nodelet.cpp b/jsk_pcl_ros/src/hinted_plane_detector_nodelet.cpp index 9a642ecd08..a35b12cc85 100644 --- a/jsk_pcl_ros/src/hinted_plane_detector_nodelet.cpp +++ b/jsk_pcl_ros/src/hinted_plane_detector_nodelet.cpp @@ -97,24 +97,40 @@ namespace jsk_pcl_ros { void HintedPlaneDetector::subscribe() { - sub_cloud_.subscribe(*pnh_, "input", 1); - sub_hint_cloud_.subscribe(*pnh_, "input/hint/cloud", 1); - sync_ = boost::make_shared >(100); - sync_->connectInput(sub_cloud_, sub_hint_cloud_); - sync_->registerCallback(boost::bind(&HintedPlaneDetector::detect, - this, _1, _2)); + if (synchronize_) { + sub_cloud_.subscribe(*pnh_, "input", 1); + sub_hint_cloud_.subscribe(*pnh_, "input/hint/cloud", 1); + sync_ = boost::make_shared >(100); + sync_->connectInput(sub_cloud_, sub_hint_cloud_); + sync_->registerCallback( + boost::bind(&HintedPlaneDetector::detect, this, _1, _2)); + } else { + sub_hint_cloud_single_ = pnh_->subscribe( + "input/hint/cloud", 1, &HintedPlaneDetector::setHintCloud, this); + sub_cloud_single_ = pnh_->subscribe( + "input", 1, + boost::bind(&HintedPlaneDetector::detect, this, _1, + boost::ref(hint_cloud_))); + } } void HintedPlaneDetector::unsubscribe() { - sub_cloud_.unsubscribe(); - sub_hint_cloud_.unsubscribe(); + if (synchronize_) { + sub_cloud_.unsubscribe(); + sub_hint_cloud_.unsubscribe(); + } else { + sub_cloud_single_.shutdown(); + sub_hint_cloud_single_.shutdown(); + } } void HintedPlaneDetector::configCallback( Config &config, uint32_t level) { boost::mutex::scoped_lock lock(mutex_); + const bool need_resubscribe = synchronize_ != config.synchronize; + synchronize_ = config.synchronize; hint_outlier_threashold_ = config.hint_outlier_threashold; hint_max_iteration_ = config.hint_max_iteration; hint_min_size_ = config.hint_min_size; @@ -131,6 +147,18 @@ namespace jsk_pcl_ros { enable_normal_filtering_ = config.enable_normal_filtering; enable_distance_filtering_ = config.enable_distance_filtering; enable_density_filtering_ = config.enable_density_filtering; + if (need_resubscribe && isSubscribed()) { + unsubscribe(); + subscribe(); + } + } + + void HintedPlaneDetector::setHintCloud( + const sensor_msgs::PointCloud2::ConstPtr &msg) + + { + hint_cloud_ = msg; + NODELET_DEBUG("hint cloud is set"); } void HintedPlaneDetector::detect( @@ -139,6 +167,12 @@ namespace jsk_pcl_ros { { vital_checker_->poke(); boost::mutex::scoped_lock lock(mutex_); + if (!hint_cloud_msg) { + NODELET_WARN_THROTTLE( + 10.0, + "hint cloud is not received. Set hint cloud or enable 'synchronize'"); + return; + } pcl::PointCloud::Ptr input_cloud (new pcl::PointCloud); pcl::PointCloud::Ptr