From d62397f9ecedb3eeb7cb34767964548373ffebcd Mon Sep 17 00:00:00 2001 From: Leonardo Oliveira Wellausen Date: Tue, 3 Oct 2023 15:09:39 +0300 Subject: [PATCH] Adding LaserScan frame --- ouster-ros/config/driver_params.yaml | 8 +++++-- ouster-ros/src/os_cloud_node.cpp | 2 +- ouster-ros/src/os_driver_node.cpp | 2 +- .../src/os_static_transforms_broadcaster.h | 21 ++++++++++++++++++- 4 files changed, 28 insertions(+), 5 deletions(-) diff --git a/ouster-ros/config/driver_params.yaml b/ouster-ros/config/driver_params.yaml index 0520ab0d..35c324b1 100644 --- a/ouster-ros/config/driver_params.yaml +++ b/ouster-ros/config/driver_params.yaml @@ -53,9 +53,13 @@ ouster/os_driver: # imu_frame[optional]: name to use when referring to the imu frame. imu_frame: os_imu # point_cloud_frame[optional]: which frame of reference to use when - # generating PointCloud2 or LaserScan messages, select between the values of + # generating PointCloud2 messages, select between the values of # lidar_frame and sensor_frame. - point_cloud_frame: os_lidar + point_cloud_frame: os_lidar + # laser_scan_frame[optional]: which frame of reference to use when + # generating LaserScan messages, select between the values of + # lidar_frame and sensor_frame. + laser_scan_frame: os_lidar # proc_mask[optional]: use any combination of the 4 flags IMG, PCL, IMU and # SCAN to enable or disable their respective messages. proc_mask: IMG|PCL|IMU|SCAN diff --git a/ouster-ros/src/os_cloud_node.cpp b/ouster-ros/src/os_cloud_node.cpp index a111242b..d2eeefbd 100644 --- a/ouster-ros/src/os_cloud_node.cpp +++ b/ouster-ros/src/os_cloud_node.cpp @@ -139,7 +139,7 @@ class OusterCloud : public OusterProcessingNodeBase { } processors.push_back(LaserScanProcessor::create( - info, tf_bcast.lidar_frame_id(), scan_ring, + info, tf_bcast.laser_scan_frame_id(), scan_ring, [this](LaserScanProcessor::OutputType msgs) { for (size_t i = 0; i < msgs.size(); ++i) scan_pubs[i]->publish(*msgs[i]); diff --git a/ouster-ros/src/os_driver_node.cpp b/ouster-ros/src/os_driver_node.cpp index 839d33e6..51063879 100644 --- a/ouster-ros/src/os_driver_node.cpp +++ b/ouster-ros/src/os_driver_node.cpp @@ -108,7 +108,7 @@ class OusterDriver : public OusterSensor { } processors.push_back(LaserScanProcessor::create( - info, tf_bcast.lidar_frame_id(), scan_ring, + info, tf_bcast.laser_scan_frame_id(), scan_ring, [this](LaserScanProcessor::OutputType msgs) { for (size_t i = 0; i < msgs.size(); ++i) scan_pubs[i]->publish(*msgs[i]); diff --git a/ouster-ros/src/os_static_transforms_broadcaster.h b/ouster-ros/src/os_static_transforms_broadcaster.h index 760b5cda..3bea4a94 100644 --- a/ouster-ros/src/os_static_transforms_broadcaster.h +++ b/ouster-ros/src/os_static_transforms_broadcaster.h @@ -23,6 +23,7 @@ class OusterStaticTransformsBroadcaster { node->declare_parameter("lidar_frame", "os_lidar"); node->declare_parameter("imu_frame", "os_imu"); node->declare_parameter("point_cloud_frame", ""); + node->declare_parameter("laser_scan_frame", ""); } void parse_parameters() { @@ -31,8 +32,10 @@ class OusterStaticTransformsBroadcaster { imu_frame = node->get_parameter("imu_frame").as_string(); point_cloud_frame = node->get_parameter("point_cloud_frame").as_string(); + laser_scan_frame = + node->get_parameter("laser_scan_frame").as_string(); - // validate point_cloud_frame + // validate point_cloud_frame and laser_scan_frame if (point_cloud_frame.empty()) { point_cloud_frame = lidar_frame; // for ROS1 we'd still use sensor_frame @@ -45,6 +48,18 @@ class OusterStaticTransformsBroadcaster { "value for point_cloud_frame"); point_cloud_frame = lidar_frame; } + if (laser_scan_frame.empty()) { + laser_scan_frame = + lidar_frame; // for ROS1 we'd still use sensor_frame + } else if (laser_scan_frame != sensor_frame && + laser_scan_frame != lidar_frame) { + RCLCPP_WARN(node->get_logger(), + "laser_scan_frame value needs to match the value of " + "either sensor_frame or lidar_frame but a different " + "value was supplied, using lidar_frame's value as the " + "value for point_cloud_frame"); + laser_scan_frame = lidar_frame; + } } void broadcast_transforms(const sensor::sensor_info& info) { @@ -62,6 +77,9 @@ class OusterStaticTransformsBroadcaster { const std::string& point_cloud_frame_id() const { return point_cloud_frame; } + const std::string& laser_scan_frame_id() const { + return laser_scan_frame; + } bool apply_lidar_to_sensor_transform() const { return point_cloud_frame == sensor_frame; } @@ -73,6 +91,7 @@ class OusterStaticTransformsBroadcaster { std::string lidar_frame; std::string sensor_frame; std::string point_cloud_frame; + std::string laser_scan_frame; }; } // namespace ouster_ros