diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt
index 4441c1be..258f2366 100644
--- a/pcl_ros/CMakeLists.txt
+++ b/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)
-#target_link_libraries(pcd_to_pointcloud ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES})
+add_library(pcd_to_pointcloud_lib SHARED tools/pcd_to_pointcloud.cpp)
+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
diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml
index c736fcad..81b1c6cb 100644
--- a/pcl_ros/package.xml
+++ b/pcl_ros/package.xml
@@ -30,6 +30,7 @@
libpcl-all-dev
pcl_conversions
rclcpp
+ rclcpp_components
sensor_msgs
geometry_msgs
tf2
diff --git a/pcl_ros/tools/pcd_to_pointcloud.cpp b/pcl_ros/tools/pcd_to_pointcloud.cpp
index 3119f91e..5c51a774 100644
--- a/pcl_ros/tools/pcd_to_pointcloud.cpp
+++ b/pcl_ros/tools/pcd_to_pointcloud.cpp
@@ -43,126 +43,85 @@
**/
-// ROS core
-#include
-#include
-#include
-#include
-#include
-
// STL
#include
#include
#include
-#include "pcl_ros/publisher.hpp"
+// PCL
+#include
+#include
+#include
-class PCDGenerator
+// ROS core
+#include
+#include "rclcpp_components/register_node_macro.hpp"
+#include
+#include
+
+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 pub_;
+ std::shared_ptr> pub_;
+ rclcpp::TimerBase::SharedPtr timer_;
////////////////////////////////////////////////////////////////////////////////
- PCDGenerator()
- : tf_frame_("/base_link"), private_nh_("~")
+ explicit PCDPublisher(const rclcpp::NodeOptions & options)
+ : 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);
- private_nh_.param("frame_id", tf_frame_, std::string("/base_link"));
- ROS_INFO(
- "Publishing data on topic %s with frame_id %s.", nh_.resolveName(
- cloud_topic_).c_str(), tf_frame_.c_str());
- }
+ tf_frame_ = this->declare_parameter("tf_frame", tf_frame_);
+ period_ms_ = this->declare_parameter("publishing_period_ms", 3000);
+ file_name_ = this->declare_parameter("file_name");
- ////////////////////////////////////////////////////////////////////////////////
- // 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(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 (!)
- bool spin()
+ // Publish callback that is periodically called by a timer.
+ void publish()
{
- int nr_points = cloud_.width * cloud_.height;
- std::string fields_list = pcl::getFieldsList(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(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;
+ cloud_.header.stamp = this->get_clock()->now();
+ pub_->publish(cloud_);
}
};
+} // namespace pcl_ros
-/* ---[ */
-int
-main(int argc, char ** argv)
-{
- if (argc < 2) {
- std::cerr << "Syntax is: " << argv[0] << " [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;
-}
-/* ]--- */
+RCLCPP_COMPONENTS_REGISTER_NODE(pcl_ros::PCDPublisher)