Merge branch 'master' into add-missing-ros_environment

.github/workflows/config.yml

@@ -4,6 +4,9 @@ on:
       - master
   pull_request:
 
+env:
+  ACTIONS_ALLOW_USE_UNSECURE_NODE_VERSION: true
+
 jobs:
   ros:
     runs-on: ubuntu-latest

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

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)

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_;

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

jsk_recognition_utils/CMakeLists.txt

@@ -12,6 +12,7 @@ endif(CCACHE_FOUND)
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
 find_package(catkin REQUIRED COMPONENTS
+  cv_bridge
   dynamic_reconfigure
   jsk_recognition_msgs
   pcl_ros
@@ -52,7 +53,7 @@ generate_dynamic_reconfigure_options(
 catkin_package(
  INCLUDE_DIRS include
  LIBRARIES jsk_recognition_utils
- CATKIN_DEPENDS jsk_recognition_msgs pcl_ros visualization_msgs message_runtime
+ CATKIN_DEPENDS cv_bridge jsk_recognition_msgs pcl_ros visualization_msgs message_runtime
 )
 
 # Cythonize pyx files

jsk_recognition_utils/package.xml

@@ -16,6 +16,7 @@
 
   <buildtool_depend>catkin</buildtool_depend>
   <build_depend>ros_environment</build_depend>
+  <build_depend>cv_bridge</build_depend>
   <build_depend condition="$ROS_PYTHON_VERSION==2">cython</build_depend>
   <build_depend condition="$ROS_PYTHON_VERSION==3">cython3</build_depend>
   <build_depend>eigen_conversions</build_depend>
@@ -36,6 +37,7 @@
   <build_depend>visualization_msgs</build_depend>
   <build_depend>yaml-cpp</build_depend>
   <build_depend>message_generation</build_depend>
+  <exec_depend>cv_bridge</exec_depend>
   <exec_depend>eigen_conversions</exec_depend>
   <exec_depend>geometry_msgs</exec_depend>
   <exec_depend>image_geometry</exec_depend>