From ff0dc3bf1e3150a3c3cd5bc837f501e007a3c079 Mon Sep 17 00:00:00 2001 From: Max Wittal Date: Thu, 23 Jan 2020 11:27:59 +0100 Subject: [PATCH] fixed transformation error for centered_laser_pose_ computation in case of laser mounted upside down tf::createQuaternionFromRPY() takes fixed angles as input such that angle_center is not affected by the 180 deg roll --- gmapping/src/slam_gmapping.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gmapping/src/slam_gmapping.cpp b/gmapping/src/slam_gmapping.cpp index 6516cd44..45896b4b 100644 --- a/gmapping/src/slam_gmapping.cpp +++ b/gmapping/src/slam_gmapping.cpp @@ -462,7 +462,7 @@ SlamGMapping::initMapper(const sensor_msgs::LaserScan& scan) else { do_reverse_range_ = scan.angle_min < scan.angle_max; - centered_laser_pose_ = tf::Stamped(tf::Transform(tf::createQuaternionFromRPY(M_PI,0,-angle_center), + centered_laser_pose_ = tf::Stamped(tf::Transform(tf::createQuaternionFromRPY(M_PI,0,angle_center), tf::Vector3(0,0,0)), ros::Time::now(), laser_frame_); ROS_INFO("Laser is mounted upside down."); }