From b2db6e9bd0c3295982e6f35a9a22e371d4ca2af1 Mon Sep 17 00:00:00 2001 From: kintzhao Date: Fri, 8 Sep 2023 19:07:54 +0800 Subject: [PATCH] fixed the revert scan message rostopic echo /scan header: seq: 72339 stamp: secs: 1694170297 nsecs: 334361646 frame_id: "laser" angle_min: 3.1415927410125732 angle_max: -3.1415927410125732 angle_increment: -0.005235987715423107 time_increment: 5.263152706902474e-05 scan_time: 0.0631578341126442 range_min: 0.0 range_max: 50.0 --- src/laser_feature_ros.cpp | 26 ++++++++++++++++++++------ 1 file changed, 20 insertions(+), 6 deletions(-) diff --git a/src/laser_feature_ros.cpp b/src/laser_feature_ros.cpp index 9414ea2..9175bef 100644 --- a/src/laser_feature_ros.cpp +++ b/src/laser_feature_ros.cpp @@ -36,13 +36,27 @@ void LaserFeatureROS::compute_bearing(const sensor_msgs::LaserScan::ConstPtr &sc std::vector bearings, cos_bearings, sin_bearings; std::vector index; unsigned int i = 0; - for (double b = scan_msg->angle_min; b <= scan_msg->angle_max; b += scan_msg->angle_increment) + if( scan_msg.angle_increment < 0) + { + for (double b = scan_msg->angle_min; b >= scan_msg->angle_max; b += scan_msg->angle_increment) + { + bearings.push_back(b); + cos_bearings.push_back(cos(b)); + sin_bearings.push_back(sin(b)); + index.push_back(i); + i++; + } + } + else { - bearings.push_back(b); - cos_bearings.push_back(cos(b)); - sin_bearings.push_back(sin(b)); - index.push_back(i); - i++; + for (double b = scan_msg->angle_min; b <= scan_msg->angle_max; b += scan_msg->angle_increment) + { + bearings.push_back(b); + cos_bearings.push_back(cos(b)); + sin_bearings.push_back(sin(b)); + index.push_back(i); + i++; + } } line_feature_.setCosSinData(bearings, cos_bearings, sin_bearings, index);