Merge branch 'fix/laser_handling_minor' into 'master'

Fix/laser handling minor

See merge request !31

CMakeLists.txt

 cmake_minimum_required(VERSION 3.0.0)
 project(homer_mapping)
 
-## Compile as C++14
-set(CMAKE_CXX_STANDARD 14)
+## Compile as C++17
+set(CMAKE_CXX_STANDARD 17)
 set(CMAKE_BUILD_TYPE Release)
 set(CMAKE_CXX_FLAGS "-Wreturn-type -fpermissive")
 

include/homer_mapping/slam_node.h

@@ -170,5 +170,6 @@ private:
      */
 
     const unsigned int MAX_ODOM_QUEUE_SIZE = 20;
+    const double m_MapSendingDelaySecs = 0.5;
 };
 

src/OccupancyMap/OccupancyMap.cpp

@@ -490,7 +490,6 @@ tf::StampedTransform OccupancyMap::getLaserTransform(std::string frame_id) const
 vector<MeasurePoint> OccupancyMap::getMeasurePoints(sensor_msgs::LaserScanConstPtr laserData) const
 {
     vector<MeasurePoint> result;
-    result.reserve(laserData->ranges.size());
 
     const double minDist = m_MeasureSamplingStep;
 
@@ -501,54 +500,56 @@ vector<MeasurePoint> OccupancyMap::getMeasurePoints(sensor_msgs::LaserScanConstP
     int laser_count = 0;
     for (const auto& range : laserData->ranges)
     {
-        if (range <= laserData->range_max && range >= laserData->range_min)
+        if (range > laserData->range_max || range < laserData->range_min)
         {
-            float alpha = laserData->angle_min
-                + static_cast<float>(laser_count) * laserData->angle_increment;
-            tf::Vector3 pin;
-            tf::Vector3 pout;
-            pin.setX(cos(alpha) * range);
-            pin.setY(sin(alpha) * range);
-            pin.setZ(0);
+            laser_count++;
+            continue;
+        }
+        float alpha
+            = laserData->angle_min + static_cast<float>(laser_count) * laserData->angle_increment;
+        tf::Vector3 pin;
+        tf::Vector3 pout;
+        pin.setX(cos(alpha) * range);
+        pin.setY(sin(alpha) * range);
+        pin.setZ(0);
 
-            pout = getLaserTransform(laserData->header.frame_id) * pin;
+        pout = getLaserTransform(laserData->header.frame_id) * pin;
 
-            Point2D hitPos(pout.x(), pout.y());
+        Point2D hitPos(pout.x(), pout.y());
 
-            if (hitPos.distance(lastUsedHitPos) >= minDist)
+        if (hitPos.distance(lastUsedHitPos) >= minDist)
+        {
+            // in-place construction in returned result
+            auto& p = result.emplace_back();
+            // preserve borders of segments
+            if ((laser_count != 0)
+                && (lastUsedHitPos.distance(lastHitPos) > m_metaData.resolution * 0.5)
+                && (hitPos.distance(lastHitPos) >= minDist * 1.5))
             {
-                // in-place construction in returned result
-                result.emplace_back();
-                auto& p = result.back();
-                // preserve borders of segments
-                if ((laser_count != 0)
-                    && (lastUsedHitPos.distance(lastHitPos) > m_metaData.resolution * 0.5)
-                    && (hitPos.distance(lastHitPos) >= minDist * 1.5))
-                {
-                    p.hitPos = lastHitPos;
-                    p.borderType = RightBorder;
-                    result.push_back(p);
-                    p.borderType = LeftBorder;
-                }
-                else
-                {
-                    p.borderType = NoBorder;
-                }
-                // TODO (DM): Given positive if statement, this attribute gets re-written!
-                p.hitPos = hitPos;
-                lastUsedHitPos = hitPos;
+                p.hitPos = lastHitPos;
+                p.borderType = RightBorder;
+                result.push_back(p);
+                // TODO (DM): This is so weird. Check earlier implementations if it is still working
+                // as expected? :D
+                p.borderType = LeftBorder;
+            }
+            else
+            {
+                p.borderType = NoBorder;
             }
-            lastHitPos = hitPos;
+            // TODO (DM): Given positive if statement, this attribute gets re-written!
+            p.hitPos = hitPos;
+            lastUsedHitPos = hitPos;
         }
+        lastHitPos = hitPos;
         laser_count++;
     }
+    if (result.empty())
+        return result;
 
     // the first and last one are border pixels
-    if (result.size() > 0)
-    {
-        result[0].borderType = LeftBorder;
-        result[result.size() - 1].borderType = RightBorder;
-    }
+    result[0].borderType = LeftBorder;
+    result[result.size() - 1].borderType = RightBorder;
 
     // calculate front check points
     for (unsigned i = 0; i < result.size(); i++)

src/slam_node.cpp

@@ -234,7 +234,8 @@ void SlamNode::callbackOdometry(const nav_msgs::Odometry::ConstPtr& msg)
             sendTfAndPose(pose, laserData->header.stamp);
 
             // send map max. every 500 ms
-            if ((laserData->header.stamp - m_LastMapSendTime).toSec() > 0.5 && m_DoMapping)
+            if ((laserData->header.stamp - m_LastMapSendTime).toSec() > m_MapSendingDelaySecs
+                && m_DoMapping)
             {
                 sendMapDataMessage(laserData->header.stamp);
                 m_LastMapSendTime = laserData->header.stamp;