Merge pull request #14 from efernandez/fix-inverted-lidar

Fix inverted LiDAR support
iris-ua · Jun 10, 2020 · 594b919 · 594b919
2 parents 0511981 + eebd205
commit 594b919
Show file tree

Hide file tree

Showing 3 changed files with 9 additions and 24 deletions.
diff --git a/include/lama/ros/slam2d_ros.h b/include/lama/ros/slam2d_ros.h
@@ -106,7 +106,6 @@ class Slam2DROS {
     // == Laser stuff ==
     // allow to handle multiple lasers at once
     std::map<std::string, int> frame_to_laser_; ///< Map with the known lasers.
-    std::vector<bool>          laser_is_reversed_;  ///< Vector that signals if the laser is reversed
     std::vector<Pose3D, Eigen::aligned_allocator<Pose3D>>        lasers_origin_;  ///< Laser origin transformation
     double max_range_;
 

diff --git a/src/pf_slam2d_ros.cpp b/src/pf_slam2d_ros.cpp
@@ -152,8 +152,10 @@ void lama::PFSlam2DROS::onLaserScan(const sensor_msgs::LaserScanConstPtr& laser_
         catch(tf::TransformException& e)
         { ROS_ERROR("Could not find origin of %s", laser_scan->header.frame_id.c_str()); return; }
 
+        double roll, pitch, yaw;
+        laser_origin.getBasis().getRPY(roll, pitch, yaw);
         Pose3D lp(laser_origin.getOrigin().x(), laser_origin.getOrigin().y(), 0,
-                  0, 0, tf::getYaw(laser_origin.getRotation()));
+                  roll, pitch, yaw);
 
         lasers_origin_.push_back( lp );
 

diff --git a/src/slam2d_ros.cpp b/src/slam2d_ros.cpp
@@ -161,12 +161,7 @@ void lama::Slam2DROS::onLaserScan(const sensor_msgs::LaserScanConstPtr& laser_sc
 
         cloud->points.reserve(laser_scan->ranges.size());
         for(size_t i = 0; i < size; i += beam_step ){
-            double range;
-
-            if (laser_is_reversed_[laser_index])
-                range = laser_scan->ranges[size - i - 1];
-            else
-                range = laser_scan->ranges[i];
+            const double range = laser_scan->ranges[i];
 
             if (not std::isfinite(range))
                 continue;
@@ -246,23 +241,12 @@ bool lama::Slam2DROS::initLaser(const sensor_msgs::LaserScanConstPtr& laser_scan
         return false;
     }
 
-    if (up.z() > 0) {
-        laser_is_reversed_.push_back(laser_scan->angle_min > laser_scan->angle_max);
-
-        Pose3D lp(laser_origin.getOrigin().x(), laser_origin.getOrigin().y(), 0,
-                  0, 0, tf::getYaw(laser_origin.getRotation()));
-
-        lasers_origin_.push_back( lp );
-        ROS_INFO("Laser is mounted upwards.");
-    } else {
-        laser_is_reversed_.push_back(laser_scan->angle_min < laser_scan->angle_max);
-
-        Pose3D lp(laser_origin.getOrigin().x(), laser_origin.getOrigin().y(), 0,
-                  M_PI, 0, tf::getYaw(laser_origin.getRotation()));
+    double roll, pitch, yaw;
+    laser_origin.getBasis().getRPY(roll, pitch, yaw);
+    Pose3D lp(laser_origin.getOrigin().x(), laser_origin.getOrigin().y(), 0,
+              roll, pitch, yaw);
 
-        lasers_origin_.push_back( lp );
-        ROS_INFO("Laser is mounted upside down.");
-    }
+    lasers_origin_.push_back( lp );
 
     int laser_index = (int)frame_to_laser_.size();  // simple ID generator :)
     frame_to_laser_[laser_scan->header.frame_id] = laser_index;