diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index 7311251c7..d2c4940dd 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -132,8 +132,19 @@ target_link_libraries(pcl_ros_tf ${PCL_LIBRARIES}) #add_executable(convert_pcd_to_image tools/convert_pcd_to_image.cpp) #target_link_libraries(convert_pcd_to_image ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) # -#add_executable(convert_pointcloud_to_image tools/convert_pointcloud_to_image.cpp) -#target_link_libraries(convert_pointcloud_to_image ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) +add_executable(convert_pointcloud_to_image tools/convert_pointcloud_to_image.cpp) +target_link_libraries(convert_pointcloud_to_image ${PCL_LIBRARIES}) +target_include_directories(convert_pointcloud_to_image PUBLIC + ${PCL_INCLUDE_DIRS}) +ament_target_dependencies(convert_pointcloud_to_image + ${dependencies}) + +### Install + +install(TARGETS convert_pointcloud_to_image pcl_ros_tf + DESTINATION lib/${PROJECT_NAME} +) + # ### Downloads # diff --git a/pcl_ros/package.xml b/pcl_ros/package.xml index c869b7481..9d2e2c313 100644 --- a/pcl_ros/package.xml +++ b/pcl_ros/package.xml @@ -25,7 +25,7 @@ Julius Kammerl William Woodall - ament_cmake + ament_cmake_ros eigen libpcl-all-dev pcl_conversions diff --git a/pcl_ros/tools/convert_pointcloud_to_image.cpp b/pcl_ros/tools/convert_pointcloud_to_image.cpp index c692f5063..7ad452bb1 100644 --- a/pcl_ros/tools/convert_pointcloud_to_image.cpp +++ b/pcl_ros/tools/convert_pointcloud_to_image.cpp @@ -39,66 +39,75 @@ \author Ethan Rublee **/ // ROS core -#include +#include // Image message -#include -#include +#include +#include // pcl::toROSMsg #include // conversions from PCL custom types #include // stl stuff #include +// to use make_shared<> +#include -class PointCloudToImage +class PointCloudToImage : public rclcpp::Node { public: void - cloud_cb(const sensor_msgs::PointCloud2ConstPtr & cloud) + cloud_cb(const sensor_msgs::msg::PointCloud2::SharedPtr cloud) { if (cloud->height <= 1) { - ROS_ERROR("Input point cloud is not organized, ignoring!"); + RCLCPP_ERROR(this->get_logger(), "Input point cloud is not organized, ignoring!"); return; } try { - pcl::toROSMsg(*cloud, image_); // convert the cloud - image_.header = cloud->header; - image_pub_.publish(image_); // publish our cloud image + pcl::toROSMsg(*cloud, this->image_); // convert the cloud + this->image_.header = cloud->header; + this->image_pub_->publish(this->image_); // publish our cloud image } catch (std::runtime_error & e) { - ROS_ERROR_STREAM( + RCLCPP_ERROR_STREAM( + this->get_logger(), "Error in converting cloud to image message: " << - e.what()); + e.what() + ); } } - PointCloudToImage() - : cloud_topic_("input"), image_topic_("output") + explicit PointCloudToImage(const std::string name) + : Node(name), cloud_topic_("input"), image_topic_("output") { - sub_ = nh_.subscribe( + this->sub_ = this->create_subscription( cloud_topic_, 30, - &PointCloudToImage::cloud_cb, this); - image_pub_ = nh_.advertise(image_topic_, 30); + std::bind(&PointCloudToImage::cloud_cb, this, std::placeholders::_1)); + + image_pub_ = this->create_publisher(image_topic_, 30); // print some info about the node - std::string r_ct = nh_.resolveName(cloud_topic_); - std::string r_it = nh_.resolveName(image_topic_); - ROS_INFO_STREAM("Listening for incoming data on topic " << r_ct); - ROS_INFO_STREAM("Publishing image on topic " << r_it); + // FIXME! + // Foxy doesn't have resolve_topic_name(), newer versions can use this to resolve remaps + // std::string r_ct = this->get_node_topics_interface()->resolve_topic_name(cloud_topic_); + // std::string r_it = this->get_node_topics_interface()->resolve_topic_name(image_topic_); + std::string r_ct = cloud_topic_; + std::string r_it = image_topic_; + RCLCPP_INFO_STREAM(this->get_logger(), "Listening for incoming data on topic " << r_ct); + RCLCPP_INFO_STREAM(this->get_logger(), "Publishing image on topic " << r_it); } private: - ros::NodeHandle nh_; - sensor_msgs::Image image_; // cache the image message - std::string cloud_topic_; // default input + sensor_msgs::msg::Image image_; // cache the image message + std::string cloud_topic_; // default inputww std::string image_topic_; // default output - ros::Subscriber sub_; // cloud subscriber - ros::Publisher image_pub_; // image message publisher + rclcpp::Subscription::SharedPtr sub_; // cloud subscriber + rclcpp::Publisher::SharedPtr image_pub_; // image message publisher }; int main(int argc, char ** argv) { - ros::init(argc, argv, "convert_pointcloud_to_image"); - PointCloudToImage pci; // this loads up the node - ros::spin(); // where she stops nobody knows + rclcpp::init(argc, argv); // this loads up the node + rclcpp::spin( + std::make_shared("convert_pointcloud_to_image") + ); // where she stops nobody knows return 0; }