test(autoware_motion_utils): add tests for missed lines (#275)

Signed-off-by: NorahXiong <norah.xiong@autocore.ai>
Co-authored-by: Yutaka Kondo <yutaka.kondo@youtalk.jp>

Author: NorahXiong

common/autoware_motion_utils/test/src/distance/test_distance.cpp

@@ -111,6 +111,36 @@ TEST(distance, calcDecelDistWithJerkAndAccConstraints)
       current_vel, target_vel, current_acc, acc_min, jerk_acc, jerk_dec);
     EXPECT_NEAR(expected_dist, *dist, epsilon);
   }
+
+  // need to decelerate
+  {
+    constexpr double current_vel = 10.0;
+    constexpr double target_vel = 0.5;
+    constexpr double current_acc = -2.0;
+    constexpr double acc_min = -3.0;
+    constexpr double jerk_acc = 1.0;
+    constexpr double jerk_dec = -0.5;
+
+    constexpr double expected_dist = 21.3333;
+    const auto dist = calcDecelDistWithJerkAndAccConstraints(
+      current_vel, target_vel, current_acc, acc_min, jerk_acc, jerk_dec);
+    EXPECT_NEAR(expected_dist, *dist, epsilon);
+  }
+
+  // no need to decelerate
+  {
+    constexpr double current_vel = 16.7;
+    constexpr double target_vel = 16.7;
+    constexpr double current_acc = 0.0;
+    constexpr double acc_min = -0.5;
+    constexpr double jerk_acc = 1.0;
+    constexpr double jerk_dec = -0.5;
+
+    constexpr double expected_dist = 0.0;
+    const auto dist = calcDecelDistWithJerkAndAccConstraints(
+      current_vel, target_vel, current_acc, acc_min, jerk_acc, jerk_dec);
+    EXPECT_NEAR(expected_dist, *dist, epsilon);
+  }
 }
 
 }  // namespace

common/autoware_motion_utils/test/src/marker/test_virtual_wall_marker_creator.cpp

@@ -16,6 +16,7 @@
 #include "gtest/gtest.h"
 
 #include <string>
+#include <vector>
 
 namespace
 {
@@ -67,32 +68,37 @@ TEST(VirtualWallMarkerCreator, oneWall)
 TEST(VirtualWallMarkerCreator, manyWalls)
 {
   autoware::motion_utils::VirtualWall wall;
+  std::vector<autoware::motion_utils::VirtualWall> walls;
   autoware::motion_utils::VirtualWallMarkerCreator creator;
   wall.style = autoware::motion_utils::VirtualWallType::stop;
   wall.ns = "ns1_";
-  creator.add_virtual_wall(wall);
-  creator.add_virtual_wall(wall);
-  creator.add_virtual_wall(wall);
+  walls.push_back(wall);
+  walls.push_back(wall);
+  walls.push_back(wall);
   wall.ns = "ns2_";
-  creator.add_virtual_wall(wall);
+  walls.push_back(wall);
   wall.style = autoware::motion_utils::VirtualWallType::slowdown;
   wall.ns = "ns2_";
-  creator.add_virtual_wall(wall);
-  creator.add_virtual_wall(wall);
+  walls.push_back(wall);
+  walls.push_back(wall);
   wall.ns = "ns3_";
-  creator.add_virtual_wall(wall);
-  creator.add_virtual_wall(wall);
-  creator.add_virtual_wall(wall);
+  walls.push_back(wall);
+  walls.push_back(wall);
+  walls.push_back(wall);
   wall.style = autoware::motion_utils::VirtualWallType::deadline;
   wall.ns = "ns1_";
-  creator.add_virtual_wall(wall);
+  walls.push_back(wall);
   wall.ns = "ns2_";
-  creator.add_virtual_wall(wall);
+  walls.push_back(wall);
   wall.ns = "ns3_";
-  creator.add_virtual_wall(wall);
+  walls.push_back(wall);
+  wall.style = autoware::motion_utils::VirtualWallType::pass;
+  wall.ns = "ns1_";
+  walls.push_back(wall);
+  creator.add_virtual_walls(walls);
 
   auto markers = creator.create_markers();
-  ASSERT_EQ(markers.markers.size(), 12UL * 2);
+  ASSERT_EQ(markers.markers.size(), 13UL * 2);
   EXPECT_TRUE(has_ns_id(markers, std::string("ns1_stop") + wall_ns_suffix, 0, 2));
   EXPECT_TRUE(has_ns_id(markers, std::string("ns1_stop") + text_ns_suffix, 0, 2));
   EXPECT_TRUE(has_ns_id(markers, std::string("ns2_stop") + wall_ns_suffix, 0));
@@ -107,8 +113,10 @@ TEST(VirtualWallMarkerCreator, manyWalls)
   EXPECT_TRUE(has_ns_id(markers, std::string("ns2_dead_line") + text_ns_suffix, 0));
   EXPECT_TRUE(has_ns_id(markers, std::string("ns3_dead_line") + wall_ns_suffix, 0));
   EXPECT_TRUE(has_ns_id(markers, std::string("ns3_dead_line") + text_ns_suffix, 0));
+  EXPECT_TRUE(has_ns_id(markers, std::string("ns1_intended_pass") + "_direction", 0));
+  EXPECT_TRUE(has_ns_id(markers, std::string("ns1_intended_pass") + text_ns_suffix, 0));
   markers = creator.create_markers();
-  ASSERT_EQ(markers.markers.size(), 12UL * 2);
+  ASSERT_EQ(markers.markers.size(), 13UL * 2);
   for (const auto & marker : markers.markers)
     EXPECT_EQ(marker.action, visualization_msgs::msg::Marker::DELETE);
   markers = creator.create_markers();