Discovered improper vector handling, hopefully fixed parts of it

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,55 @@ 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++)