fix high cpu usage (#9)

CMakeLists.txt

@@ -11,6 +11,7 @@ find_package(sensor_msgs REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(tf2_sensor_msgs REQUIRED)
+find_package(pcl_ros REQUIRED)
 
 include_directories(include)
 
@@ -45,6 +46,7 @@ ament_target_dependencies(pointcloud_to_laserscan
   "tf2"
   "tf2_ros"
   "tf2_sensor_msgs"
+  "pcl_ros"
 )
 rclcpp_components_register_node(pointcloud_to_laserscan
   PLUGIN "pointcloud_to_laserscan::PointCloudToLaserScanNode"

README.md

@@ -17,15 +17,15 @@ This ROS 2 component projects `sensor_msgs/msg/PointCloud2` messages into `senso
 
 ### Parameters
 
-* `min_height` (double, default: 0.0) - The minimum height to sample in the point cloud in meters.
-* `max_height` (double, default: 1.0) - The maximum height to sample in the point cloud in meters.
-* `angle_min` (double, default: -π/2) - The minimum scan angle in radians.
-* `angle_max` (double, default: π/2) - The maximum scan angle in radians.
-* `angle_increment` (double, default: π/360) - Resolution of laser scan in radians per ray.
+* `min_height` (double, default: 2.2e-308) - The minimum height to sample in the point cloud in meters.
+* `max_height` (double, default: 1.8e+308) - The maximum height to sample in the point cloud in meters.
+* `angle_min` (double, default: -π) - The minimum scan angle in radians.
+* `angle_max` (double, default: π) - The maximum scan angle in radians.
+* `angle_increment` (double, default: π/180) - Resolution of laser scan in radians per ray.
 * `queue_size` (double, default: detected number of cores) - Input point cloud queue size.
 * `scan_time` (double, default: 1.0/30.0) - The scan rate in seconds. Only used to populate the scan_time field of the output laser scan message.
-* `range_min` (double, default: 0.45) - The minimum ranges to return in meters.
-* `range_max` (double, default: 4.0) - The maximum ranges to return in meters.
+* `range_min` (double, default: 0.0) - The minimum ranges to return in meters.
+* `range_max` (double, default: 1.8e+308) - The maximum ranges to return in meters.
 * `target_frame` (str, default: none) - If provided, transform the pointcloud into this frame before converting to a laser scan. Otherwise, laser scan will be generated in the same frame as the input point cloud.
 * `transform_tolerance` (double, default: 0.01) - Time tolerance for transform lookups. Only used if a `target_frame` is provided.
 * `use_inf` (boolean, default: true) - If disabled, report infinite range (no obstacle) as range_max + 1. Otherwise report infinite range as +inf.
@@ -47,4 +47,3 @@ This ROS 2 component re-publishes `sensor_msgs/msg/LaserScan` messages as `senso
 * `queue_size` (double, default: detected number of cores) - Input laser scan queue size.
 * `target_frame` (str, default: none) - If provided, transform the pointcloud into this frame before converting to a laser scan. Otherwise, laser scan will be generated in the same frame as the input point cloud.
 * `transform_tolerance` (double, default: 0.01) - Time tolerance for transform lookups. Only used if a `target_frame` is provided.
-* `use_inf` (boolean, default: true) - If disabled, report infinite range (no obstacle) as range_max + 1. Otherwise report infinite range as +inf.

package.xml

@@ -26,6 +26,7 @@
     <depend>tf2</depend>
     <depend>tf2_ros</depend>
     <depend>tf2_sensor_msgs</depend>
+    <depend>pcl_ros</depend>
 
     <exec_depend>launch</exec_depend>
     <exec_depend>launch_ros</exec_depend>

src/pointcloud_to_laserscan_node.cpp

@@ -52,6 +52,8 @@
 #include "tf2_sensor_msgs/tf2_sensor_msgs.h"
 #include "tf2_ros/create_timer_ros.h"
 
+#include "pcl_ros/transforms.hpp"
+
 namespace pointcloud_to_laserscan
 {
 
@@ -167,7 +169,7 @@ void PointCloudToLaserScanNode::cloudCallback(
   if (scan_msg->header.frame_id != cloud_msg->header.frame_id) {
     try {
       auto cloud = std::make_shared<sensor_msgs::msg::PointCloud2>();
-      tf2_->transform(*cloud_msg, *cloud, target_frame_, tf2::durationFromSec(tolerance_));
+      pcl_ros::transformPointCloud(scan_msg->header.frame_id, *cloud_msg, *cloud, *tf2_);
       cloud_msg = cloud;
     } catch (tf2::TransformException & ex) {
       RCLCPP_ERROR_STREAM(this->get_logger(), "Transform failure: " << ex.what());