给轨迹+MPPI版导航加入了障碍物检测

src/navigation/obstacle_nav2/test/test_trajectory_guard.cpp

@@ -0,0 +1,162 @@
+#include <gtest/gtest.h>
+
+#include "obstacle_nav2/trajectory_guard.hpp"
+
+namespace
+{
+
+geometry_msgs::msg::PoseStamped pose(double x, double y)
+{
+  geometry_msgs::msg::PoseStamped p;
+  p.header.frame_id = "odom";
+  p.pose.position.x = x;
+  p.pose.position.y = y;
+  p.pose.orientation.w = 1.0;
+  return p;
+}
+
+nav_msgs::msg::Path straightPath()
+{
+  nav_msgs::msg::Path path;
+  path.header.frame_id = "odom";
+  for (int i = 0; i <= 10; ++i) {
+    path.poses.push_back(pose(0.2 * i, 0.0));
+  }
+  return path;
+}
+
+nav_msgs::msg::OccupancyGrid gridWithOrigin(double origin_x, double origin_y)
+{
+  nav_msgs::msg::OccupancyGrid grid;
+  grid.header.frame_id = "odom";
+  grid.info.resolution = 0.1;
+  grid.info.width = 40;
+  grid.info.height = 30;
+  grid.info.origin.position.x = origin_x;
+  grid.info.origin.position.y = origin_y;
+  grid.data.assign(grid.info.width * grid.info.height, 0);
+  return grid;
+}
+
+void markOccupied(nav_msgs::msg::OccupancyGrid & grid, int mx, int my)
+{
+  ASSERT_GE(mx, 0);
+  ASSERT_GE(my, 0);
+  ASSERT_LT(mx, static_cast<int>(grid.info.width));
+  ASSERT_LT(my, static_cast<int>(grid.info.height));
+  grid.data[my * grid.info.width + mx] = 100;
+}
+
+}  // namespace
+
+TEST(TrajectoryGuard, NearestPathIndexFindsClosestPose)
+{
+  const auto path = straightPath();
+  EXPECT_EQ(obstacle_nav2::nearestPathIndex(path, pose(0.43, 0.02)), 2u);
+}
+
+TEST(TrajectoryGuard, AdvanceByDistanceStopsAtRequestedArcLength)
+{
+  const auto path = straightPath();
+  EXPECT_EQ(obstacle_nav2::advanceByDistance(path, 0, 0.55), 3u);
+}
+
+TEST(TrajectoryGuard, SlicePathIncludesEndIndex)
+{
+  const auto path = straightPath();
+  const auto sliced = obstacle_nav2::slicePath(path, 2, 4);
+  ASSERT_EQ(sliced.poses.size(), 3u);
+  EXPECT_DOUBLE_EQ(sliced.poses.front().pose.position.x, 0.4);
+  EXPECT_DOUBLE_EQ(sliced.poses.back().pose.position.x, 0.8);
+}
+
+TEST(TrajectoryGuard, StitchPathsAppendsOriginalFromRejoinIndex)
+{
+  const auto original = straightPath();
+  nav_msgs::msg::Path bypass;
+  bypass.header.frame_id = "odom";
+  bypass.poses.push_back(pose(0.0, 0.0));
+  bypass.poses.push_back(pose(0.5, 0.3));
+
+  const auto stitched = obstacle_nav2::stitchPaths(bypass, original, 4);
+  ASSERT_EQ(stitched.poses.size(), 9u);
+  EXPECT_DOUBLE_EQ(stitched.poses[0].pose.position.y, 0.0);
+  EXPECT_DOUBLE_EQ(stitched.poses[1].pose.position.y, 0.3);
+  EXPECT_DOUBLE_EQ(stitched.poses[2].pose.position.x, 0.8);
+}
+
+TEST(TrajectoryGuard, CheckPathAheadReportsOccupiedCell)
+{
+  auto path = straightPath();
+  auto grid = gridWithOrigin(-1.0, -1.0);
+  markOccupied(grid, 15, 10);
+
+  obstacle_nav2::GuardSettings settings;
+  settings.lookahead_distance = 2.0;
+  settings.occupied_threshold = 50;
+  settings.footprint_half_length = 0.01;
+  settings.footprint_half_width = 0.01;
+  settings.footprint_padding = 0.0;
+
+  const auto result = obstacle_nav2::checkPathAhead(path, 0, grid, settings);
+  EXPECT_TRUE(result.blocked);
+  EXPECT_EQ(result.reason, "occupied");
+}
+
+TEST(TrajectoryGuard, CheckPathAheadKeepsClearPathUnblocked)
+{
+  auto path = straightPath();
+  auto grid = gridWithOrigin(-1.0, -1.0);
+
+  obstacle_nav2::GuardSettings settings;
+  settings.lookahead_distance = 2.0;
+  settings.occupied_threshold = 50;
+  settings.footprint_half_length = 0.01;
+  settings.footprint_half_width = 0.01;
+  settings.footprint_padding = 0.0;
+
+  const auto result = obstacle_nav2::checkPathAhead(path, 0, grid, settings);
+  EXPECT_FALSE(result.blocked);
+  EXPECT_EQ(result.reason, "clear");
+}
+
+TEST(TrajectoryGuard, FindClearRejoinIndexSkipsBlockedArea)
+{
+  auto path = straightPath();
+  auto grid = gridWithOrigin(-1.0, -1.0);
+  for (int mx = 13; mx <= 18; ++mx) {
+    markOccupied(grid, mx, 10);
+  }
+
+  obstacle_nav2::GuardSettings settings;
+  settings.rejoin_min_distance = 0.5;
+  settings.rejoin_max_distance = 2.0;
+  settings.occupied_threshold = 50;
+  settings.footprint_half_length = 0.01;
+  settings.footprint_half_width = 0.01;
+  settings.footprint_padding = 0.0;
+
+  const auto rejoin = obstacle_nav2::findClearRejoinIndex(path, 0, grid, settings);
+  ASSERT_TRUE(rejoin.has_value());
+  EXPECT_GE(*rejoin, 5u);
+}
+
+TEST(TrajectoryGuard, RetryBlockedRepairWaitsForCostmapUpdateAndRetryDelay)
+{
+  const std::optional<std::size_t> last_repair_index = 10u;
+
+  EXPECT_FALSE(obstacle_nav2::shouldRetryBlockedRepair(
+    last_repair_index, 11u, 5, 0.8, 1.0, true));
+  EXPECT_FALSE(obstacle_nav2::shouldRetryBlockedRepair(
+    last_repair_index, 11u, 5, 1.2, 1.0, false));
+  EXPECT_TRUE(obstacle_nav2::shouldRetryBlockedRepair(
+    last_repair_index, 11u, 5, 1.2, 1.0, true));
+}
+
+TEST(TrajectoryGuard, RetryBlockedRepairAllowsProgressedPathImmediately)
+{
+  const std::optional<std::size_t> last_repair_index = 10u;
+
+  EXPECT_TRUE(obstacle_nav2::shouldRetryBlockedRepair(
+    last_repair_index, 16u, 5, 0.1, 1.0, false));
+}