Merge pull request #2844 from furushchev/hint-cloud-no-sync

Add option 'synchronize' to hinted_plane_detector_nodelet
jsk-ros-pkg · Jun 19, 2024 · cc31b0e · cc31b0e
2 parents f05c0d5 + 54abe2d
commit cc31b0e
Show file tree

Hide file tree

Showing 4 changed files with 52 additions and 8 deletions.
diff --git 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
@@ -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
@@ -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 <dynamic_reconfigure::Server<Config> > 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
@@ -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<message_filters::Synchronizer<SyncPolicy> >(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<message_filters::Synchronizer<SyncPolicy> >(100);
+      sync_->connectInput(sub_cloud_, sub_hint_cloud_);
+      sync_->registerCallback(
+          boost::bind(&HintedPlaneDetector::detect, this, _1, _2));
+    } else {
+      sub_hint_cloud_single_ = pnh_->subscribe<sensor_msgs::PointCloud2>(
+          "input/hint/cloud", 1, &HintedPlaneDetector::setHintCloud, this);
+      sub_cloud_single_ = pnh_->subscribe<sensor_msgs::PointCloud2>(
+          "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<pcl::PointNormal>::Ptr
       input_cloud (new pcl::PointCloud<pcl::PointNormal>);
     pcl::PointCloud<pcl::PointXYZ>::Ptr