From b0e52bdb6fa568a2ed6df09cb573ae12b7c14cf8 Mon Sep 17 00:00:00 2001 From: Orange <2314753575@qq.com> Date: Wed, 5 Aug 2026 22:17:53 +0800 Subject: [PATCH] 111 --- .../obstacle_nav2/obstacle_array_layer.hpp | 6 ++ .../obstacle_nav2.launch.cpython-310.pyc | Bin 0 -> 3048 bytes .../launch/trajectory_guard.launch.py | 58 ++++++++++++++++++ .../src/obstacle_array_layer.cpp | 39 ++++++++---- .../test/test_obstacle_array_layer.cpp | 20 ++++++ 5 files changed, 112 insertions(+), 11 deletions(-) create mode 100644 src/navigation/obstacle_nav2/launch/__pycache__/obstacle_nav2.launch.cpython-310.pyc create mode 100644 src/navigation/obstacle_nav2/launch/trajectory_guard.launch.py 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 0000000000000000000000000000000000000000..c8ee62d94d70fe62a46bf133552324c62fbafd0f GIT binary patch literal 3048 zcmbtWOOM+|5*8n#sOL!Yvg5HGChKIZ#2zKiWwF>Sc3{sKMka|dGYJq~2#Vd(&?ebL zcXMnJ@?M;)|3MDPasSL-7wGE(3+zwW#U|dWCaqD%xvdBlzkXJA)mPPI-EOM}zZ(tl zQ`xRtcWZH=9{lXS;jjT><;n@Q!LLOx@V(HLUTMuX$ z!_0DQpU?7yrc$*Za+YWNA2 zmXb)TG1QI6b9u(nw=6heIS=X3R2$_GteHl$oCjKQzg%_jJmXXdG}urss63`gkOA%< z(`P)AlBVAUNgSdd-uyNJ^x%IV{^D;&ttKH>FO3)YHsNhZgIL5S^;hN#6R<-X8e4$f zs^13O1YhHvI^Z_xXzT#)R=5GUS78_MT7{d^d}94*FKVPu20vLZS}W~!Xt$T`4YCRC zj;t?Qq_waX9kTU!T=dAhugt~T3{TmwR(N{Fd({fJS}~|b-`AtRt45pE=sJ!rP}40OS(pnv^9Mgz zKX!hqkq<%RhV06o_TnzN{mKH2UK~~!z4%du(Tg8f7`=F>!notz3ZobIDvVwnX}m^0 zS+vQgaH9TVuvlMg%(^d;_4kVRza}i9lF#q5G@g5GB4iN8)Juc&Xr}qIr@C)?edxWX zIw<`KkJ4G5sZK5+O`^n?Q9@OH8pHyb+aPor`!?B<2D0zs;AdT*go`u zgUSWr&w+fZrn)pm^7{$X6thP}MGR9Fc&l?Xs~nP=rC6Qm&j(r$Euy zwq^k+7kj9o*qjDJ5+eMZWImpG?>Xr0PUsn&m~mfnN{e@3nJ<~2F*qEQ0?HHE7iSJl zz5&Ugu2ttlmn>&aqz_Y0p&+z{PDy_n#ZMrPoM%)N zYrCT`A^tgycfDxp?G|@;z0W`QcF_mB9tF>O%NagYCi1MTP69nfc2$q@Xcnbmz_qK2 z#wgass<=K{87Tv|R}SrESk#(eM_FZ5qp|?1i6wZbEFz@p#X|erCp-dsi~cAU>8GFg z%RN;8atMl@(dDiSmBrQw{8ka{Vwa)C`{>+u8L-lO%^;UdhZzj*mm2+`DzT!t@eOzd z_?O3;t={d2K|v>M=!G#0pTa4+Zz6Rb#6YDsxv1R5&4-$to+hU^W!#%EdFF+n4=5>$ z-yc&!`8g$j@Jd>GU3%yLP+uOOGa%H=poo>7m}Aoh^!(2+%9@!1KoUk~ZtG6;9PgD(#6ctJ`$ z&cJN%^w7)lNgRnY%0;n#u}Fu`t3r2cKNbv+QSEp+NM6baRW}d!Pfm|dAD{RS4i5Lf zIXd+pAAf!L=;+|YM+t7VQ_SbOcIvRt@kZx(=_`{7W#gHZ{e(p+@8U=sg@dAr!bMR> z(E*|Ak)(;>7IxSu8X$(7s*ydN;pMB!G_Fbx_Gq5yOypa*U>n6%6xUGfptue~tpU&O z!F3J=03;Z-%>Rrt3`E(xaVd@U)InK9a!yIPgHY=j0KbYCKt6w z$z=+a3|3bzJHAQ-HFzsC{#xUB0srS@bOyD0B=VF;f{x}<90kmOCPt8_EBSPhcX;-y z1pK=5A`|g*2MT5&X{Fy^$&Xv0Rs0i#Yq>_p>KLm}-|QO>JoanTG!1w_+P|2#^KY|h zL)-bqbU+$_Uz@KT%lWVAw0^tay)+NnF0Qh0U;Aep_iSq&x^3DewtxP=P+jj17MHs@ zf9D5q)ZsS&3|?vn=$%SGN(p`LXZpKqulgz!n(fLdBdKW%6;fB)y+D8LiIT>e;hObG zXf>$jEQ_tsiJ;!A`peg90Q~YA5jtzsW;IXba7#xr#|u*Eq|^DO)~gLT6H7aEey!^y x2w9S_vrG6ZjeEZeZ}Q8Xc!&^h+XG{{xQysy6@t literal 0 HcmV?d00001 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);