111

src/navigation/obstacle_nav2/include/obstacle_nav2/obstacle_array_layer.hpp

@@ -5,6 +5,7 @@
 #include <cstdint>
 #include <memory>
 #include <mutex>
+#include <optional>
 #include <string>
 #include <thread>
 #include <vector>
@@ -67,6 +68,10 @@ public:
     nav2_costmap_2d::Costmap2D & grid,
     const std::vector<CircleObstacle> & obstacles,
     double resolution, double origin_x, double origin_y);
+  static std::optional<SnapshotBounds> applySnapshotIfNotEmpty(
+    nav2_costmap_2d::Costmap2D & grid,
+    const std::vector<CircleObstacle> & obstacles,
+    double resolution, double origin_x, double origin_y);
 
 private:
   void obstacleCallback(const obstacle_scanner::msg::ObstacleArray::SharedPtr msg);
@@ -95,6 +100,7 @@ private:
   std::string global_frame_;
   double obstacle_timeout_{0.5};
   double transform_tolerance_{0.2};
+  bool retain_previous_on_empty_snapshot_{true};
   double default_obstacle_radius_{0.05};
   double minimum_obstacle_radius_{0.02};
   double maximum_obstacle_radius_{0.50};

src/navigation/obstacle_nav2/launch/trajectory_guard.launch.py

@@ -0,0 +1,58 @@
+#!/usr/bin/env python3
+"""Launch obstacle_nav2 with the C++ trajectory guard node."""
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    pkg_dir = get_package_share_directory("obstacle_nav2")
+    base_launch = os.path.join(pkg_dir, "launch", "obstacle_nav2.launch.py")
+    guard_params = LaunchConfiguration("guard_params")
+
+    obstacle_nav2 = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(base_launch),
+        launch_arguments={
+            "use_sim_time": LaunchConfiguration("use_sim_time"),
+            "global_frame": LaunchConfiguration("global_frame"),
+            "use_static_map": LaunchConfiguration("use_static_map"),
+            "map_yaml": LaunchConfiguration("map_yaml"),
+            "enable_motion": LaunchConfiguration("enable_motion"),
+            "start_base": LaunchConfiguration("start_base"),
+            "start_lidar": LaunchConfiguration("start_lidar"),
+            "start_obstacle_scanner": LaunchConfiguration("start_obstacle_scanner"),
+        }.items(),
+    )
+
+    trajectory_guard = Node(
+        package="obstacle_nav2",
+        executable="trajectory_guard_node",
+        name="trajectory_guard_node",
+        output="screen",
+        parameters=[guard_params],
+    )
+
+    return LaunchDescription(
+        [
+            DeclareLaunchArgument("use_sim_time", default_value="false"),
+            DeclareLaunchArgument("global_frame", default_value="odom"),
+            DeclareLaunchArgument("use_static_map", default_value="false"),
+            DeclareLaunchArgument("map_yaml", default_value=""),
+            DeclareLaunchArgument("enable_motion", default_value="true"),
+            DeclareLaunchArgument("start_base", default_value="true"),
+            DeclareLaunchArgument("start_lidar", default_value="true"),
+            DeclareLaunchArgument("start_obstacle_scanner", default_value="true"),
+            DeclareLaunchArgument(
+                "guard_params",
+                default_value=os.path.join(pkg_dir, "config", "trajectory_guard.yaml"),
+            ),
+            obstacle_nav2,
+            trajectory_guard,
+        ]
+    )

src/navigation/obstacle_nav2/src/obstacle_array_layer.cpp

@@ -3,6 +3,7 @@
 #include <algorithm>
 #include <chrono>
 #include <cmath>
+#include <optional>
 #include <string>
 #include <vector>
 
@@ -58,6 +59,7 @@ void ObstacleArrayLayer::onInitialize()
   node->declare_parameter(name_ + ".minimum_obstacle_radius", 0.02);
   node->declare_parameter(name_ + ".maximum_obstacle_radius", 0.50);
   node->declare_parameter(name_ + ".extra_inflation", 0.02);
+  node->declare_parameter(name_ + ".retain_previous_on_empty_snapshot", true);
 
   node->get_parameter(name_ + ".enabled", enabled_);
   node->get_parameter(name_ + ".topic", topic_);
@@ -67,6 +69,8 @@ void ObstacleArrayLayer::onInitialize()
   node->get_parameter(name_ + ".minimum_obstacle_radius", minimum_obstacle_radius_);
   node->get_parameter(name_ + ".maximum_obstacle_radius", maximum_obstacle_radius_);
   node->get_parameter(name_ + ".extra_inflation", extra_inflation_);
+  node->get_parameter(
+    name_ + ".retain_previous_on_empty_snapshot", retain_previous_on_empty_snapshot_);
 
   global_frame_ = layered_costmap_->getGlobalFrameID();
 
@@ -290,6 +294,17 @@ ObstacleArrayLayer::SnapshotBounds ObstacleArrayLayer::applySnapshot(
   return bounds;
 }
 
+std::optional<ObstacleArrayLayer::SnapshotBounds> ObstacleArrayLayer::applySnapshotIfNotEmpty(
+  nav2_costmap_2d::Costmap2D & grid,
+  const std::vector<CircleObstacle> & obstacles,
+  double resolution, double origin_x, double origin_y)
+{
+  if (obstacles.empty()) {
+    return std::nullopt;
+  }
+  return applySnapshot(grid, obstacles, resolution, origin_x, origin_y);
+}
+
 ObstacleArrayLayer::SnapshotBounds ObstacleArrayLayer::mergeBounds(
   const SnapshotBounds & first, const SnapshotBounds & second)
 {
@@ -378,7 +393,10 @@ void ObstacleArrayLayer::obstacleCallback(
     valid_obstacles.push_back({obs.center_x, obs.center_y, effective_r});
   }
 
-  {
+  if (valid_obstacles.empty() && retain_previous_on_empty_snapshot_) {
+    return;
+  }
+
   std::lock_guard<std::mutex> lock(data_mutex_);
   const SnapshotBounds previous_bounds = current_bounds_;
   current_bounds_ = applySnapshot(
@@ -389,7 +407,6 @@ void ObstacleArrayLayer::obstacleCallback(
   }
   has_received_obstacles_ = current_bounds_.valid;
   last_obstacle_time_ = node->now();
-  }
 }
 
 }  // namespace obstacle_nav2

src/navigation/obstacle_nav2/test/test_obstacle_array_layer.cpp

@@ -186,6 +186,26 @@ TEST(ObstacleArrayLayerTest, EmptySnapshotClearsPreviousObstacle)
   EXPECT_EQ(grid.getCost(mx, my), nav2_costmap_2d::FREE_SPACE);
 }
 
+TEST(ObstacleArrayLayerTest, EmptySnapshotCanBeIgnoredToPreservePreviousObstacle)
+{
+  nav2_costmap_2d::Costmap2D grid(40, 40, kResolution, kOriginX, kOriginY);
+  const std::vector<ObstacleArrayLayer::CircleObstacle> occupied{{0.0, 0.0, 0.1}};
+
+  const auto occupied_bounds = ObstacleArrayLayer::applySnapshot(
+    grid, occupied, kResolution, kOriginX, kOriginY);
+
+  unsigned int mx, my;
+  ASSERT_TRUE(grid.worldToMap(0.0, 0.0, mx, my));
+  EXPECT_TRUE(occupied_bounds.valid);
+  EXPECT_EQ(grid.getCost(mx, my), nav2_costmap_2d::LETHAL_OBSTACLE);
+
+  const auto retained = ObstacleArrayLayer::applySnapshotIfNotEmpty(
+    grid, {}, kResolution, kOriginX, kOriginY);
+
+  EXPECT_FALSE(retained.has_value());
+  EXPECT_EQ(grid.getCost(mx, my), nav2_costmap_2d::LETHAL_OBSTACLE);
+}
+
 TEST(ObstacleArrayLayerTest, TransformSnapshotUsesMessageTimestamp)
 {
   auto clock = std::make_shared<rclcpp::Clock>(RCL_SYSTEM_TIME);