diff --git a/src/navigation/obstacle_nav2/include/obstacle_nav2/obstacle_array_layer.hpp b/src/navigation/obstacle_nav2/include/obstacle_nav2/obstacle_array_layer.hpp index ab5cb48..5bc6890 100755 --- a/src/navigation/obstacle_nav2/include/obstacle_nav2/obstacle_array_layer.hpp +++ b/src/navigation/obstacle_nav2/include/obstacle_nav2/obstacle_array_layer.hpp @@ -5,6 +5,7 @@ #include #include #include +#include #include #include #include @@ -67,6 +68,10 @@ public: nav2_costmap_2d::Costmap2D & grid, const std::vector & obstacles, double resolution, double origin_x, double origin_y); + static std::optional applySnapshotIfNotEmpty( + nav2_costmap_2d::Costmap2D & grid, + const std::vector & 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}; diff --git a/src/navigation/obstacle_nav2/launch/__pycache__/obstacle_nav2.launch.cpython-310.pyc b/src/navigation/obstacle_nav2/launch/__pycache__/obstacle_nav2.launch.cpython-310.pyc new file mode 100644 index 0000000..c8ee62d Binary files /dev/null and b/src/navigation/obstacle_nav2/launch/__pycache__/obstacle_nav2.launch.cpython-310.pyc differ diff --git a/src/navigation/obstacle_nav2/launch/trajectory_guard.launch.py b/src/navigation/obstacle_nav2/launch/trajectory_guard.launch.py new file mode 100644 index 0000000..9a0791f --- /dev/null +++ b/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, + ] + ) diff --git a/src/navigation/obstacle_nav2/src/obstacle_array_layer.cpp b/src/navigation/obstacle_nav2/src/obstacle_array_layer.cpp index 5f959c1..f5add7d 100755 --- a/src/navigation/obstacle_nav2/src/obstacle_array_layer.cpp +++ b/src/navigation/obstacle_nav2/src/obstacle_array_layer.cpp @@ -3,6 +3,7 @@ #include #include #include +#include #include #include @@ -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::applySnapshotIfNotEmpty( + nav2_costmap_2d::Costmap2D & grid, + const std::vector & 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,18 +393,20 @@ void ObstacleArrayLayer::obstacleCallback( valid_obstacles.push_back({obs.center_x, obs.center_y, effective_r}); } - { - std::lock_guard lock(data_mutex_); - const SnapshotBounds previous_bounds = current_bounds_; - current_bounds_ = applySnapshot( - *this, valid_obstacles, getResolution(), origin_x, origin_y); - pending_clear_bounds_ = mergeBounds(pending_clear_bounds_, previous_bounds); - if (previous_bounds.valid || current_bounds_.valid) { - ++bounds_generation_; - } - has_received_obstacles_ = current_bounds_.valid; - last_obstacle_time_ = node->now(); + if (valid_obstacles.empty() && retain_previous_on_empty_snapshot_) { + return; } + + std::lock_guard lock(data_mutex_); + const SnapshotBounds previous_bounds = current_bounds_; + current_bounds_ = applySnapshot( + *this, valid_obstacles, getResolution(), origin_x, origin_y); + pending_clear_bounds_ = mergeBounds(pending_clear_bounds_, previous_bounds); + if (previous_bounds.valid || current_bounds_.valid) { + ++bounds_generation_; + } + has_received_obstacles_ = current_bounds_.valid; + last_obstacle_time_ = node->now(); } } // namespace obstacle_nav2 diff --git a/src/navigation/obstacle_nav2/test/test_obstacle_array_layer.cpp b/src/navigation/obstacle_nav2/test/test_obstacle_array_layer.cpp index 7b34e03..9a60b11 100755 --- a/src/navigation/obstacle_nav2/test/test_obstacle_array_layer.cpp +++ b/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 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(RCL_SYSTEM_TIME);