Create a vector of obstacle observations from laser scan

KNR-Selfie · Apr 28, 2019 · 9dbbb9f · 9dbbb9f
1 parent bc84830
commit 9dbbb9f
Show file tree

Hide file tree

Showing 2 changed files with 45 additions and 8 deletions.
diff --git a/src/ObstacleObservationsExtractor.cpp b/src/ObstacleObservationsExtractor.cpp
@@ -19,13 +19,20 @@ ObstacleObservationsExtractor::ObstacleObservationsExtractor(IMeasurementValidat
 ObstacleObservations ObstacleObservationsExtractor::extractObstacleObservations(LaserScanPtr scan)
 {
   ObstacleObservations observations;
-
   for (int i = 0; i < scan->ranges.size(); i++)
   {
-    float range = scan->ranges[i];
-    if (validator_->isValid(range))
+    ObstacleObservationPtr observation(new ObstacleObservation());
+
+    while (i < scan->ranges.size() && validator_->isValid(scan->ranges[i]))
+    {
+      PointPtr point = transformer_->transformCoordinates(scan, i, scan->ranges[i]);
+      observation->push_back(point);
+      i++;
+    }
+
+    if (observation->size() > 0)
     {
-      transformer_->transformCoordinates(scan, i, scan->ranges[i]);
+      observations.push_back(observation);
     }
   }
 

diff --git a/test/ObstacleObservationsExtractorTests.cpp b/test/ObstacleObservationsExtractorTests.cpp
@@ -12,6 +12,8 @@ using sensor_msgs::LaserScanPtr;
 
 using selfie_obstacle_detection::Point;
 using selfie_obstacle_detection::PointPtr;
+using selfie_obstacle_detection::ObstacleObservationPtr;
+using selfie_obstacle_detection::ObstacleObservations;
 using selfie_obstacle_detection::IMeasurementValidator;
 using selfie_obstacle_detection::ICoordinatesTransformer;
 using selfie_obstacle_detection::ObstacleObservationsExtractor;
@@ -44,22 +46,50 @@ TEST(ObstacleObservationsExtractorTestSuite, basicTest)
   scan->ranges.push_back(0.0);
   scan->ranges.push_back(3.0);
   scan->ranges.push_back(5.0);
+  scan->ranges.push_back(4.0);
   scan->ranges.push_back(0.0);
+  scan->ranges.push_back(1.0);
+  scan->ranges.push_back(2.0);
 
   EXPECT_CALL(validator, isValid(_))
   .WillRepeatedly(Return(true));
 
   EXPECT_CALL(validator, isValid(0.0))
   .WillRepeatedly(Return(false));
 
-  PointPtr point(new Point(0, 0));
+  PointPtr p11(new Point(0, 0));
   EXPECT_CALL(transformer, transformCoordinates(scan, 1, 3.0))
-  .WillOnce(Return(point));
+  .WillOnce(Return(p11));
 
+  PointPtr p12(new Point(0, 0));
   EXPECT_CALL(transformer, transformCoordinates(scan, 2, 5.0))
-  .WillOnce(Return(point));
+  .WillOnce(Return(p12));
 
-  extractor.extractObstacleObservations(scan);
+  PointPtr p13(new Point(0, 0));
+  EXPECT_CALL(transformer, transformCoordinates(scan, 3, 4.0))
+  .WillOnce(Return(p13));
+
+  PointPtr p21(new Point(0, 0));
+  EXPECT_CALL(transformer, transformCoordinates(scan, 5, 1.0))
+  .WillOnce(Return(p21));
+
+  PointPtr p22(new Point(0, 0));
+  EXPECT_CALL(transformer, transformCoordinates(scan, 6, 2.0))
+  .WillOnce(Return(p22));
+
+  ObstacleObservations observations = extractor.extractObstacleObservations(scan);
+  ASSERT_EQ(observations.size(), 2);
+
+  ObstacleObservationPtr obs1 = observations[0];
+  ASSERT_EQ(obs1->size(), 3);
+  EXPECT_EQ((*obs1)[0], p11);
+  EXPECT_EQ((*obs1)[1], p12);
+  EXPECT_EQ((*obs1)[2], p13);
+
+  ObstacleObservationPtr obs2 = observations[1];
+  ASSERT_EQ(obs2->size(), 2);
+  EXPECT_EQ((*obs2)[0], p21);
+  EXPECT_EQ((*obs2)[1], p22);
 }
 
 int main(int argc, char **argv)