Migrate pcd to pointcloud tool (#359)

* Migrate pcl_ros pcd_to_pointcloud tool

Signed-off-by: Ivan Santiago Paunovic <ivanpauno@ekumenlabs.com>

* Remove log used for debugging

Signed-off-by: Ivan Santiago Paunovic <ivanpauno@ekumenlabs.com>

* Undo unnecessary changes

Signed-off-by: Ivan Santiago Paunovic <ivanpauno@ekumenlabs.com>

* Remove commented out code

Signed-off-by: Ivan Santiago Paunovic <ivanpauno@ekumenlabs.com>

* Address peer review comments

Signed-off-by: Ivan Santiago Paunovic <ivanpauno@ekumenlabs.com>

* rename node

Signed-off-by: Ivan Santiago Paunovic <ivanpauno@ekumenlabs.com>

pcl_ros/CMakeLists.txt

@@ -10,12 +10,13 @@ endif()
 
 ## Find system dependencies
 find_package(Eigen3 REQUIRED)
-find_package(PCL REQUIRED QUIET COMPONENTS core features filters io segmentation surface)
+find_package(PCL REQUIRED QUIET COMPONENTS core common features filters io segmentation surface)
 
 ## Find ROS package dependencies
 find_package(ament_cmake REQUIRED)
 find_package(pcl_conversions REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(tf2 REQUIRED)
@@ -118,8 +119,19 @@ ament_target_dependencies(pcl_ros_tf
 #
 ### Tools
 #
-#add_executable(pcd_to_pointcloud tools/pcd_to_pointcloud.cpp)
+add_library(pcd_to_pointcloud_lib SHARED tools/pcd_to_pointcloud.cpp)
-#target_link_libraries(pcd_to_pointcloud ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES})
+target_link_libraries(pcd_to_pointcloud_lib
+  ${PCL_LIBRARIES})
+target_include_directories(pcd_to_pointcloud_lib PUBLIC
+  ${PCL_INCLUDE_DIRS})
+ament_target_dependencies(pcd_to_pointcloud_lib
+  rclcpp
+  rclcpp_components
+  sensor_msgs
+  pcl_conversions)
+rclcpp_components_register_node(pcd_to_pointcloud_lib
+  PLUGIN "pcl_ros::PCDPublisher"
+  EXECUTABLE pcd_to_pointcloud)
 #
 #add_executable(pointcloud_to_pcd tools/pointcloud_to_pcd.cpp)
 #target_link_libraries(pointcloud_to_pcd ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES})
@@ -169,12 +181,12 @@ install(
 install(
   TARGETS
     pcl_ros_tf
+    pcd_to_pointcloud_lib
 #    pcl_ros_io
 #    pcl_ros_features
 #    pcl_ros_filters
 #    pcl_ros_surface
 #    pcl_ros_segmentation
-#    pcd_to_pointcloud
 #    pointcloud_to_pcd
 #    bag_to_pcd
 #    convert_pcd_to_image

pcl_ros/package.xml

@@ -30,6 +30,7 @@
   <depend>libpcl-all-dev</depend>
   <depend>pcl_conversions</depend>
   <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
   <depend>sensor_msgs</depend>
   <depend>geometry_msgs</depend>
   <depend>tf2</depend>

pcl_ros/tools/pcd_to_pointcloud.cpp

@@ -43,126 +43,85 @@
 
  **/
 
-// ROS core
-#include <ros/ros.h>
-#include <pcl/io/io.h>
-#include <pcl/io/pcd_io.h>
-#include <pcl/point_types.h>
-#include <pcl_conversions/pcl_conversions.h>
-
 // STL
 #include <chrono>
 #include <string>
 #include <thread>
 
-#include "pcl_ros/publisher.hpp"
+// PCL
+#include <pcl/io/io.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_types.h>
 
-class PCDGenerator
+// ROS core
+#include <rclcpp/rclcpp.hpp>
+#include "rclcpp_components/register_node_macro.hpp"
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <pcl_conversions/pcl_conversions.h>
+
+namespace pcl_ros
+{
+class PCDPublisher : public rclcpp::Node
 {
 protected:
   std::string tf_frame_;
-  ros::NodeHandle nh_;
-  ros::NodeHandle private_nh_;
 
 public:
   // ROS messages
-  sensor_msgs::PointCloud2 cloud_;
+  sensor_msgs::msg::PointCloud2 cloud_;
 
   std::string file_name_, cloud_topic_;
-  double wait_;
+  size_t period_ms_;
 
-  pcl_ros::Publisher<sensor_msgs::PointCloud2> pub_;
+  std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::PointCloud2>> pub_;
+  rclcpp::TimerBase::SharedPtr timer_;
 
   ////////////////////////////////////////////////////////////////////////////////
-  PCDGenerator()
+  explicit PCDPublisher(const rclcpp::NodeOptions & options)
-  : tf_frame_("/base_link"), private_nh_("~")
+  : rclcpp::Node("pcd_publisher", options), tf_frame_("/base_link")
   {
     // Maximum number of outgoing messages to be queued for delivery to subscribers = 1
 
     cloud_topic_ = "cloud_pcd";
-    pub_.advertise(nh_, cloud_topic_.c_str(), 1);
+    tf_frame_ = this->declare_parameter("tf_frame", tf_frame_);
-    private_nh_.param("frame_id", tf_frame_, std::string("/base_link"));
+    period_ms_ = this->declare_parameter("publishing_period_ms", 3000);
-    ROS_INFO(
+    file_name_ = this->declare_parameter<std::string>("file_name");
-      "Publishing data on topic %s with frame_id %s.", nh_.resolveName(
-        cloud_topic_).c_str(), tf_frame_.c_str());
-  }
 
-  ////////////////////////////////////////////////////////////////////////////////
-  // Start
-  int
-  start()
-  {
     if (file_name_ == "" || pcl::io::loadPCDFile(file_name_, cloud_) == -1) {
-      return -1;
+      RCLCPP_ERROR(this->get_logger(), "failed to open PCD file");
+      throw std::runtime_error{"could not open pcd file"};
     }
     cloud_.header.frame_id = tf_frame_;
-    return 0;
+    int nr_points = cloud_.width * cloud_.height;
+
+    auto fields_list = pcl::getFieldsList(cloud_);
+    auto resolved_cloud_topic =
+      this->get_node_topics_interface()->resolve_topic_name(cloud_topic_);
+
+    pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>(cloud_topic_, 10);
+    timer_ = this->create_wall_timer(
+      std::chrono::milliseconds(period_ms_),
+      [this]() {
+        this->publish();
+      });
+
+    RCLCPP_INFO(
+      this->get_logger(),
+      "Publishing data with %d points (%s) on topic %s in frame %s.",
+      nr_points,
+      fields_list.c_str(),
+      resolved_cloud_topic.c_str(),
+      cloud_.header.frame_id.c_str());
   }
 
   ////////////////////////////////////////////////////////////////////////////////
-  // Spin (!)
+  // Publish callback that is periodically called by a timer.
-  bool spin()
+  void publish()
   {
-    int nr_points = cloud_.width * cloud_.height;
+    cloud_.header.stamp = this->get_clock()->now();
-    std::string fields_list = pcl::getFieldsList(cloud_);
+    pub_->publish(cloud_);
-    double interval = wait_ * 1e+6;
-    while (nh_.ok()) {
-      ROS_DEBUG_ONCE(
-        "Publishing data with %d points (%s) on topic %s in frame %s.", nr_points,
-        fields_list.c_str(), nh_.resolveName(cloud_topic_).c_str(), cloud_.header.frame_id.c_str());
-      cloud_.header.stamp = ros::Time::now();
-
-      if (pub_.getNumSubscribers() > 0) {
-        ROS_DEBUG("Publishing data to %d subscribers.", pub_.getNumSubscribers());
-        pub_.publish(cloud_);
-      } else {
-        // check once a second if there is any subscriber
-        ros::Duration(1).sleep();
-        continue;
-      }
-
-      std::this_thread::sleep_for(std::chrono::microseconds(static_cast<uint32_t>(interval)));
-
-      if (interval == 0) {      // We only publish once if a 0 seconds interval is given
-        // Give subscribers 3 seconds until point cloud decays... a little ugly!
-        ros::Duration(3.0).sleep();
-        break;
-      }
-    }
-    return true;
   }
 };
+}  // namespace pcl_ros
 
-/* ---[ */
+RCLCPP_COMPONENTS_REGISTER_NODE(pcl_ros::PCDPublisher)
-int
-main(int argc, char ** argv)
-{
-  if (argc < 2) {
-    std::cerr << "Syntax is: " << argv[0] << " <file.pcd> [publishing_interval (in seconds)]" <<
-      std::endl;
-    return -1;
-  }
-
-  ros::init(argc, argv, "pcd_to_pointcloud");
-
-  PCDGenerator c;
-  c.file_name_ = std::string(argv[1]);
-  // check if publishing interval is given
-  if (argc == 2) {
-    c.wait_ = 0;
-  } else {
-    c.wait_ = atof(argv[2]);
-  }
-
-  if (c.start() == -1) {
-    ROS_ERROR("Could not load file %s. Exiting.", argv[1]);
-    return -1;
-  }
-  ROS_INFO(
-    "Loaded a point cloud with %d points (total size is %zu) and the following channels: %s.",
-    c.cloud_.width * c.cloud_.height, c.cloud_.data.size(), pcl::getFieldsList(c.cloud_).c_str());
-  c.spin();
-
-  return 0;
-}
-/* ]--- */