Add test case for already reached

at-wat · Sep 14, 2023 · a57cb4d · a57cb4d
1 parent 8e8dee3
commit a57cb4d
Show file tree

Hide file tree

Showing 3 changed files with 26 additions and 7 deletions.
diff --git a/planner_cspace/src/planner_3d.cpp b/planner_cspace/src/planner_3d.cpp
@@ -1699,6 +1699,9 @@ class Planner3dNode
       Astar::Vec expected_start_grid;
       if (start_pose_predictor_.process(start_metric, cm_, map_info_, previous_path_, expected_start_grid))
       {
+        ROS_DEBUG("Start grid is moved to (%d, %d, %d) from (%d, %d, %d) by start pose predictor.",
+                  expected_start_grid[0], expected_start_grid[1], expected_start_grid[2],
+                  start_grid[0], start_grid[1], start_grid[2]);
         result_start_poses.push_back(Astar::VecWithCost(expected_start_grid));
         return isPathFinishing(start_grid, end_grid) ? StartPoseStatus::FINISHING : StartPoseStatus::NORMAL;
       }
@@ -1739,12 +1742,15 @@ class Planner3dNode
     }
     if (result_start_poses.empty())
     {
+      const Astar::Vec original_start_grid = start_grid;
       if (!searchAvailablePos(cm_, start_grid, tolerance_range_, tolerance_angle_))
       {
         ROS_WARN("Oops! You are in Rock!");
         return StartPoseStatus::START_OCCUPIED;
       }
-      ROS_INFO("Start moved");
+      ROS_INFO("Start grid is moved to (%d, %d, %d) from (%d, %d, %d) by relocation.",
+               start_grid[0], start_grid[1], start_grid[2],
+               original_start_grid[0], original_start_grid[1], original_start_grid[2]);
       result_start_poses.push_back(Astar::VecWithCost(start_grid));
     }
     for (const Astar::VecWithCost& s : result_start_poses)
@@ -1868,8 +1874,10 @@ class Planner3dNode
     {
       if (s.v_ == end_grid)
       {
-        ROS_DEBUG("Start is same as end.");
+        ROS_DEBUG("The start grid is the same as the end grid. Path planning skipped.");
         path_grid.push_back(end_grid);
+        is_goal_same_as_start = true;
+        break;
       }
     }
     if (!is_goal_same_as_start && !as_.search(starts, end_grid, path_grid,

diff --git a/planner_cspace/test/src/test_dynamic_parameter_change.cpp b/planner_cspace/test/src/test_dynamic_parameter_change.cpp
@@ -121,7 +121,7 @@ class DynamicParameterChangeTest
     return false;
   }
 
-  ::testing::AssertionResult comparePath2(const nav_msgs::Path& path1, const nav_msgs::Path& path2)
+  ::testing::AssertionResult comparePath(const nav_msgs::Path& path1, const nav_msgs::Path& path2)
   {
     if (path1.poses.size() != path2.poses.size())
     {
@@ -232,7 +232,7 @@ TEST_F(DynamicParameterChangeTest, StartPosePrediction)
   publishMapAndRobot(1.65, 0.65, M_PI);
   ros::Duration(0.5).sleep();
   sendGoalAndWaitForPath();
-  EXPECT_FALSE(comparePath2(initial_path, *path_));
+  EXPECT_FALSE(comparePath(initial_path, *path_));
 
   // Enable start pose prediction.
   move_base_->cancelAllGoals();
@@ -246,15 +246,26 @@ TEST_F(DynamicParameterChangeTest, StartPosePrediction)
   publishMapAndRobot(1.65, 0.65, M_PI);
   ros::Duration(0.5).sleep();
   sendGoalAndWaitForPath();
-  EXPECT_TRUE(comparePath2(initial_path, *path_));
+  EXPECT_TRUE(comparePath(initial_path, *path_));
 
   // The path does not change as a part of the previous path is kept and it is not possible to keep distance from
   // the obstacle.
   map_overlay_.data[13 + 5 * map_overlay_.info.width] = 100;
   publishMapAndRobot(1.65, 0.65, M_PI);
   ros::Duration(0.5).sleep();
   sendGoalAndWaitForPath();
-  EXPECT_TRUE(comparePath2(initial_path, *path_));
+  EXPECT_TRUE(comparePath(initial_path, *path_));
+
+  // It is expected that the robot reaches the goal during the path planning.
+  move_base_->cancelAllGoals();
+  map_overlay_.data[13 + 5 * map_overlay_.info.width] = 0;
+  publishMapAndRobot(1.25, 0.95, M_PI / 2);
+  ros::Duration(0.5).sleep();
+  sendGoalAndWaitForPath();
+  const nav_msgs::Path short_path = *path_;
+  // In the second path planning after cancel, the exptected start pose is same as the goal.
+  sendGoalAndWaitForPath();
+  EXPECT_TRUE(comparePath(short_path, *path_));
 }
 
 int main(int argc, char** argv)

diff --git a/planner_cspace/test/test/dynamic_parameter_change_rostest.test b/planner_cspace/test/test/dynamic_parameter_change_rostest.test
@@ -11,7 +11,7 @@
     <param name="linear_expand" value="0.0"/>
     <param name="linear_spread" value="0.3"/>
   </node>
-  <node pkg="planner_cspace" type="planner_3d" name="planner_3d" output="screen">
+  <node pkg="planner_cspace" type="planner_3d" name="planner_3d">
     <remap from="move_base_simple/goal" to="/goal" />
   </node>
   <node pkg="map_server" type="map_server" name="map_server_global" args="$(find planner_cspace)/test/data/global_map.yaml" />