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)