From 6360c0dd16150e5f80fac6dd5d4335ab2ae05ffe Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Mon, 27 Jun 2022 09:17:52 -0400 Subject: [PATCH 1/2] Fix default parameters in README (#65) (#71) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Fix default parameters in README Some default parameters in the README file are wrong after looking at the source code https://github.com/ros-perception/pointcloud_to_laserscan/blob/rolling/src/pointcloud_to_laserscan_node.cpp#L58 Co-authored-by: Chris Lalancette (cherry picked from commit be047d2a075ff946699ba1397b96d054244b967e) Co-authored-by: Carlos Andrés Álvarez Restrepo --- README.md | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/README.md b/README.md index 8f40e935..dd8db4d6 100644 --- a/README.md +++ b/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. From e851257c4b8578b752757d814efff9bf47643221 Mon Sep 17 00:00:00 2001 From: Kensei Nakamura Date: Fri, 1 Jul 2022 05:46:02 +0900 Subject: [PATCH 2/2] fix high cpu usage (#9) --- CMakeLists.txt | 2 ++ package.xml | 1 + src/pointcloud_to_laserscan_node.cpp | 4 +++- 3 files changed, 6 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index d3c7c876..c0b02b7c 100644 --- 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 index 39a7469a..4e69abda 100644 --- a/package.xml +++ b/package.xml @@ -26,6 +26,7 @@ tf2 tf2_ros tf2_sensor_msgs + pcl_ros launch launch_ros diff --git a/src/pointcloud_to_laserscan_node.cpp b/src/pointcloud_to_laserscan_node.cpp index d4566870..f3184fa3 100644 --- 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(); - 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());