fix high cpu usage (ros-perception#9)

knb · Jun 30, 2022 · e851257 · e851257
1 parent 6360c0d
commit e851257
Show file tree

Hide file tree

Showing 3 changed files with 6 additions and 1 deletion.
diff --git a/CMakeLists.txt b/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"

diff --git a/package.xml b/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>

diff --git a/src/pointcloud_to_laserscan_node.cpp b/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());