Migrate extract_indices filter (#394)

* - migrate extract_indices filter
- add test to check if filter node/component output result

Signed-off-by: Daisuke Sato <daisukes@cmu.edu>

* add test_depend to package.xml

Signed-off-by: Daisuke Sato <daisukes@cmu.edu>

* add launch_testing_ros as test_depend too

Signed-off-by: Daisuke Sato <daisukes@cmu.edu>

* fixed test not to depend on ros2cli

Signed-off-by: Daisuke Sato <daisukes@cmu.edu>

---------

Signed-off-by: Daisuke Sato <daisukes@cmu.edu>

pcl_ros/CMakeLists.txt

@@ -26,6 +26,7 @@ find_package(tf2_ros REQUIRED)
 set(dependencies
   pcl_conversions
   rclcpp
+  rclcpp_components
   sensor_msgs
   geometry_msgs
   tf2
@@ -81,7 +82,7 @@ ament_target_dependencies(pcl_ros_tf
 #
 ### Declare the pcl_ros_filters library
 add_library(pcl_ros_filters SHARED
-#  src/pcl_ros/filters/extract_indices.cpp
+  src/pcl_ros/filters/extract_indices.cpp
   src/pcl_ros/filters/filter.cpp
 #  src/pcl_ros/filters/passthrough.cpp
 #  src/pcl_ros/filters/project_inliers.cpp
@@ -92,6 +93,10 @@ add_library(pcl_ros_filters SHARED
 )
 target_link_libraries(pcl_ros_filters pcl_ros_tf ${PCL_LIBRARIES})
 ament_target_dependencies(pcl_ros_filters ${dependencies})
+rclcpp_components_register_node(pcl_ros_filters
+  PLUGIN "pcl_ros::ExtractIndices"
+  EXECUTABLE filter_extract_indices_node
+)
 class_loader_hide_library_symbols(pcl_ros_filters)
 #
 ### Declare the pcl_ros_segmentation library
@@ -163,6 +168,7 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 
   find_package(ament_cmake_gtest REQUIRED)
+  add_subdirectory(tests/filters)
   #add_rostest_gtest(test_tf_message_filter_pcl tests/test_tf_message_filter_pcl.launch src/test/test_tf_message_filter_pcl.cpp)
   #target_link_libraries(test_tf_message_filter_pcl ${catkin_LIBRARIES} ${GTEST_LIBRARIES})
   #add_rostest(samples/pcl_ros/features/sample_normal_3d.launch ARGS gui:=false)

pcl_ros/include/pcl_ros/filters/extract_indices.hpp

@@ -40,9 +40,9 @@
 
 // PCL includes
 #include <pcl/filters/extract_indices.h>
-
+#include <mutex>
+#include <vector>
 #include "pcl_ros/filters/filter.hpp"
-#include "pcl_ros/ExtractIndicesConfig.hpp"
 
 namespace pcl_ros
 {
@@ -53,9 +53,6 @@ namespace pcl_ros
 class ExtractIndices : public Filter
 {
 protected:
-  /** \brief Pointer to a dynamic reconfigure service. */
-  boost::shared_ptr<dynamic_reconfigure::Server<pcl_ros::ExtractIndicesConfig>> srv_;
-
   /** \brief Call the actual filter.
     * \param input the input point cloud dataset
     * \param indices the input set of indices to use from \a input
@@ -63,10 +60,10 @@ protected:
     */
   inline void
   filter(
-    const PointCloud2::ConstPtr & input, const IndicesPtr & indices,
+    const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices,
     PointCloud2 & output)
   {
-    boost::mutex::scoped_lock lock(mutex_);
+    std::lock_guard<std::mutex> lock(mutex_);
     pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2);
     pcl_conversions::toPCL(*(input), *(pcl_input));
     impl_.setInputCloud(pcl_input);
@@ -76,16 +73,13 @@ protected:
     pcl_conversions::moveFromPCL(pcl_output, output);
   }
 
-  /** \brief Child initialization routine.
+  /** \brief Parameter callback
-    * \param nh ROS node handle
+    * \param params parameter values to set
-    * \param has_service set to true if the child has a Dynamic Reconfigure service
     */
-  virtual bool
+  rcl_interfaces::msg::SetParametersResult
-  child_init(ros::NodeHandle & nh, bool & has_service);
+  config_callback(const std::vector<rclcpp::Parameter> & params);
 
-  /** \brief Dynamic reconfigure service callback. */
+  OnSetParametersCallbackHandle::SharedPtr callback_handle_;
-  void
-  config_callback(pcl_ros::ExtractIndicesConfig & config, uint32_t level);
 
 private:
   /** \brief The PCL filter implementation used. */
@@ -93,6 +87,8 @@ private:
 
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  explicit ExtractIndices(const rclcpp::NodeOptions & options);
 };
 }  // namespace pcl_ros
 

pcl_ros/package.xml

@@ -49,6 +49,12 @@
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>ament_cmake_pytest</test_depend>
+  <test_depend>launch</test_depend>
+  <test_depend>launch_ros</test_depend>
+  <test_depend>launch_testing</test_depend>
+  <test_depend>launch_testing_ros</test_depend>
+  <test_depend>sensor_msgs</test_depend>
 
     <!--
     <export>

pcl_ros/src/pcl_ros/filters/extract_indices.cpp

@@ -35,37 +35,51 @@
  *
  */
 
-#include <pluginlib/class_list_macros.h>
 #include "pcl_ros/filters/extract_indices.hpp"
 
-//////////////////////////////////////////////////////////////////////////////////////////////
+pcl_ros::ExtractIndices::ExtractIndices(const rclcpp::NodeOptions & options)
-bool
+: Filter("ExtractIndicesNode", options)
-pcl_ros::ExtractIndices::child_init(ros::NodeHandle & nh, bool & has_service)
 {
-  has_service = true;
+  rcl_interfaces::msg::ParameterDescriptor neg_desc;
+  neg_desc.name = "negative";
+  neg_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL;
+  neg_desc.description = "Extract indices or the negative (all-indices)";
+  declare_parameter(neg_desc.name, rclcpp::ParameterValue(false), neg_desc);
 
-  srv_ = boost::make_shared<dynamic_reconfigure::Server<pcl_ros::ExtractIndicesConfig>>(nh);
+  // Validate initial values using same callback
-  dynamic_reconfigure::Server<pcl_ros::ExtractIndicesConfig>::CallbackType f = boost::bind(
+  callback_handle_ =
-    &ExtractIndices::config_callback, this, _1, _2);
+    add_on_set_parameters_callback(
-  srv_->setCallback(f);
+    std::bind(&ExtractIndices::config_callback, this, std::placeholders::_1));
 
-  use_indices_ = true;
+  std::vector<std::string> param_names{neg_desc.name};
-  return true;
+  auto result = config_callback(get_parameters(param_names));
-}
+  if (!result.successful) {
-
+    throw std::runtime_error(result.reason);
-//////////////////////////////////////////////////////////////////////////////////////////////
-void
-pcl_ros::ExtractIndices::config_callback(pcl_ros::ExtractIndicesConfig & config, uint32_t level)
-{
-  boost::mutex::scoped_lock lock(mutex_);
-
-  if (impl_.getNegative() != config.negative) {
-    impl_.setNegative(config.negative);
-    NODELET_DEBUG(
-      "[%s::config_callback] Setting the extraction to: %s.", getName().c_str(),
-      (config.negative ? "indices" : "everything but the indices"));
   }
 }
 
-typedef pcl_ros::ExtractIndices ExtractIndices;
+//////////////////////////////////////////////////////////////////////////////////////////////
-PLUGINLIB_EXPORT_CLASS(ExtractIndices, nodelet::Nodelet);
+rcl_interfaces::msg::SetParametersResult
+pcl_ros::ExtractIndices::config_callback(const std::vector<rclcpp::Parameter> & params)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+
+  for (const rclcpp::Parameter & param : params) {
+    if (param.get_name() == "negative") {
+      // Check the current value for the negative flag
+      if (impl_.getNegative() != param.as_bool()) {
+        RCLCPP_DEBUG(
+          get_logger(), "Setting the filter negative flag to: %s.",
+          param.as_bool() ? "true" : "false");
+        // Call the virtual method in the child
+        impl_.setNegative(param.as_bool());
+      }
+    }
+  }
+  rcl_interfaces::msg::SetParametersResult result;
+  result.successful = true;
+  return result;
+}
+
+#include "rclcpp_components/register_node_macro.hpp"
+RCLCPP_COMPONENTS_REGISTER_NODE(pcl_ros::ExtractIndices)

pcl_ros/tests/filters/CMakeLists.txt

@@ -0,0 +1,35 @@
+find_package(ament_cmake_pytest REQUIRED)
+
+# build dummy publisher node and component
+add_library(dummy_topics SHARED
+  dummy_topics.cpp
+)
+target_link_libraries(dummy_topics ${PCL_LIBRARIES})
+ament_target_dependencies(dummy_topics
+  rclcpp
+  rclcpp_components
+  pcl_conversions
+  sensor_msgs
+  PCL
+)
+
+# generate test ament index to locate dummy_topics comonent from tests
+set(components "")
+set(components "${components}pcl_ros_tests_filters::DummyTopics;$<TARGET_FILE:dummy_topics>\n")
+file(GENERATE
+OUTPUT
+"${CMAKE_CURRENT_BINARY_DIR}/test_ament_index/share/ament_index/resource_index/rclcpp_components/pcl_ros_tests_filters"
+CONTENT "${components}")
+
+ament_add_pytest_test(test_filter_extract_indices_node_component
+  test_filter_component.py
+  ENV DUMMY_PLUGIN=pcl_ros_tests_filters::DummyTopics
+      FILTER_PLUGIN=pcl_ros::ExtractIndices
+  APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index
+)
+ament_add_pytest_test(test_filter_extract_indices_node
+  test_filter_executable.py
+  ENV DUMMY_PLUGIN=pcl_ros_tests_filters::DummyTopics
+      FILTER_EXECUTABLE=filter_extract_indices_node
+  APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index
+)

pcl_ros/tests/filters/dummy_topics.cpp

@@ -0,0 +1,109 @@
+/*
+ * Copyright (c) 2022  Carnegie Mellon University
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of Carnegie Mellon University nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <pcl_msgs/msg/point_indices.hpp>
+#include <pcl_msgs/msg/model_coefficients.hpp>
+
+using namespace std::chrono_literals;
+
+namespace pcl_ros_tests_filters
+{
+class DummyTopics : public rclcpp::Node
+{
+public:
+  explicit DummyTopics(const rclcpp::NodeOptions & options);
+
+private:
+  void timer_callback();
+  rclcpp::TimerBase::SharedPtr timer_;
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr point_cloud2_pub_;
+  rclcpp::Publisher<pcl_msgs::msg::PointIndices>::SharedPtr indices_pub_;
+  rclcpp::Publisher<pcl_msgs::msg::ModelCoefficients>::SharedPtr model_pub_;
+  size_t count_;
+};
+
+DummyTopics::DummyTopics(const rclcpp::NodeOptions & options)
+: Node("dummy_point_cloud2_publisher", options), count_(0)
+{
+  point_cloud2_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("point_cloud2", 10);
+  indices_pub_ = this->create_publisher<pcl_msgs::msg::PointIndices>("indices", 10);
+  model_pub_ = this->create_publisher<pcl_msgs::msg::ModelCoefficients>("model", 10);
+  timer_ = this->create_wall_timer(
+    100ms, std::bind(&DummyTopics::timer_callback, this));
+}
+
+void DummyTopics::timer_callback()
+{
+  builtin_interfaces::msg::Time now_msg;
+  now_msg = get_clock()->now();
+
+  sensor_msgs::msg::PointCloud2 point_cloud2_msg;
+
+  pcl::PointCloud<pcl::PointXYZ> random_pcl;
+
+  unsigned int global_seed = 100;
+  for (int v = 0; v < 1000; ++v) {
+    pcl::PointXYZ newPoint;
+    newPoint.x = (rand_r(&global_seed) * 100.0) / RAND_MAX;
+    newPoint.y = (rand_r(&global_seed) * 100.0) / RAND_MAX;
+    newPoint.z = (rand_r(&global_seed) * 100.0) / RAND_MAX;
+    random_pcl.points.push_back(newPoint);
+  }
+
+  // publish point cloud
+  pcl::toROSMsg<pcl::PointXYZ>(random_pcl, point_cloud2_msg);
+  point_cloud2_msg.header.stamp = now_msg;
+  point_cloud2_pub_->publish(point_cloud2_msg);
+
+  // publish indices
+  pcl_msgs::msg::PointIndices indices_msg;
+  indices_msg.header.stamp = now_msg;
+  indices_msg.indices.push_back(0);
+  indices_pub_->publish(indices_msg);
+
+  // publish model
+  pcl_msgs::msg::ModelCoefficients model_msg;
+  model_msg.header.stamp = now_msg;
+  model_msg.values.push_back(0);
+  model_msg.values.push_back(0);
+  model_msg.values.push_back(1);
+  model_msg.values.push_back(0);
+  model_pub_->publish(model_msg);
+}
+
+}  //  namespace pcl_ros_tests_filters
+
+#include "rclcpp_components/register_node_macro.hpp"
+RCLCPP_COMPONENTS_REGISTER_NODE(pcl_ros_tests_filters::DummyTopics)

pcl_ros/tests/filters/test_filter_component.py

@@ -0,0 +1,90 @@
+#
+# Copyright (c) 2022  Carnegie Mellon University
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+#     * Redistributions of source code must retain the above copyright
+#       notice, this list of conditions and the following disclaimer.
+#     * Redistributions in binary form must reproduce the above copyright
+#       notice, this list of conditions and the following disclaimer in the
+#       documentation and/or other materials provided with the distribution.
+#     * Neither the name of Carnegie Mellon University nor the names of its
+#       contributors may be used to endorse or promote products derived from
+#       this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+
+import os
+import unittest
+
+import launch
+import launch.actions
+import launch_ros.actions
+import launch_testing.actions
+import launch_testing.markers
+import pytest
+
+from launch_testing_ros import WaitForTopics
+from sensor_msgs.msg import PointCloud2
+
+
+@pytest.mark.launch_test
+@launch_testing.markers.keep_alive
+def generate_test_description():
+    dummy_plugin = os.getenv('DUMMY_PLUGIN')
+    filter_plugin = os.getenv('FILTER_PLUGIN')
+    use_indices = os.getenv('USE_INDICES')
+    approximate_sync = os.getenv('APPROXIMATE_SYNC')
+
+    parameters = {}
+    if use_indices:
+        parameters['use_indices'] = (use_indices == 'true')
+    if approximate_sync:
+        parameters['approximate_sync'] = (approximate_sync == 'true')
+    print(parameters)
+
+    return launch.LaunchDescription([
+        launch_ros.actions.ComposableNodeContainer(
+            name='filter_container',
+            namespace='',
+            package='rclcpp_components',
+            executable='component_container',
+            composable_node_descriptions=[
+                launch_ros.descriptions.ComposableNode(
+                    package='pcl_ros_tests_filters',
+                    plugin=dummy_plugin,
+                    name='dummy_publisher',
+                ),
+                launch_ros.descriptions.ComposableNode(
+                    package='pcl_ros',
+                    plugin=filter_plugin,
+                    name='filter_node',
+                    remappings=[('/input', '/point_cloud2')],
+                    parameters=[parameters],
+                ),
+            ],
+            output='screen',
+        ),
+        launch_testing.actions.ReadyToTest()
+    ])
+
+
+class TestFilter(unittest.TestCase):
+    def test_filter_output(self):
+        wait_for_topics = WaitForTopics([('output', PointCloud2)], timeout=5.0)
+        assert wait_for_topics.wait()
+        assert 'output' in wait_for_topics.topics_received(), "Didn't receive message"
+        wait_for_topics.shutdown()

pcl_ros/tests/filters/test_filter_executable.py

@@ -0,0 +1,60 @@
+import os
+import unittest
+
+import launch
+import launch.actions
+import launch_ros.actions
+import launch_testing.actions
+import launch_testing.markers
+import pytest
+
+from launch_testing_ros import WaitForTopics
+from sensor_msgs.msg import PointCloud2
+
+
+@pytest.mark.launch_test
+@launch_testing.markers.keep_alive
+def generate_test_description():
+    dummy_plugin = os.getenv('DUMMY_PLUGIN')
+    filter_executable = os.getenv('FILTER_EXECUTABLE')
+    use_indices = os.getenv('USE_INDICES')
+    approximate_sync = os.getenv('APPROXIMATE_SYNC')
+
+    ros_arguments = ["-r", "input:=point_cloud2"]
+    if use_indices:
+        ros_arguments.extend(["-p", "use_indices:={}".format(use_indices)])
+    if approximate_sync:
+        ros_arguments.extend(["-p", "approximate_sync:={}".format(approximate_sync)])
+
+    return launch.LaunchDescription([
+
+        launch_ros.actions.ComposableNodeContainer(
+            name='filter_container',
+            namespace='',
+            package='rclcpp_components',
+            executable='component_container',
+            composable_node_descriptions=[
+                launch_ros.descriptions.ComposableNode(
+                    package='pcl_ros_tests_filters',
+                    plugin=dummy_plugin,
+                    name='dummy_publisher',
+                ),
+            ],
+            output='screen',
+        ),
+        launch_ros.actions.Node(
+            package='pcl_ros',
+            executable=filter_executable,
+            output="screen",
+            ros_arguments=ros_arguments
+        ),
+        launch_testing.actions.ReadyToTest()
+    ])
+
+
+class TestFilter(unittest.TestCase):
+    def test_filter_output(self):
+        wait_for_topics = WaitForTopics([('output', PointCloud2)], timeout=5.0)
+        assert wait_for_topics.wait()
+        assert 'output' in wait_for_topics.topics_received(), "Didn't receive message"
+        wait_for_topics.shutdown()