From 0a8307c020e7f1ad678f424820851b5fb9067fa3 Mon Sep 17 00:00:00 2001 From: Leander Stephen D'Souza Date: Thu, 15 May 2025 23:12:40 +0100 Subject: [PATCH 01/46] Add speed filter zones to depot and warehouse maps. (#5146) * Added keepout prefix to keepout zone related params. Signed-off-by: Leander Stephen D'Souza * Add namespace support for speed filter zones. Signed-off-by: Leander Stephen D'Souza * Added speed filter zones to depot and warehouse maps. Signed-off-by: Leander Stephen D'Souza * Added dedicated launch file for speed zone support. Signed-off-by: Leander Stephen D'Souza * Updated rviz2 config to include speed zone filter. Signed-off-by: Leander Stephen D'Souza * Preserve initial nature of costmap filters tests. Signed-off-by: Leander Stephen D'Souza * Enable namespace for the speed limit topic. Signed-off-by: Leander Stephen D'Souza * Update speed filter zones in depot and warehouse maps Signed-off-by: Leander Stephen D'Souza * Readjust speed filter zones in depot and warehouse maps. Signed-off-by: Leander Stephen D'Souza * Enable namespace support for speed limit topic. Signed-off-by: Leander Stephen D'Souza * Reduced central speed zone in warehouse map. Signed-off-by: Leander Stephen D'Souza --------- Signed-off-by: Leander Stephen D'Souza --- nav2_bringup/launch/bringup_launch.py | 30 +++ nav2_bringup/launch/keepout_zone_launch.py | 10 +- nav2_bringup/launch/speed_zone_launch.py | 219 ++++++++++++++++++ nav2_bringup/launch/tb4_simulation_launch.py | 17 ++ nav2_bringup/maps/depot_speed.pgm | Bin 0 -> 185487 bytes nav2_bringup/maps/depot_speed.yaml | 7 + nav2_bringup/maps/warehouse_speed.pgm | Bin 0 -> 1684105 bytes nav2_bringup/maps/warehouse_speed.yaml | 7 + nav2_bringup/params/nav2_params.yaml | 31 ++- nav2_bringup/rviz/nav2_default_view.rviz | 23 ++ .../plugins/costmap_filters/speed_filter.cpp | 5 +- .../costmap_filters/test_keepout_launch.py | 1 + .../src/costmap_filters/test_speed_launch.py | 2 + 13 files changed, 339 insertions(+), 13 deletions(-) create mode 100644 nav2_bringup/launch/speed_zone_launch.py create mode 100644 nav2_bringup/maps/depot_speed.pgm create mode 100644 nav2_bringup/maps/depot_speed.yaml create mode 100644 nav2_bringup/maps/warehouse_speed.pgm create mode 100644 nav2_bringup/maps/warehouse_speed.yaml diff --git a/nav2_bringup/launch/bringup_launch.py b/nav2_bringup/launch/bringup_launch.py index 858e6a9784f..2725a0244dc 100644 --- a/nav2_bringup/launch/bringup_launch.py +++ b/nav2_bringup/launch/bringup_launch.py @@ -36,6 +36,7 @@ def generate_launch_description() -> LaunchDescription: slam = LaunchConfiguration('slam') map_yaml_file = LaunchConfiguration('map') keepout_mask_yaml_file = LaunchConfiguration('keepout_mask') + speed_mask_yaml_file = LaunchConfiguration('speed_mask') graph_filepath = LaunchConfiguration('graph') use_sim_time = LaunchConfiguration('use_sim_time') params_file = LaunchConfiguration('params_file') @@ -45,6 +46,7 @@ def generate_launch_description() -> LaunchDescription: log_level = LaunchConfiguration('log_level') use_localization = LaunchConfiguration('use_localization') use_keepout_zones = LaunchConfiguration('use_keepout_zones') + use_speed_zones = LaunchConfiguration('use_speed_zones') # Map fully qualified names to relative ones so the node's namespace can be prepended. remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] @@ -80,6 +82,11 @@ def generate_launch_description() -> LaunchDescription: description='Full path to keepout mask yaml file to load' ) + declare_speed_mask_yaml_cmd = DeclareLaunchArgument( + 'speed_mask', default_value='', + description='Full path to speed mask yaml file to load' + ) + declare_graph_file_cmd = DeclareLaunchArgument( 'graph', default_value='', description='Path to the graph file to load' @@ -95,6 +102,11 @@ def generate_launch_description() -> LaunchDescription: description='Whether to enable keepout zones or not' ) + declare_use_speed_zones_cmd = DeclareLaunchArgument( + 'use_speed_zones', default_value='True', + description='Whether to enable speed zones or not' + ) + declare_use_sim_time_cmd = DeclareLaunchArgument( 'use_sim_time', default_value='false', @@ -189,6 +201,22 @@ def generate_launch_description() -> LaunchDescription: }.items(), ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(launch_dir, 'speed_zone_launch.py') + ), + condition=IfCondition(PythonExpression([use_speed_zones])), + launch_arguments={ + 'namespace': namespace, + 'speed_mask': speed_mask_yaml_file, + 'use_sim_time': use_sim_time, + 'params_file': params_file, + 'use_composition': use_composition, + 'use_respawn': use_respawn, + 'container_name': 'nav2_container', + }.items(), + ), + IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join(launch_dir, 'navigation_launch.py') @@ -218,6 +246,7 @@ def generate_launch_description() -> LaunchDescription: ld.add_action(declare_slam_cmd) ld.add_action(declare_map_yaml_cmd) ld.add_action(declare_keepout_mask_yaml_cmd) + ld.add_action(declare_speed_mask_yaml_cmd) ld.add_action(declare_graph_file_cmd) ld.add_action(declare_use_sim_time_cmd) ld.add_action(declare_params_file_cmd) @@ -227,6 +256,7 @@ def generate_launch_description() -> LaunchDescription: ld.add_action(declare_log_level_cmd) ld.add_action(declare_use_localization_cmd) ld.add_action(declare_use_keepout_zones_cmd) + ld.add_action(declare_use_speed_zones_cmd) # Add the actions to launch all of the navigation nodes ld.add_action(bringup_cmd_group) diff --git a/nav2_bringup/launch/keepout_zone_launch.py b/nav2_bringup/launch/keepout_zone_launch.py index 58d5a5987bf..502b6de4548 100644 --- a/nav2_bringup/launch/keepout_zone_launch.py +++ b/nav2_bringup/launch/keepout_zone_launch.py @@ -40,7 +40,7 @@ def generate_launch_description() -> LaunchDescription: use_keepout_zones = LaunchConfiguration('use_keepout_zones') log_level = LaunchConfiguration('log_level') - lifecycle_nodes = ['filter_mask_server', 'costmap_filter_info_server'] + lifecycle_nodes = ['keepout_filter_mask_server', 'keepout_costmap_filter_info_server'] # Map fully qualified names to relative ones so the node's namespace can be prepended. remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] @@ -116,7 +116,7 @@ def generate_launch_description() -> LaunchDescription: condition=IfCondition(use_keepout_zones), package='nav2_map_server', executable='map_server', - name='filter_mask_server', + name='keepout_filter_mask_server', output='screen', respawn=use_respawn, respawn_delay=2.0, @@ -128,7 +128,7 @@ def generate_launch_description() -> LaunchDescription: condition=IfCondition(use_keepout_zones), package='nav2_map_server', executable='costmap_filter_info_server', - name='costmap_filter_info_server', + name='keepout_costmap_filter_info_server', output='screen', respawn=use_respawn, respawn_delay=2.0, @@ -162,7 +162,7 @@ def generate_launch_description() -> LaunchDescription: ComposableNode( package='nav2_map_server', plugin='nav2_map_server::MapServer', - name='filter_mask_server', + name='keepout_filter_mask_server', parameters=[ configured_params, {'yaml_filename': keepout_mask_yaml_file} @@ -172,7 +172,7 @@ def generate_launch_description() -> LaunchDescription: ComposableNode( package='nav2_map_server', plugin='nav2_map_server::CostmapFilterInfoServer', - name='costmap_filter_info_server', + name='keepout_costmap_filter_info_server', parameters=[configured_params], remappings=remappings, ), diff --git a/nav2_bringup/launch/speed_zone_launch.py b/nav2_bringup/launch/speed_zone_launch.py new file mode 100644 index 00000000000..240cc55a3b1 --- /dev/null +++ b/nav2_bringup/launch/speed_zone_launch.py @@ -0,0 +1,219 @@ +# Copyright (c) 2025 Leander Stephen Desouza +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration, PythonExpression +from launch_ros.actions import LoadComposableNodes, Node, SetParameter +from launch_ros.descriptions import ComposableNode, ParameterFile +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description() -> LaunchDescription: + # Get the launch directory + bringup_dir = get_package_share_directory('nav2_bringup') + + namespace = LaunchConfiguration('namespace') + speed_mask_yaml_file = LaunchConfiguration('speed_mask') + use_sim_time = LaunchConfiguration('use_sim_time') + autostart = LaunchConfiguration('autostart') + params_file = LaunchConfiguration('params_file') + use_composition = LaunchConfiguration('use_composition') + container_name = LaunchConfiguration('container_name') + container_name_full = (namespace, '/', container_name) + use_respawn = LaunchConfiguration('use_respawn') + use_speed_zones = LaunchConfiguration('use_speed_zones') + log_level = LaunchConfiguration('log_level') + + lifecycle_nodes = ['speed_filter_mask_server', 'speed_costmap_filter_info_server'] + + # Map fully qualified names to relative ones so the node's namespace can be prepended. + remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] + + configured_params = ParameterFile( + RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites={}, + convert_types=True, + ), + allow_substs=True, + ) + + stdout_linebuf_envvar = SetEnvironmentVariable( + 'RCUTILS_LOGGING_BUFFERED_STREAM', '1' + ) + + declare_namespace_cmd = DeclareLaunchArgument( + 'namespace', default_value='', description='Top-level namespace' + ) + + declare_speed_mask_yaml_cmd = DeclareLaunchArgument( + 'speed_mask', + default_value='', + description='Full path to speed mask yaml file to load', + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + 'use_sim_time', + default_value='false', + description='Use simulation (Gazebo) clock if true', + ) + + declare_params_file_cmd = DeclareLaunchArgument( + 'params_file', + default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), + description='Full path to the ROS2 parameters file to use for all launched nodes', + ) + + declare_use_composition_cmd = DeclareLaunchArgument( + 'use_composition', + default_value='False', + description='Use composed bringup if True', + ) + + declare_container_name_cmd = DeclareLaunchArgument( + 'container_name', + default_value='nav2_container', + description='the name of container that nodes will load in if use composition', + ) + + declare_use_respawn_cmd = DeclareLaunchArgument( + 'use_respawn', + default_value='False', + description='Whether to respawn if a node crashes. Applied when composition is disabled.', + ) + + declare_use_speed_zones_cmd = DeclareLaunchArgument( + 'use_speed_zones', default_value='True', + description='Whether to enable speed zones or not' + ) + + declare_log_level_cmd = DeclareLaunchArgument( + 'log_level', default_value='info', description='log level' + ) + + load_nodes = GroupAction( + condition=IfCondition(PythonExpression(['not ', use_composition])), + actions=[ + SetParameter('use_sim_time', use_sim_time), + Node( + condition=IfCondition(use_speed_zones), + package='nav2_map_server', + executable='map_server', + name='speed_filter_mask_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params, {'yaml_filename': speed_mask_yaml_file}], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings, + ), + Node( + condition=IfCondition(use_speed_zones), + package='nav2_map_server', + executable='costmap_filter_info_server', + name='speed_costmap_filter_info_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings, + ), + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_speed_zone', + output='screen', + arguments=['--ros-args', '--log-level', log_level], + parameters=[{'autostart': autostart}, {'node_names': lifecycle_nodes}], + ), + ], + ) + # LoadComposableNode for map server twice depending if we should use the + # value of map from a CLI or launch default or user defined value in the + # yaml configuration file. They are separated since the conditions + # currently only work on the LoadComposableNodes commands and not on the + # ComposableNode node function itself + load_composable_nodes = GroupAction( + condition=IfCondition(use_composition), + actions=[ + SetParameter('use_sim_time', use_sim_time), + LoadComposableNodes( + target_container=container_name_full, + condition=IfCondition(use_speed_zones), + composable_node_descriptions=[ + ComposableNode( + package='nav2_map_server', + plugin='nav2_map_server::MapServer', + name='speed_filter_mask_server', + parameters=[ + configured_params, + {'yaml_filename': speed_mask_yaml_file} + ], + remappings=remappings, + ), + ComposableNode( + package='nav2_map_server', + plugin='nav2_map_server::CostmapFilterInfoServer', + name='speed_costmap_filter_info_server', + parameters=[configured_params], + remappings=remappings, + ), + ], + ), + + LoadComposableNodes( + target_container=container_name_full, + composable_node_descriptions=[ + ComposableNode( + package='nav2_lifecycle_manager', + plugin='nav2_lifecycle_manager::LifecycleManager', + name='lifecycle_manager_speed_zone', + parameters=[ + {'autostart': autostart, 'node_names': lifecycle_nodes} + ], + ), + ], + ), + ], + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Set environment variables + ld.add_action(stdout_linebuf_envvar) + + # Declare the launch options + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_speed_mask_yaml_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_params_file_cmd) + ld.add_action(declare_use_composition_cmd) + ld.add_action(declare_container_name_cmd) + ld.add_action(declare_use_respawn_cmd) + ld.add_action(declare_use_speed_zones_cmd) + ld.add_action(declare_log_level_cmd) + + # Add the actions to launch all of the map modifier nodes + ld.add_action(load_nodes) + ld.add_action(load_composable_nodes) + + return ld diff --git a/nav2_bringup/launch/tb4_simulation_launch.py b/nav2_bringup/launch/tb4_simulation_launch.py index f92181332bf..f9197a3085c 100644 --- a/nav2_bringup/launch/tb4_simulation_launch.py +++ b/nav2_bringup/launch/tb4_simulation_launch.py @@ -55,6 +55,7 @@ def generate_launch_description() -> LaunchDescription: namespace = LaunchConfiguration('namespace') map_yaml_file = LaunchConfiguration('map') keepout_mask_yaml_file = LaunchConfiguration('keepout_mask') + speed_mask_yaml_file = LaunchConfiguration('speed_mask') graph_filepath = LaunchConfiguration('graph') use_sim_time = LaunchConfiguration('use_sim_time') params_file = LaunchConfiguration('params_file') @@ -62,6 +63,7 @@ def generate_launch_description() -> LaunchDescription: use_composition = LaunchConfiguration('use_composition') use_respawn = LaunchConfiguration('use_respawn') use_keepout_zones = LaunchConfiguration('use_keepout_zones') + use_speed_zones = LaunchConfiguration('use_speed_zones') # Launch configuration variables specific to simulation rviz_config_file = LaunchConfiguration('rviz_config_file') @@ -104,6 +106,12 @@ def generate_launch_description() -> LaunchDescription: description='Full path to keepout mask file to load', ) + declare_speed_mask_yaml_cmd = DeclareLaunchArgument( + 'speed_mask', + default_value=os.path.join(bringup_dir, 'maps', f'{MAP_TYPE}_speed.yaml'), + description='Full path to speed mask file to load', + ) + declare_graph_file_cmd = DeclareLaunchArgument( 'graph', default_value=os.path.join(bringup_dir, 'graphs', 'turtlebot4_graph.geojson'), @@ -144,6 +152,11 @@ def generate_launch_description() -> LaunchDescription: description='Whether to enable keepout zones or not' ) + declare_use_speed_zones_cmd = DeclareLaunchArgument( + 'use_speed_zones', default_value='True', + description='Whether to enable speed zones or not' + ) + declare_rviz_config_file_cmd = DeclareLaunchArgument( 'rviz_config_file', default_value=os.path.join(bringup_dir, 'rviz', 'nav2_default_view.rviz'), @@ -216,6 +229,7 @@ def generate_launch_description() -> LaunchDescription: 'slam': slam, 'map': map_yaml_file, 'keepout_mask': keepout_mask_yaml_file, + 'speed_mask': speed_mask_yaml_file, 'graph': graph_filepath, 'use_sim_time': use_sim_time, 'params_file': params_file, @@ -223,6 +237,7 @@ def generate_launch_description() -> LaunchDescription: 'use_composition': use_composition, 'use_respawn': use_respawn, 'use_keepout_zones': use_keepout_zones, + 'use_speed_zones': use_speed_zones, }.items(), ) @@ -282,6 +297,7 @@ def generate_launch_description() -> LaunchDescription: ld.add_action(declare_slam_cmd) ld.add_action(declare_map_yaml_cmd) ld.add_action(declare_keepout_mask_yaml_cmd) + ld.add_action(declare_speed_mask_yaml_cmd) ld.add_action(declare_graph_file_cmd) ld.add_action(declare_use_sim_time_cmd) ld.add_action(declare_params_file_cmd) @@ -298,6 +314,7 @@ def generate_launch_description() -> LaunchDescription: ld.add_action(declare_robot_sdf_cmd) ld.add_action(declare_use_respawn_cmd) ld.add_action(declare_use_keepout_zones_cmd) + ld.add_action(declare_use_speed_zones_cmd) ld.add_action(set_env_vars_resources) ld.add_action(world_sdf_xacro) diff --git a/nav2_bringup/maps/depot_speed.pgm b/nav2_bringup/maps/depot_speed.pgm new file mode 100644 index 0000000000000000000000000000000000000000..c2e449b34e5463f0800b3db18ed7a738e1ca4877 GIT binary patch literal 185487 zcmeI5U5@Ow5k}|vu3~^Z$1vu>+@{U87J{hwd|^xxOt{r=nUUjOs`KmPf{ z-+p}k`in2W`0Dk$KYaW8_rL!8FTecZ$4|fh@>j24fBBnFUw!kcvR zIMNa>$J~x9#RW6uNn9426?1deB#+42#&gIeN`|p*5t>zY`J}}93woa2OP+6eBn>Q0m)DBq zFVAL|fF!pwO;Ei!sq8%v-fr(}HD|TjbRS+cNcKWcU!-NK1h*V$*|ySZ5r2${d$q4& zaW99}7>$!-hkue%+G5A)p+aOlx9+99CLpc!IQpu`(iXuiC)Jnj>`Rau))&jR`m((y z*BP5NyH`;zxZL9P39|FEid|v6mMN{Z*pX|(>wfi``z%&=a& zSX)kV6KD0-)8o!%onYRNTPK|2=0i^0i*vmR1L4T(X4I!N%(W#0@F^#`@u0$MvXtaX z;yiWZHJ#NgCN)*}EueG-A?clU={FsBMqc4UVp=y{(TR;Ts}qlZ!=ZJ-8V87Vt$xE% zPHGagKAGH4y3h}N*r2W1#w?{h;d}7|tKghVTgI((4$e`Zn!sZ|v3Y95PkZrb>uJ0G z^b6NH@$|EBf@dUvww|%qA9KyjiI3s@jAvQKt+_rqea4RXfY&VJ)+3(ne?W9xQiFhu zTWG5R3HMC|;(Ck6D>uQTQI$Z@mONgm#^8j&Ed;`xh_Y@0N1G-A9k-fM1VCV%06%Xq z&Wsj)1khF=C!)p)u-+PHMvFcIXseGCQR4(y%a1doMIQmQ)yIjbaRO*-oEa_p2(aGj z<3!Xr0kk#Fj23+a&{iKOqQ(iJt#M|w=p&%o;>RHRU=cM$fcwNDTm%mgKwAT_2puB8 zdTR(5!2<-))&Q)1=>I<2w!gs7tD~*k`F%8Hh3mN;hS5^jTihoaWSXWAD6RX7i?w`H zLtem#)Yd&mF6Kl}I!!y;Y9M0VY5?-xXv=4#9<P98L~oU(^e3fSiE~hw3X1=1=jK#mqJ`Jc!f>HX3vUfYeQ$2(AI`! zn-s%RYgu+hZJFXiTN^sF#GII6(djTwf~&S<;BuDpXe&c!6({{FbSu>d*y8xKOom}6 z_4x6;adNa}K*>$aiGfC3tp_3L^PaGKlVyCU$UqkT#;l08av>RIv?WkPT&g3HQ26XL zP+3=8k(u=&7ut$!+*j6Hf=09@D=husRH{d99S1J7<*F(=R9i=GjaDDdE!q-s$xbCE zlc5xKEiaSfkrCQz$bdO9y~pHp(kQ67&{Cl-nhM%Fhzo7Cw8eMaDWRa9I3vcvO1@Q6 z?DFGWaXX3&Z7pcL9qX-9J1VU(6UvfHR$rljwmk7P>}YER6hstPS8%C)qjREWUwTAOE%^L})G^MuIA!6M-x!>6=XOr7Pxm%Qp zFpOF03|1zLTM3U{KwBG^FmTD8qO9O$3wm0`l?v)uUeH!f<5-q?zyx;;eV}FS)OG)$ zQ;V2XO$(9r^y_|?2HM&I*;i-PmUOdh`+U!=XQx{=Cg=B(KwFK@nDRa`uByYr+UwTF zMcwM&#GJI1>1}^hw6!O)t52z|_^kNx+L9$!{NFNt zNNIP*>iN-Dqf2JhR_J8i8tLELE$2+f%!|a+6po%w;&$bJla#Px4dK@Vi$3FGeD8R^JzUu&fsoju_F_0ZpH>o^CB~$x3YpfU6;{w>sacYa{kT;Hk!~Te>_h z$I#Xd?9f&Nny$;jimVZa`$zz79hnrCu-?)SE(#-+9s+2qK}obFE3!ry?jr%Tb!1Xl zqP7YXl|BM!tHB9vH30c;wB@r=kFMpJ5gU|DH`+3>Y{oEdvDn&hjFiw;gBEjQ1CZ}V zTRt20psfZV-;K6>Hk-B@F}O_vXv@U185y@W7K0_U)d0qQVgr!xMq54`^`NZ=Am5F) zd^S;A`pl;hgxey3wk#@{jrCRrW2}g_8oba}1CZ}VTRt20u-<9_^4)05XA`x>daFUm zb)zj8O?2qE)ri4u54p{)imUCTFOaGM0smWgFEGHz`w21{tG0Ss+5 z0QqjT<+D)_+G+su-Dt~avucaq@@X(~+-S=Ivzl4UuOx=|Xsdw?Z8ZS-ZnWjIQBQoH z$gg8GFj;Q2Wl_m&sx5xiCnM5bL|YA1qAh-ntO3b(qb-|DvNCQZ6r%;S)gTtMRsZ8} zjrLFMMq7=}S8X+7aGM0smWgFEhR@}>N!)OZl+ad#7TRh6^4)05XQLjp)d1wX(U#8! zZ8g|5+-R!-h_!qJkncuYJ{$EgCpG~2ZnWjIL0b(r4L90q0Ak!~0P@{v%V(n=wABFQ zyU~`<25mLiG~8&b0f_s=1|Z*!wtP0~VNPrS^4)05XA`x>4{tRHxo))OqKOX1tp*_9 zjkbI?>S4Xr0OY&Tmd^%lHP|%VXsZE;Ik5rAccU$zje5{l1CZ}VTRxkpE$$N=gj_e; za?wNwYxxEs-;K6>HtIoJ4M4sdZTW1_R)bB$jkX$qSj#s6`EIo3vr!M)Y5?-xXv=34 zwZ(m6gOKY+TP~XDU@hMO z$OOxM2%xR|aO5aQ0?dg=CRpx60BzleBS$$BKwC#9SnflB^;U!9Pi&DugOlNA+%lkK zCX|&3WK{Z!XsgO8fu-=%PgPQelR43Vl1wP7AdtB96&SZ_oC>m3YN!k!+A^Rd6LKmD zBrbggv{mC&kfl;XW%$sR0VSD`Q$Zkc=_{bE8mEFRl^QC;hqeqT$%LE=0*Oms0d3Vd z6=bQ@P#He7Wk5+L1)VtH!AyOQnX&@bS640VSD`Q$Zkc=_@d9)i@Posnk#z zKD1>(Nhah}5J+753e1T$P6b&iHB^R=am#>`OvtGqkht^}&{mC8L6%AlmEluuy}Y~_ zE==#psUR?@h@h>vmp8O!V=Kr~u_3uu(H38g;o(6}1pyC}V@F#K7&RAUsnk#zKAjU; zZy997gq#Wj4kgbVwdHxA-CmHTGAIXGZn2X0pdqJ%fQQMkqb&!FnhUa2YN!k!bD{wy znUGUKAaUs{sJ8k-pD?N*OJz_2j9WEe$f+PuLuL5TmH{Q13bIrZm%akps&PV21%VnW z!^eH10VSCVvQ!e6z5?2+aY9Z7ff_2q$9lg`T4CQmeqos3IZ#Z&OO?yfhx#Ssi87_Xv=_-OvtGqkht^}g0}cg3>Q^FmWs>C zb}(+)T%r{@6$EOi44*!$Htd*$KtYzupePu(YQT_FL7;}p@I`I$TVw{BU@FK`u_3uu znG@w#6y1QQb|9yMz@Qdn+>$?JGuZ4&!V9ug278@3u?7q|6$EOi44-Q2{he{kV8eQ= zP}b-dzs6z9s_X5-O$$Mg31yY(w27O)r%SyxRFGF*&X29(WoeRock!3L(M zH0fHtC-q0fOx3g3Co_f|ZB=<)-x{1N&Z#n}!`ge96Zb@l`cyr8eKG}`W`#Mi zis<_8oGODVtD}?k){)6(nW|^6Pi6ztRYF@eOxJhkR2kGz8NM(nUj4%cMpm$`KR3~HzhALEt*C7Gt`+3S-@T>1)VtH$a2?wl%v8Y;uboM=Evrm1@N z`eYK9z5?S`jnnnrIaLNVRE7_28BmgGs-C?*nZ%{9z?@j)bbWVDl|c=a;bYt~pd`~& zJ$rpJiA!GrZPhqk-yt@b`U+^P#_9U*oGODFD#M4i3@FJo zRnK0ZOybg4;6Aa&>H6-RDuWs-!-uvED9JQc&t9KQ;?h@OPONddzB{MNpoYrup)CVS zGELR9*C&&>^c9#BYn-m{&Z#n}p)!1oTLzS5nyP27PbP8cE1<0!r|Y|OstjtV44?dF z^~HZ^+sJAb(^NfseKLdH!g{L)?E3DUDuWs-!-uvED9JQc&t9KQ;?h^p^%j$&2kZL& zoG1eil4oZv?~#f2iMsacRO+A%FWNGoalfLh}xR3ALVnA z`Ls13CsBxGH5sdz()vJg%H&RcZ{2Y4i0p=jr#6{zT_d3MVWK=wob6kyC*&3Mp_ryy z25K+PmDQ#!eOf(Pa+=erZ}%GxuE^#BrHYr);fBjOuSuz`xsr>WX!4Tbc3EN+B{TTG z^x{DOfD?_S{WBQ1mZyptIw?y3=@#9acPnKiqA|6|JVnuJTi6zDEh&X8Li6}PfU2cEX;RFan+7#4@(lb39>WAAO16W;rnUB@%dx*#Y(WR-=|vJ34qSj%O+ z$y;MF$9;&B!ty3ckmVh*o-X*;HZiFfuYP%n@51G%>%pb@vPtu$rf;j|WUF7R(XS_x zV)Vt+%28s1OF?USMr;1jFEz-d(r&Re_G9;+eSJH|&$jddFX?C77bFe=2tWV==M(rpiCd97 literal 0 HcmV?d00001 diff --git a/nav2_bringup/maps/depot_speed.yaml b/nav2_bringup/maps/depot_speed.yaml new file mode 100644 index 00000000000..314ede87c3f --- /dev/null +++ b/nav2_bringup/maps/depot_speed.yaml @@ -0,0 +1,7 @@ +image: depot_speed.pgm +mode: scale +resolution: 0.05 +origin: [0.0, 0.0, 0] +negate: 0 +occupied_thresh: 1.0 +free_thresh: 0.0 diff --git a/nav2_bringup/maps/warehouse_speed.pgm b/nav2_bringup/maps/warehouse_speed.pgm new file mode 100644 index 0000000000000000000000000000000000000000..1bb6828ff344390477f5da7c04e04e8711986432 GIT binary patch literal 1684105 zcmeIb+phGyvYt1N*Hug)$2OK7FXsYs5ahtdx`h~C2n+;H;A{Zj;~cm2{UIfB(`s!f zu}D=>PtSjjrYNdde5)RjQmgy>=YRT#|KV@{^MCu7|Lywo;$|MqYH{2%|r-~O-vfBL6?_zkb0KYOpAKR=JWet&-Af4=1YADrbK1yG(crDy4~W)xAtrUD_sr{s>0iP{`X zwb#t;)e>SrvjWhbMJLfde=CNSd?pS>`wBpN7DH&y#Gp8y0??kt6xuT}D2}H9v}ZAO zzg;oizu*(NOcaXaC;;tQEEDbVmi*gqTxRM}G_L@(XCX{0`GxlE*DSJT6j8vY0??kt z4cap?D2}H9v}ZAe_Dl?l<0$~`Sxlil6NBP-3P5`n(;$X>a(4NqJ5SDL@=&y{KuB<& zoOO{5Vz}Z=d-^$f>v6_^6i5jU?JgVQN)s15p+__H|myW z<30P(9_686Ujb;3F?47z6S}W*MgNHf--?O)P%y7RPW#(ad(mS4*V{Aj%?FtQpunU+ zPWy7j`dVd{n{?3r+6)+d;Lx5T4DCq^N+T)&?J26zp0uDeq5{yKqU!I-#rtv-`ftCJ zk+h*Sf&$Qg+L;H5}P-{$qJ=&Su z{e8KpLS8l&VcbQ5pa8T7B|hKX=WnY=<0JW>4cS5gGYUZacBs-_I`=D-^Q!*k*EZWa zQEN#7Xy1<32kpy8aN8}ZMFA@cK>K#2pnY2xYAq`O?c33V_HA9LwX6WNZ%1#s2_K)I zo3^-a-ONfcEX^LHo8Y)LK>m+P9;( z(C&VHGW@HLtmLn z)S`eD1)zO9Qu#`rxqVwBYON^%?b{Jcw7dO$<8@mkYKr?|-S2kj`!Bn1x1<&YtSA8On~|DOcaZ0Z=jF|Vs6+wX z3PAg2d>rlBtvS#BpAYgk526wUbSnVu+wlpsPkZ?_i*1ppHKqWxZwG9l-CeJ{*D}v< zx1<&YtSA8O+mT{!-`0g%%L+jIcJvn7=bfzNw?(4Xm;%tg9kAs#-MrJ&KHaI?Zc8l+ z*iZo4x8wA^lIH`sZKbHSqkuu$C+yr*n?LRHu_2-U$4Z{NbP|$c<8cMd((cyQnRrcU z|Cr~qPAQ^*Ed_RI*E=H_r$vf8bqS6EQz-DMz%K2I)2{N zF0{|9_u2Dl{m_8+>O+PA~;6z$vIP>TW*3PAg|iXXJk8{*UM*4)1B4YepBp#ZdRtLSc;`K#(> zrzI(LoEsjC;;u-B6{$C;-z{k+8x^q;4|%ifPFh0)QTto?b|Zu ztL*R-)7vfY*M9UdZ$SHfIXUuFfiY;8UjQ#8&ieT^ef;eGRMO%G&|WMR4Mh|%O#3{3 zkIb?f77=7Il9QO1TlEosABV|ZQekX;DqxcKX{uc=s%I3%Az28Y#Og`=XLQ&?0Z9c+ z(mvxi%g~DU-0pOCX?C?RpQ+p}LI0ABEw)hLQvt)Y2TDAP;*@;M=S^u}Tz4IM;6J0o z779oz0PWjqF0{`(C)!(`QI?a|vX%h%?RZctp@2!+mw$A18vjCxgciAth4vY~4z!26 zbh!_dQ9xD!leGKm+i;ERi*7?!mGBetJ-!vQxZdcjC9MngEGuA>_Gz+RWS=jprDM>4 zZl6wH;%6;sU9e|a0mHP`KZ zrVK+w0rRxy_vb|vnT>qRyYsDqMzk+ZH|j%B5=@y_z$WePE`0dzxyQ1>^=?l4Qf@^1 zvVNF4VQx1Mlz~=-hBZ{*!nO#(9M*F;Gn0;?cVVK__REcU!&X{K8FpdfIZs7b8ho9?9Jv~|B?Oz-_*x+R*_7B z*`PmLrA10!mh1~#DBu;aPJ7!sXbXqaugYXlilolOd7Rswb-!GZ?nJ~>jEtgyb=sTF z?b)Yeb01;JEf@L4_sL+Zamaa#CHo3kr@hDA-ipN8?vg|E*(ELN?WYzaP^W-N+9ynA zFy^B80{G+O@3ypO;|Mnj)MW_{jiJB}?c(@Is^u?s!v}B+?ULrbt&X%Wt~0kw#}Ip8 z3hdCX`kx=1=i2-?UBYXJ?Uxc5xDCyK@G$>ce_|;=iNSyo_&cHJxaX~ml;Y4A+S7VY9;Ugq=3?zEKPp1iom z7|a#eq20jTzAUV#UuqS1*PTE-f9Z@8%DHy6<$R3-?X_xh{J8=Go8=UCd>ngG;GP0Iw8tF0XSMB7^PIT!(TeuTZqvE_ z<7#304o-thlD3h+x-A8EXg4so=Ol`|`$1xUeJvQ>k3L#G0W9b*rpzl~m-hS%*Tvrq^ULFU$4u`I$Z|c|m0{wWEMR+Lw7ZU&%{PiH>jogw1l!TWFU)13I(*R5klP z7p1${2|Cp;RbYp9@e5%=c^BGeiy!m6SSJ(b9`elM$3|9vySR0=kau|7FbAT*wgNk} zi>LN>@3(3{XkS*qw+uae(8oBY3zB#@;$hF|eozIh)81xo?-Fci?;;-u~At zl0fZBA1it0_I5&JIVfO|_T_uw%Lpb+OkVD;=XQUez~n$eF7d8i0lTy>U|6Pox(t~! zgnRPZNg2UEgS3CliT3Z(&1WrE@*~iseGJh4ScE>#TG0Mj9A4D%4(*zMh0f1j1uO3Yetb z!R_dnRd!|nG9-C$)w}J6F|AIPI+NoF7e};dCDLq6fj8p{JiP%h-=+JRXkV5n=^@$R2(0*S?jx1HcAnl(&Of`Q|v~;+d+U2zKtr$&_8P*q*v`-VE zyM4VpT^@d;&RCG#0MhT)IP|TqQH;s{W_RtVfJxftW$!f1`C`{3 zBn$0)0(T`}XM^xdCVI+ zet`ne{#cN|uiI5$Ks}}t9g8Ud?P5}1H3aQf6{CHn0^78!zU{TFfZfM$%agOJpX%R_ z&lc?_Z#V7_kv+N;ut@vYT{_h#r`>pO(Z1W59Vk$*z&7owFCY^o;prNqv@ZimkIvd0 zM|~1vp_2m8-bv{1ZsJ)h-js*-Z5YX#s3} zB|n|2+-R8FOEQ%<^$J*|eSRv%{I^NM+z##aX^e#;1uW7Y@5#?kocd?bipHv}5wt4{ z$oTGT({ALRJhYFWpo~yZ0caN#^O~t}|ACeKHQi`^TY+ubRWFSLCGqB#?UlTHBryC8 zrE1x4rx*vSQNSYYGw#zvDRa>#CE=_E?P_Qk-4S!N`)`4}m;d!f_i~(1|5?0E2koQ7 zrEYu{Y4?zSOy>@Hb;6!IKTg^Zu*5%gf>#CHT&}dIh#=*F3im0)$_X_>*cz`dpzt zWpL1}0^77Je!6CYWDw%bW(|(pRA7_#Y0^0dn7rC4Eaozar)%D}79kEHf=FhKpXwS)SQ$9PN zOufpSI|{8#j~$$}^AYKyLy6T{g0wwqX_I!2`e%{kw8Zr40=Xm6eO@3jAd%l1FpD|u*tXU4yI)X*-qf{?Vs z-2P@J+l?LCB`KA%1?}xJ{>_%5U5ZdiJJ9}SCfki2+9fHKvIXt!GXBk$p{k637oP4{a!m?5YUQ0XA$t%rB+-j%5YiVb0Zr~*qv@^Hs%t*XyrvS8ztso=~@d;eJlz*{hXqSXk%oeo2 zn8|iShjuYSB@ID)yOe*iWoVa#RLmB%znIB(Lx*-TLM07Bd%Ki>v1MqNgjCEHw7;0i zc0-4DF+wE`L3_KDf3am~mxNTz7PP;Z$#z4Bb}>RF4MBUmlz*{hXqSXk%oeo2n8|iS zhjuYS1r4Qt2en<~vHU&-pj{ABIaAE-??bV}h@o8$Q8`P@?HyA8g;c*ZAr&(P z?QdkU{g|O$3{gQt(B3}t-`E4R3qmSp3fkYuV*4>eyBMN^hM>KD=D)EAXcvT3%oMc0 zk;V37elP96iiZyE?KA(4Jt$6jv6+%*>E4*xjQLxE_tLKU-*{zi|ISou>J@l1?Q)9; zX-m*vpV?UWRsh_Pi?rczU{0JKZcDQyGV>oXe*-wHsxG^rByp#3{jsi{{0 z+9l|ewgK(+nT>^S1)yD;R0(^~{++4R)GGk(5_AgNa6gAzA6YD*z*>R#(ysWI^nrby zrfiES@Lt*#f8!O}MRFS>YXzWPcoi;T4%*jg%C?9C&@O?eunlMz$!(0R6@YeOQYFkm z`#Me87Eu7&CGZrs0qr8WjghqiitkLzA@o=FN+jt$(Ya6HlAG&@v5f+E75Jk!=if`a z8~FshvOiOf_3-J+=;y@ePRE760eTnk9@&?j72Nb41A86yf{% zwmMSCsd#lEW>3UWph3h~GP2U}!e&I;8@q5^8pD; zIepbVU*WwH_v=1;;jB0bbKi{gks+&|k5_9Ca^=$f9PtdP8Aj%oW8E*hH2>ax%}8w= zo2SgR^2hkD9on+>%W2m&btc*;(xLqWn*JO2!Ta#_z8UEwKinF6BUdhB1?@w}ymD}P z%3LddbbRg5maRYB8hRsF(G=~O<3k5X-{3OkFBU(>a`D)P%|F~4dLtJx(yr?hxa3y8 zlFu9;Izajcmnna-_%W7?$2M&K;nvU_xrh-f11tIbGxwnbq;GI}%3LddjOE&)En9!M zHS|WVBD4=3YN^5HDRZs-;lZ^-TekjiYv_$!#Ynq>l{|C%V`-VE%(e1|%hwKV+4{q+ zp*M0BBkihI^5HY~c+u_(t}b(Y=m6;(T&DcR;>TDn9^0__hg(B$%-90#$gFcm^ z{lTQnQ|4Ov!|7{>wru_3*3cWdiV-XMr!8wVjBX|9p}j`X%474CxmNz@_}ZZ@TYtDU z^hT~?#7fmlp1FO@1k5w<+47kBZjCC}8QQlLqOOesdFDM^ZZkfOR&$-9U1Nks^(oK1 zXUn51{5`H*XK4S{hniLj~*P}f1o-L27@p@#r&d|PAhwXL> z3djrp8s-m_(6!d1AN>l|t4TQL=iawrOD zRX{)OT(@h@9sIJU0JK|+82+NbT?O>hE_#>l?-H-OT5}uRUqlMto1I)CUy@7X?lNS+;v1HTBajdY7&&z9+2)3jg3kISFLh?twId_Og)Y ztp*DJ;6urycDp=~M*3;zF5N)s)_Ewm3C|Dg>~qWR^wZ8tzGeK!ww*(JyF8FY(0)A& z1`7Y+L%AN?jUxuq2--K|dr~z}_y-@#Ng&I152O*amxVlUHBk5mA4(pz+vR~Yg7)iK zFi`jhAIkOEZX7X?M$oP6AoBdmxRVy)5K;tAWBl_)zkw-7XKL5wu^= zf`P(6_)xCLcH@YFG=lbx_?}b^6#l`7auUe0-2-U^?PVd)TMZQc!H1GZ?RI$}jV#dq z`}K0q*JX}h2*>S^=LcTskEV|*kVoxy`Oy)`cAi1=m(Jg%i(X~SL%VDijB4#R;rW3B zi39CNvt<^*J{r3y%g98-u0?p&UWbM3EJIruNOwfO8$aa zH2qNEI@%jYd|0%-@eJ)(G=uglYSA`VV1wQ3Rk=aqphH4H?^hbL%vu3xxxXC*Hb>KB?R!}+AuK)O9cyMw)WF%Rtn zrCaBroN!#WyR*+NyL*cE-%8s5WR73Zr!2lFtuFLO(?=CJ31r#sM@Jyrc?M1P+cmO% zY*;Iw+PD6R*I&0Z$Q<9WzgJZj-;-8*#;Mn)z)2v>c553{g+CcIJ!!w-F)R5CV$t+N zfij#=T76iwy#ehfvEJBiJK}Hhej|9-s|FGb+Fu?1fiih(DA!}Vam1Ekqjk`}5#N(4 zqoIDG+b4l6+dYtO&|VhuywyPAAABfz)NYpt(g@nGXTd<>AABg+W4m$0KpH{&Mto1I z1`7Y+Lpcd#+3tZfg7&hI=dA_`|KLN(qjtMIkVepcJqrd3|KLNp9@~v02GR)HH{yFz zHBk5mAIeD}%XSZ>5ww?uJa08n_y-?K9<|%$fi!~l>sc^R_y-@#_1JD4F_1>kz7gM( zs)52k_)ty)S+;v1ji9|O@~GV|52TS*+WCI_KuOqaD7OjE4{YXLf5Xt8 z2ez!-KT7m?Pl2*{p0v8xwaTNc($240s7xniPMvUEwp-blYTSYLvXJMkYV_vVo(kkq zyIt<-aO1w9{dyKO4hiPP6u2JSjU&XodEOATZ^ZYc>iMYQL74(4fh^ly)|H7?R%z#+ zyovOB*vT@SPg*@Z2FdQ#)9!v=_V??>Z_Y~Qz__+fI4;{gkUZ*X=NG02%C_D^DZ}}s zRd1&o_6O}JvEJC-FdUc`RbV4{*Q-Q5de#`UzdHQSCSUX3ug7-dh~{4Q>j2s};(Jon zFFN#8r@%=d%XZhfpz3I7FAI6zN>u=H=0JfwYPZV=_O0#OqW!`@($-`Gh@}OPZ zCOkiIAZ@m2FaP&fdk6vTJyagEj~v?D<$oFi+1kGZ^S&S8YujO4Ksr(=!V6RRG%i>P>Gb@SFnB{+#yoi~@ZXfcCz6(;EsrrvS7+r#(HR zKwkx*y|3Q%h62wi0PW9dPtPdOR{?16t2e!&z;g;f`*YgUGYa%o0NVTNO>ZdhoC47P zoc8pL0(}*L_P%=48wxz90JJ}+Jw2m9Uj?AOuio^A0?#S1N&9o|eD}<+S$u0k4GP>+ z0NQWK#z81xPyuK+7&Kf)fm;ed`z_fx2n7r(0PO~YhRY~$O95!VB^w8!fI$VI-C)pg z83k@BV1f3Z-}TFVESEWcYv3G&0-=m-g1yJBz0Ryy8g!?~kG&Ai}@qBEw zKmioU70^$6xHUH~;HNuw(4L2oG71zbV2SqlL>;sjMwezNkShS~xf+yFU}OcLedOpe zHWbJec$W6O_nor4F&9_xq2N~XsdYXY$|!&W`wAp#{(9gOxV-nB@;2?+$L;%LV-E_T zK(0W}SeQloeFrY@eW!d%`_6HH{gn@sG76wTwj#DVw?jK=i~?l}FaR?puJ}ZzG#oPBC>*O)YB*WdXECo-ZRz?D*1|?$7+{=@WguM*(Q>8EeP#&V>H^8~yO;^Fo0J3P5`UQFl!q(H>6& z(7vl5JL(jG_B#Ep9S!Z*^rE$_0?;n2=dra?+F8mU>&V`F3PAfk-R>Q=N&CEV_jl-= zQGdIR>-KxD2_FyjOSZiCowA$qA`YS8c-U#3S1SzDgnAgo z-aEJ}jNBM5<#+_HpuJUWOKES_KX%`(0JQgve}uUd)q}mj9T=a^2<;+{<@YL} zp7yuhrE~CePyW3;wjDXNx7GL4`qn}vo9ySJYZRvW&!gEwfjR}Cy-vStM??EHy=X0~ zfO^`u0C9D>-#cYlp+u~04Q6X@7wHTmD6mo>CxyFoD{;g(9qrlu_vQZFQsv*=%iok2 z?-zq8;1y6$`{;M+{9U=vRP?6=UMSE&feqR_CR^V7PI+tb>*{t|=hY3=sx|2x|9%d5 zy_9$QgL|#g9KWSNj>nRT(7q*l+{yRS+onZt8RtiA-N|2_Ter}(?0_UfASM=X^ zU4BH~S1iS*K4AR43P5|$*iW<<{ORcveZ5BkXzv>9L(=_IbknP=OLX=g1)%-;SWlPf zT&q7Hh~Yhe_TfX!08wC30NP0f6nKjQ(EgT4GfotMc5(m(-lBj3+SkvT`46g%K1=tO ztYVxx6@Ye~altDTkXHcO8*BKYQLIHUNpj}>5ETVu;1)yDLT<{77|5{}}hO8_v78X&!v;r9&e<}TU;I@9LDtCLYY&tGDj{@=vNtiWo#&rD*)}e8kA9BWCfsIZFKX{RFu_@568MIkf%|& zC;!vUi@f)p@_V1I*%9P{>TdZ!M{O0zb0^T=w#VFh-uq4&+INKMSe=KAvS9`CoPl;# zfy_hmY+02oICEctJdL3JzK#u!%tP~RxxwHt`Gf*_8bSLL5!mfB4~^pTuK(;%r$C-Y z(5@;xd1w@uRcVGZhYI9r1nq}X_1>9>Msc~fEcAC@fjo_%{l1P3j?6=&xZGgmm~5m# zo<`8#NaTBF^Ux?R-y0l9J)uCJM$rC51a|w(L!-F7>pwfxDUhcTw5tkF9va1ERhr?< zp#pgtLHnUpy?5rJQC#jV3;o?!AWtJ`zprD1BlFNGE;krCCL1Y`rxCO_68WClJT!{S z_XhWIl#0=Pcr@rnE;O{O3UVGA#bp7_RE$nw9O_NvLPNW%aOI&t}h@+L?~eLsL@mN$AD<EWXfHZN9W{dXYgtU| zG6kSLKT&O!kEm?558BJJmsZylfc7oI^SV|-0$S$;B%VJuWDFE#`*<0l!DqHP?_Ok4y)inj6eT(qCt`*TocA@=R z7Sp;+fo8M|eUN{|MBJCdxl`WahtJ!~a)ef+DA0^{p@;M$(MN?H!t-iqA0=ZM$9)B$ zJwGRGm5)v{t@dr5o9@S$BT-;h0NS?*&+A$dePkEf;j@JT&1mPk{RroiebC+_*Rcuh z`8>W=J^{1IZfJ+h779T77U6ka6P!Ojar@6@d2q zM732G#kkNIv{$Azhi@tX?OTNBb%lV6HiP!fWTmoF0cg+3-YSb?Txbm1E7O|8Hx+>P zEyDAU0xzN6|ML;^%b^#F(B41a=mmzfQF06@S)K`(Gb>`)@F}Kgq0`|Xu0{*cD?OB*8iYWl?-xuxYD|u)aOF$EbMB=Srq%Wr3E#;>P zmAPF(9_n@h+CP?_IWhAxROmGF(aZgf&84x!J@-2=pY$XW{ewO>-PH}m(B6r}TY*AC zQ0AoQr2YF#F~88RAOTId0PP=BlYZJavdVoFm;0TUKL+=`Ark#>v$2M`eIr~CR*A&> zLH|S7ubJD!fU|ez_Dl?l2MR!YoQQlOpBI;v{Kx*MH#R3rXg^4Bc4Z1c`v<_#emx;R z1pM6CJUM0AN3pL0*MqxpL|>OTduz9n|IFWqw>I%%Zr@A^D!&xiVE1~}m(~6i@1Wgr zy&)~MZ-_$Gh62~o-Z)~zZtp5+&o4q^`D7`w5zFgUM}2IaoO-7Q{Y3??2Y2I$XzjTI zv`518!V!sX-$pF2R~_`RxfFRWNnYRc_26zCp+H4w&moWH0}}nNjaXio2>t$UuzS5qfsSvXeM@S!&(cBrR=6IlyN>q8 z5fAzwyOzN{pBLw4==}Y%lR2{2?(LPa+I#Zb?#7T&=&is8yVt9FJH6E(wC884P(C>g zT@Urf5&M0((-1f1x03a#x{c^vuX@V)ykBS!gy--3Bu;y-hkE0P{XU>Q&rk*Bjp$vk zQZNqpwa@*mE8mloGR;fwnz;Pv<}J6UO2sRA&$}0JexLp-`ZK##;ZT_Y-|C=cwrg&g z|8{tAF4eg<*%9;prD8rj_4aX)KkPc%8%G3}9(#AEn<=TPar>fO#%#4;iveS_rzBkR z(52s0=l=Nr=C?d+roDMZyK={_+705Od+T8BO?SrETsX4%bcIRIn@u$o4a^s>;S90q*+8ak) z=}>puJl@d05$cVqqlnH6c2!9E5HG8E?|B~ar_1xSbWDvVfknD(T2Zn_ORLKMDJ?yCAZ;tQyLh%NpDX zJ_NVIcF}t{sN}sYpeLX4k~#s3+PF! zl8$V&$@OJ!KMDJ?yCAZ;tQyLh%NpDXJ_NVIcF}t{sN}sYpeL5AY7P-#QeiC+Q&(o#4+<}p@fS$DK;Af{U@{mom zb5H&x_J99-L1=PWHIy@#HMleTkU7<=cpA%+2CbayZ#R!Uw3oyl+Vh;LDR;>BvVfkn z>fmRmF7jwW`$^cB-36h^Wz|s5T-M;u>_g^Mt72I|Pg=EduD{(p_7m;Xcl-MLvD4$^ z()!^V#s7YL7)CCuhH~)By=!o1_91hsRq=%5vfZtm>u)!YJ#)KQ^mAD?l(WRr;Lhwr z=2WXOE`;a-+s#q4#lUA*q>u)!YJuCT> zutR&EF4g4@*u`w+Bu zN-Q1Slm+ypm5!lEcmeGvq2AbCpnEQ>hH`<+LpIrn-u0?+l*v<(>-MrtD2RS8tA=uc z%R@FP3+PF!ag@nZk(K;O*q7Y}(a&YoP%dzJ$R=e0J!v(LGI=UawEyzkH7CJ0{+#?c zxmz%jvrrc8FX+celP}tNmhLnI3PPL9s-axq@{mnVV_DK*9A)xUWF=n`d+Xnd87FrO zMp72glU4=&7-<6SC!yZhZ6vg~`RC6@^sZNpBcu%N&93$5TM6s0#pwUd*>MuY_DHUy zy>WzX33Bg+XupE{mEX(QrrrHJN4ej5IUn0bc_WtBt2XRXrBi|H!QD7Q=g{tWm6O7C z`$niYs_y8_F^?#)5xwhGk9gjs(=Xb&Cx4m_O#*?@77CokvZO%^$DXnIMf;U?c6*n% zSMpr9w^x3Q9!g+7Z8L@xeiWB!?|U|Y_Hk0}iIJR=T+-kP$J_LInf5kr>{*5OJ!Pmb zQJ@6o(>5g?*=Pgpr@{VeU?Z}4=@j^i-JObYlq{sZ@A^H-%iNx{pmbeIlWIqM~8-w5wXmGzjv$p4c_mhB!#FK91|c;0H9=np@VJZ!hi z%rYPVjPvAeIvXlRpUf|_>r7MvTXM_DnWZ$#Pe3;M1S~^!a{Dk}}2z{%?QHfy&D&kr0&w0pF78$YynlX}RGa%gXt$5G~Gw2u=wMpB}{ zZNl>dOFFXA<_hiI*6&F=>A5{wrt}5{a%gXt#}VoZ?GOAwCA5!|YK-J@1uoIvz}u7c z0<<^K9FuQVK!kSp8?YDjyVUk){qMy1?fbtud%;bbqQGbhh|qpbl$}$Ln2n^Qit@2EvJ3S3t}5AB=2L;I#IRHDEg1)%+oY8-X(HI6|6iwZ!y#i-#k z3fxfu+V7~wF(_bB0cf`vHGD>aI|@Mi9o0An1uQCHlJ-A8Ke^v|*6ZX?H(evrX9V;me%?ey9IrPX#-^xK47v^Kyk0 z9Et*iE0B}oG57PUdwcHnan9KB1=+n=k_*dhrKfow6{qHtPZXKv=1I& zhKB-C0cej>P(XnfDKK5I@2#HOtksXL?)kz>{zX~IP}?ZrY3I7VjkZ`t0bL3(x9f@r z-k?A`1w8GKe*%}U?ODmU3pSQtt^h0fmnQ(je~|(+?f#iKB`@ao7iAwq?W@2{3PAgdg3VB0uE27^zPu^8cz4gtZQeuR zS-O{J>MMr7{;h|PLA$5m4qk^l-4{#!p$cjL1MLSvWmhK!4%s@~xuCV}r#te1%R{?y zr#IneMQ6y{nu>Af)a|l2xiIgB_M`}!aw??P_j|tUW1jzRI@KJ)wsXv3b?(4U!@0@Q z^GSOj&ZUVK&b=SOr+I{73~onGcwQbqEg4}G1$?&6cfF^78Nnr-zyIy?ZvH@f&OpEU zg>p#C*yNGI?AGGkmye$=c?E;lo-!@3>Ei|;iPbA7JTGhU8$XOGu*xsD=|AXN-j?EA zHw|psA4~?x6l}>=lA7Co!j?K?;m&CS-qlBS3j{F zfc`|?oAWJ}y^F&uTaRseVW-!8lAX&r;d%KnFX(9rF5jCBw%JO)@qC_}Zz2pvQJ|*+ zB}?nnW+_c?%c?vr&Ohh$xaCH}O1@`E=}Vge{u0)|f?wIqv{2>W^e_}q*7KH4_Vuqx0m(6dS~b`ygMt<9;Kx4It3=qJ?qQ=r^3X3pw6EM z795vmzOCQu$@$4|lIOIA!`+o~-X?DnAnf)YVfP<8Mh9hLo-ef>S^ojs zvh{KE%QyJG12!KRud+X^)NFkEQ)6j;tdH(u9opZQ>Wp}rpOl&juHOi^x~KAl z$uWr^dp|ZN(}jO-A9!lte^?*J$J|cxzfXbn+`deHe#-Xic2CgsXC{AQWg+I9%8#0C zBP*T~d9OiH57U!5(Eh$0XT(8)iS`VZK!c~;wej#xdl&Fs#h+yQn@ctGB0`1{3c~8S!_B>1wO*Mgb|L)qjzaE(BZkKzX zn#7H%!d3Z}zNz0GPZ3kux7dAs*x7A$V~3CFf84Yl4lSmmJW*@TLW8NhX(Kq| zHkR&}8sB6xF>lAl;n1bo)W|o?xBbR^{9HTPp6!Pn=4j8xM-c_86v)~5Hk70EwYTEX zFI&YG$DJ0KS!nksk%HD2n&eAu$?^pkXo>T(`H?7wagXM*&vydSZbmD9}~`=JvMwVjTtaD8SsVCl+{u z0&Nv=Pu2J@-gB3(t;|?Q0R;-Ml2;G}TtI;?3P5`ondt-t6es}g3W9(OD9}ZL<+`13 z#dJ}dPEg=U1(vzpLC<&Ko{R&1qdY|F zD8NcyM;!111==eB?d>(D2Ncku04sSNali`{Xs-Zw>Dp^d4=A8R0ao%l;(!+@&|U#( zZ?7>upnwhqxNg@G2fRRm_6l&_-dJu#(pi2fRRm_6jh!x7U~+P(X(Q%XPbZ z<2!IVB7zqv&`JU3_EySb7X`E_0PWf$f$j3+-AW zF|HrX?OI|B?OGx+t{;5ft|hk6t|b!V`oY|;CAQG6B@*NM!F9Wq*h0IONQ~j!t~w8R$LwM1fEKe$V$CAQG6B@*NM!Q8GTw$QF6665*- z?OI|B?OGx+t{>2@CAQG6B@*NM!6$H9VhinBA~CKX%#IWpd~9_NQYE?N3Kw{Qg$j3+-AWF|Z$8w<`%Qv?~e3z;3XT*AiT4*Aj_w{eX5Yv4wUmkr>wxuG_W5 z7TUE$VqibGZdVdqXjc-5f!$ywuO+z9t|b!V`T^})VhinBA~CKXT(@h9EwpQi#K3<1 z{7$c7pOY7Xs7L%o9Mxu|hj6PtL?l?*9A$I(mAskYi#V#y$OD*&NbrE)J-USUlI-Xa zi4s4|wdo<;st*y3RyIf39R9l9Oz=e<)n?=YOhhDj!0#?yE|WXg?MGSCBN9jbo88w# zxK$q_TCHr3vdK!mBs+RUyTlK3ZF&f|>O(}MmCaE$S;?1VM~`Ti_+hS158+mQh-kF3 zIm#xqmt;qeXqWh5u1yc&R(*(Qw6ZzOCfDr+>Cq$F1#Z}D(nGjaA0i^HY>u+Yb$dy6 z^oVweALiQh5N_3nh(;@$qinL0FUgJ`(Jt}BT$>)kt@;qrXk~MhO=vI4jvmo2@xxr3 z9>T5q5YcF5bC^x8+Y8d8N3;vvu-BxAaH~EqFv&Lxi&q7TlFEL(aPp9n_RaS zq(_fv7r0@sNe|&xeTay(vN_5o*X;!@y-y(T?`TlFC#(#qyAo6ug69zCL6;D)^>J%n5JAtKVs<|vyB z?7wbBKgy6k$6+S;B93Y^@)?URTOty4@w9&@T(_5GM~}Fa_+hS158+mQh-kF3Im#w; zdr5Zmh<1q|=GycSZqaHdPKXx z4SP*`2)F7(M5L9?Q8v>`o=@QR^Q1?#_j@K19@7*&Jq*xxFAgdPKXx4SP*` z2)F7(M5L9?Q8t;|OR}R!v`hRj*QSSXt3E_DTGj@981jst*yhRyIf3WNt6Xjvmo2@xxr39>T5q5YcF5bC^x$_JZ{2 z5$ysu>^12j+^P=|kybW`*@X6j^ym@o0ype6=^@;z4-t`8Hiy}S_JZ{25$ysu>^12j z+^P=|kybWG*$mh1?#F9J8PexC%miP=QEf&(W6@21j`+5ks>O(}UmCa!`ncEA} zqerw0+_2ZAhj6PtL_}KI9A=Zby&ye$M7zKZdrf)>x9USgq?OH4HpA!b{0ik!mh_1B zQU7N5^$>2=hlo}yo5O6fk}pV)9?>pv!(Njf!mauc5ou*}m`!LeNRJ-TE^xzMlODpY z`VbLmWpkKKXfH^Q9?>pv!(Njf!mauc5ou*}m`!LeNRJ-TE^xzMlODpY`VbLmWpkL# z5j6bN3@UnH@mNgaH~E5O?n8o>O(}NmCa!`7utE2 z?kG!oMEj_Jv-^4ox9USgtCh`BHu=20Bs+RUyTlK3ZF&f|>O(}MmCa!`6Yj$d>0=;f zf-mBzHX|RQ=(8arK_6H9_rhJeg7oMSmI62IHR&PTst*y7RyK#(67D5s_9lhuLIqFG!Cb(JpYqUXvcet@;oVX=QVi%>{P% za1FmFzn>>PqP^d{;XOTsTlFEL*2?BEo2=vu(xXST3*4~Rq=#^;K14)X*&JpQ+6&U7 zN3;vvu-BxAaH~EA(xXST3*4~Rq=#^;K14)X*&JpQ+6&U7N3;vvu-BxA zaH~E+s$OSKs>qy9@A9sLtHGhx&Qs)#>k)PISi=_9z83|Q+hMd;FI zM61|u^^3V(N#wPGD&mh4ZE<5!0NRzLf(TvOj406-H=tct4rl{a#2;M};|(yV@Ca#IJ!Jb$K3vOyhZ5JX5?v6>Hkv!X#dpXu`O+&ium)GnsmIW0QclKW$9lj zLYFop{bi)bBL%o_KT@aTK5d|i_|s88x++kBxxGN79!*5((q^QGaP(2G0JN8D)wHEH zP(}P{su;#ED*)}6C2Mnl2wmEYw2=y{cNO5C{9WaQkJARKh(E#_VXjgE=JrYfr4ARN zOPdiXaj+w!0JO{KSZzfcs3QJUtAYVh1z5?8%6Z?I2wmEY+?RtRTPVQX-a^$IHno8& z;?E5oIHaKh(B4qoE2c&0(q`m}0JLqd0JOK)xTpthpo;iYG;$hsQ-JICZc-n2Btn-q zBgaE#Z*K*d+j}d%)1Nj_Mf}+rE7cDvz})_j@at|x=+b6nJxI2nRe-tuS^Z=0+CUZY zCytJS1_i<|Rq@+3Pe((9E^S7h7M1=#6<}`v)Z?)&ZJ>(y^O%}+yr}@RZ_3iYQiLvT zM*7Q0k4FkX`;j^w_h|!F#Gj7((N%#0&|V-?k0v5?X*1G8IQl480NTs7YT8m8s3QI} zRSe^o6@d23lC?QNgf4AH+DL`fy9z-2UFC$2(*~-DKf)Seu2KPLuM|+~a1pw+8IckP zJ2DDDyNr(2Rk}w+|YqT z8Y;lt-cZ~trbXz|X5@+hv~8~db9;M@i+a!os)#>DBd1X}1(@5rNqyXr2wmEY91oej zy%m7=-iq(^rwvpQe|E-7^+O6k`$NL7yA`2Jo00V(*?v|5Xn$7!*t<4RMf{1Qqo6_o zXjc)2KYv8%(q=@7wz#n<0PRXrK^v$d{wUEFH=tct4v5gD&4@0E@rKXamE?dnP(}Pv zqAhML3b2w_k_sYpX)~fkTik$lT{)l)R1tr4NsKql?MiY$gf4AHlxT|^ivrNDBo(xQ zD&mh4ZE+*e{yV>Zevsr*1w`o5X5>*t>HJFp=Jqd1o~h6Vs)#?&NJ+1I3h)Wso-&>5 zMd;FIq_c)}ccK7u`-wokc54Gw#GhW`(NCEI&|aof^Hw5sX*1GXH9EMY0JL9{t6gJl zpo;j@PA)9pQGo0AJE}D}MuaYHMjGgZ$qEIS+bhJAJX9N~BK}AUgspl7nA__$6tN&e zmo_6J%3wrH0caPKvd)k;P(}QylZ2zC6@YeWMQ_>@p-Y>Qn_6&KBL$$nk+7IW0d)#M zySk9z5DGL>0NNV~i&+#m_-3~3P8KMkl+vsG*STC8wrb96i}xCw5tmV4xvCJ1)#l= zu$V;wbqYYcx{%-y3N%sx+8YUrSrkyG0KZhFE+jaF0*w@4Zf_(kW>G+$0?@85Bshcu zjTC_PM#5qi1=J}3?dn2;LnzQl0cdX|EM`$aodVFVE+jaF0zwM>{CpaM_D?<7LIKkX zeDM9V!ky3CO~)4JQQ%X7557OpckRs3{@48a9NLImuNmlw!Fzs8>*@T%BWVASL%{~L zD+q#J7uM7H$4Javx4#C0*9`O{583FU0JQf|`AQ$qenlP&fKGqgJ$s{_AS zP#%&O4fH$`{$Y_2_p0~vu3pf>%yqkq#pG*M$bWvP!?nltH3K~lgp1c4GlzW^dViqX zbpKDaOWoXR-kARWx|cs`@N!ytF>J^Y&P=n%xBu#o{A&h!4xp!9;H5w2=TZo?2wGZ@ zBlubwef>jy%|Oo+7~0P@DsK@IOFsh3O1=_xcewtchM-iARsTTG^HhX(zqmi|>IE&r zLdk+0K4_Hyc-Hj~^)&-M2TUU5zi&qUW9$JR{Rp_sa863lex74bw|LD!3xt+9!xwZj zYLgGJgO(qaYDXTtz0bUjamJgc!w>X4)#_vZ?zugm+=G@jCr;F`)0~_OvVc8wW z=?)_U-OtRhr#|M;9<^N3#zu0~N8ZLbp*`ehnvYJ;Z~G1Qm$s>!SCj%jHYf<*?g#d4 zkBxCcdxEb%=FIIu&7h?X3WB$1Oz-cvF-~X?`N`a#l&CKSer!+>yxphyY>$m`LVL>W z`j|7f2Q`D1HYf<*o-w_@-^MrvXdfoU>tjBQ6am`X??Fo&6a;VgX+GOyW1IrC8yV<% z;?~C;+JnkLOB=}%ygg%jf4_}!3ef&%lpE3YF^6_&4+^M!C&gn}kRnP^T?ks*nDc_S z`*Zi%9vkBnnA?pE^gMyAyXb|&{(&%loj z3WB$Z-m4`W=+lz>2&25Ym+Ea?w$NX)yC+%G6BjJOVAC(`?HEjfMGkh-=ZHyDz zQ`XhT9NL4LK}#DH1aB`Qo;9~IP6677NpbGd1y<|d13xw>2;OG+UM$)eC$y)`u8;XJ zC%z=g%~I6z<7)nQn!kp2BLh88nfjPRdr&!OX(Ks;w~5}XB^%=unA?pE zH00s_dwtBIJ*XSCWNr_VXn2odxq3E>?V&wN0qr5BLt5ELj^J$ySgxMUVtXT}wR6$ACx5O{X^WUv&|WH8k9ILEk>1M%69FyI5-AEq zDr^$8v@sO~Z!f2QJix{{-AbNs#Vi?IuLRfa;baiCG%*!KZzH`|3ub7)IV^~VhVqfi` zJxO9Bw|k^rOe+(LL~m0hFP4lz@S1_1=Xkvo@0#0>K#p1-H98MTrxx`$ub{nCsKmO7 z8G7-&o;5cTbP>rMBKvltmIrP2kM6xEdYgsvTJcDW-lI9_uYRDzOlT5Pm`Lt}bhxnV z=i~P)Rs)Y(uIew?@7OeYdkyid^=oK1GSKtnsgL>ZFj}0e_1R)Q(FZMU2ok)_@V!{H zF;1SGO<7wX^B2>eWSmOzN|?EQrOcz7F)SBPW~sT6)7rVp(0;B_X^W^Ow3kZOqaCzI zm1BmcGuhSBFlW9f%FRO1(uN?x+pFP@?bjDFH&ttX7_>);CB7A7Vk(OxZdZu>m=Gj- z8}YqbuyI!7x;A5e?h)Qo8b?X^-$9v{P!QubmA+LK_zXi=MD@uMDSua#2rcxaDm)=R-k zK8P8$dheEZ3P210xvO8Xb)*(Be@$=9!$ryGC@J~HpKDryOF?43>4Z! zb{GkIV6xqNOe<(_C-Qa6F)SfrFTZo$o+N|zB?DtxLHl(k+Z_?Zl2Z0!3EGoj&<^eG zSgp5QPx~+TOZ@Hd!z7+%QUwIhQin;wun+GdmSW?PfEU}VLf-$!*sT|V^+8>1btLqq+l(JV# z+>?)rK|8d6A*QGT+G8*zq%bj+#SynFL@iAS61|P5?W+YNr+JBi@(El>4kJNtM|rRu z(+b*yBpTjhSVF>He`h5hHG}pgr(;?{y9UaG4>2q;V=ot=JxT`c&>qOr_zvwcDiTtd zn9Aaa+ZCdgCIpGz#?$uIf|1jF{XqG=J*9?`ptqwuSdM80?LiU^?=dVXWiOWaygdm9 z?MqI_w1RdGlm{PTSW?PfEJ1q`4BDYRkfre*+GAAerQp{rf|yZD=Jp_ohW8kjG(lY~ zF}EkdnA?|}j%fw$8YmAw#IU54y;y?wBp9?qdmu~WJG95B)JwriK8P8$WNr_VXn2od zNfXr7(lAfg{JA>vulCICOHRkMf_4p*2OnZszB0GCVi?+giif<{5Za+Vkfre*+GA8C zq%bj+#SynFL@iAS6209!i=WmTIn7H9l<&ZWYzRSj|C+$Zs~uKKEyuKaBkdodcN=$)T#k*7HIR$1>Obk6Z8{>GnkRz=)`Vs?WZV%ZJX%7|iyNQ$+ zJrBgRGBNb%?Pu67Y<<%Hd7vT*BnArYA-vnP=cypdlwM;otrVsDzFN`S5XZ~!d5Blm z@?HBb^x3m=?eZ|)0j%VsYLRxonwx8id`Y|jqW3Lu?ayBUj?aOH-rj?14UGz3Z@?z;V zw2!lrmwA>>_u0Mmr2a*_1>B*%_~gdio}`MW_&@QV)3lK7@~42vkBQ+&Z&M^Mma@Q4 zVg`%Kh9BIzIJ&$!*X>b_Ea*X4W$~5=d(&bV{p+sV7d688l|?_e6%+P)VHnzFR`R;z zy~(D0WlSq&>ApSE=wKYW&tX;b> z!lfynz$ML~eOX|nrH!T)$DS_|5G31cYtWvw(Np>nyC*-d<+8te2HDAznDL5IeShfD z+jQQ(So)mXv&j46e#gD{)DM(r=~8CIxm|6%JLOkQD@CckKlJErisZ#oj`S+_`L2Bz z`s}G6=s2{C-jnYWKCQjPw0a@!T4Un;ieb5UGE2?5ZqKz)9O|B~#k7Lxd(;ftm-LHi1??AsY}!1AC1&j9 z0<=fTpdH$q;#z4O+M|FHQka;^;)vT7Sjo53w!(6KigTAPPVM|Va3K?8TA4WgMQ>9i zFP4lf>gor|CvYhL-ALeLSYpOrE$|6kQjC>+q@5>cHBIUL-Nx_l34G%f+Mzu_9^Vt~7I2T= zrbu2bSs13o_uHX8WrmRjU4*h(A@XBFk?3uTUH(;5_ojB3C5}l*VIj9mw5V?Lvb%I#w^wIOX9EJXb5DMWS=5r1eCI4a z>@J4o>V-;b&nIwE3uw1Ud%Wz3alBrzv8aRgcruib!bZ^Jked~vmIefofSr|mvs7wi zIA-YeGkZ9Kxjn=tUbpMV{XMixj8uO-Lkb?h%l%t1$NjuvZ#;7vNRH@hIA`B^H!x0@ zcLGB-aU-SxF%c<75u-q_!zOWVKWwF9r$F3)-AZ0R?`~=jVYzccn~(8ERD`X{2@xsW zqq74@QKjR2a7-%$dPHCEBj9K^F;I!2S{UhR2D_7UO|;{&&^}2W#55p>OWzp1ih}-; zx)V~x_~M}k&THqii-g=S>3BH>f|>^8aI^<+V@BU7=pU(@&;$lL#@sHUgi8|rLGN)k zieeg=2qb9tHhSn<)rZ7D{UwY3k)F!r265wrzCL6OEl$NcZ4kvYkr~n3bvTQ+O~lhTe#YpdRL^gcAN{&0#0+*%{Kxgdp`0xM5m|eEmR8+>SiQOo;aLj^5GYeY9JM zs|4*9##!KFNYK4COi8_k6MTUb)lFhrnGhs;TN2{UHYS!ZGEdipZ@gCL$g>9OW6nLg z7{PGrG=V{Vm`Pz2<**9(4J&5k=$jOQP+xyqX}tcl#wTzIcCOn)8b;bfh35Bq z2$5221)T7X^PJP++5@>bqshF)95eY7@_+&u!q+}=awt9=NhIQQfe@X)>_Ahcg4 zu}6Dok17jD!9Dq)W~4n-XnwDcJS%z4@!j&RKIUAv2VJ9yqX}tclHqNtj0{@Z*v@dxG?KgpJ zdKk1vc_sec9B7~AjA>;;kmzk%q+Kj=-5xPAQmBtPcj-c?qn0Kxh~8dgJT*5lOzzT! z^mOm_F^BdjCoB0Vg`$E$igTAPJI5;u?W3+h`=j#jbPnxN7y&7Gx+bUz?Li4m?*b{# z+|JzomD14uC5eIxXpcbbPk9*>nr3AC)cl(ObnBie3p9kF@N!svtB%x z@*aB6d?dvL2KA9QF-&NO_Dq3_;tb3L`;_@%dN+Xqw8yjaX$sHVfpbNDBx{al50)yyn9Kh9riD5FgXXl~% zn7^0y>GoVg1lqH_QdAU3@nP=Lh50CD?d6h%b*o_Zg?sS+J=ZF`ZnvPwr}OzhO3Tdc zf?3tYRDI0fHn;D?9w&Db7}Q7J#4uUOXXlann7@bi=|i?P=u! zA9)kQgm!4p6euoE$sK0)>8NGnCwxT)+Ka{P*s%VzHqPhmAwx?q);8K%xJs4Cu7rEE z=6Ro{`KcyoX=2i=hrq-zncHKsib!GPk*aLaw~Hs|HE6%BY{LWVPixHW*&KhuK*K;P zGY##^0y4P+?J+6>QoLwxkFsqSpnY4HXKMvgoX^|S*%ZgCB@@FG zxmbnvlpD|^`Y_k+&@KmN$SM;U z2ryvbqz3I-e%HsG>-H$;LOXMNR7698xjjY!+M^Wpg+)wgua{EL0<=dlp*>1bUkGBd zlCRfN&H}VYEklCdzJcKmv~l?shup5f+}=*x`<9_SY6tC6iuwX~>FPC-H!opTI>a z6cw1;V=zQ$w=k8(5jQL>V!Efd8g?ROx<2wIhRNKXB}{$HncJhBi z&>p3zFF<>}hVmAmJ!%T=QHuJ)BIX|p3;FyXy199amX&;c%Jj7W?SgtUw+o6XW{SBz zN(t>ziuwZgGPFnSpgl@aUzo*o*=6DMS08y3 z!{igVEU)Wh&L?nD&P7aUkBVq0Ft^7jKzo#;zOabN(>3){3R-~nC?>y47p15#Ft^uh zC~pDUqo&XvrKm4(PrhD5c?-}UH5FOOTev9|N8GT$O1`1Ew@pKPlum^9Xzk?%XxEqk z&>o`#?NN&Q!XlI;$fdQp2wRA6q8!GQKCMSUSYT~lwWy@mS7Gq<;ol57w4G3TCqj0kgkltNK~ zxjhC$g!X9d<%LMQ1s&=m&)jYy&bg2EF=uX%dPds0OBXeHzQEibV<19%wD$5s5Yxn@ z7o5HM6O{g6`?PRU<5{{4%KDg_iF!8bM#XrS&H@JYk%#t1dDP5oeaxXf24@k|EgfT( z-a>ukO$?K{Jxjs*m_vJ%GtwSw6c_3v5ABM>yXjhe%pL8UmS0%8h{-Qisfs;!>0&rU zXb1OM&qYjD@-e+M6repu0otP!^@T`#y{YyV>LbrezI~Krd#H~&EBP3aNV|o_PJQH| zJx1P0p+4r^rHi44_9%s-0xS6#3}}y1)EA(=UPE~c&>l4vp*>oAc>&rrCIGa@sEE*R z;W9OjxM2a>8;X0|G_*(QL}-uJUS5E9jR^qlF)AXoGq=xrLVL_BMFnV&!4RQ6T6=i` z+BGHsw8yB3(9YaG>j~{KuM`!aJqAOB_GsZFl$GlQhfc6*+ zXpd6V7ofdfLwO6(9yNvbC`El?5tDDc)=Md90otRO&>p3zFEF>)Ybb94+M}k>9;K)+ zKzqH0@)n>yY6|U9iuyvNz1~!N3-yub8?Wu7B-=xM%=yM^j7X&2!iN;~k%#sec_W4T zm_vIEHMB=56ct#>$6!Evl%l=>?e!YUTY&bcDYQo^>I=|buc5pJXpfpgdz7NS5NWSB z)!ssVSNAzdyGh=ox5~VljjS}?J)+>9;K)+MB3|3wYN|odFJ-^QIhSU zKIY8rF(S|&rBGC0ZjZr$_9#Vt0ov;|l(zuwQB!D-Qq&hB?eDm5H}VevLVL_?Xpd6V z7b5NTrrKMmk384y?V}{yLw(G-ZjTX(w7+9+hxQnGBL!#|3>35riYaD_m3)*E+M^Wp z1!%9=P~HNxM@^wUN>N{kwAY(zZ=pW&tmNBANw$ajm~-78BNAzU%XPbjak}3P`}wVW z{rt7OVyJBv>Xm@@82LzhtWjL3k34g`;_z;|Rv&X{kHSKGltNK~xjhC0+M^Wpg-E-F z6>fdxncM4Armuzim@~J>h(UXlLQ#RaJq82XqZIXpNPE4h_7>_R&)nWVO0qrF$DFx6 zMkLbCNa}`GUmU z{`@FJJb-o)8D}Grc0rrXObJBZ!b$Cq$kR2=qR`i{K+J!QtmxVc3+?SSzTN|Kd(;ft zqZIW8L$o^ts-Gu)(XUz5=Y|Rk0x>rd^bT^rj&>7Ed4cnqiE$PrxCre|&w{STZxU(O zFNXjE{`Rbif!62vkGuG{pS~_!_3Jqw^Z~RGh-Rc+z*b`uuL-z@c@ATSVjse$Sb=~)6hCF)l>%NufXLyBY}YFhsk1=%gzB=X3m9X!o25 z=E*~*{JqBNpmUclM9&uO)sZ(sfIx~5Lc1G=1nnW+U;h3w+W**??*vlZ$a(D)KLIH$ z+?hSqL8QUUXg9)uK#Cg~>1h%RNCE9ZWoQpdXnGe&acECh0HbJ8oubfQEnx!#60|pP z>j{$rQZToNz(IRZLeslIijOj%2}l9$A-5y#7H(hFN8Shn^#dIy!v&-mMvDOL?e|Fg zZ3`U_tdIPQXzv)8mR(7F1ZO1bEoGc-N7|X&+lyV?L;Y!OnD2B8NWt755--voDm1^> zN1o5yHOF_$xB8g#4Y;VQD%!(8RJiwEA9*8dy83|*GoJ}aF^m?G+Pej4zpGr!;{;ON z!j)NxgaT4Pdk`AhgA$tF1ybC|D&|ym0V$w8s0{5v2~F<;DGu#COZS!1@1otxw?Lsi zCaQoG%-l ze`WN`Xg9(Dw8umhkb>*>peD2jB{aPYq_~mu+NtUSQb2o98QOyqn%)Ic9NJmQf2B0E ze@UXC0@`B`1f*bY4{AbtP(stYK#DWBGq-=GG_-$7qM!oWV-N(SU~Ug;LVHj`)4M>5 zD|lE+L{R}Lpglwa+Jh3B-s@A`%J+XxRj-$Vr)#3fFQeW4BZ6`0iVFfMZeyV5F%Xb~ zxjo4IGTMzWAduolMjG;P_gz2=Xb=r|cJAO&-K$hddW zZYAylDQ@Gm7SeG4T|f#eQQsFqY7ewaDJf}3AjP4byL4YE3GH8!D5!w;7z6<+nA@G2 z)7c-|V_qpL2&6c3yUw}&_|DN`)DCx^{oCAPvGzt1%+Sj3n z_DGnff*|cmB7Ow%pWht!yJT-3=Y#rY+CN2sc1@5+UqoqF5%DA1fBzir{q+CZ7PM!2 zv67E~sVcOi{h5fn1xA*2Q!%f|%ji3pN@FUxfI)mgr`ni>9w>kPu_=A(zJ^Azy!ZOf^}^J+LK;h3oQ|!PdOWZUC>?N zr~lmBoB>lVOqI)E+*Q3#4__Z&^HxF1#JqAneVT~2dKSXBU-sS$MwCLlSqcnjiT#ec z^#cv_*-i&`ss*H&v``cA6V5`0-jC3f_}hmsV%GPuZT?#A9_69FKJsClYGS`FQ2jum zJv-a`s4I{7b{F!JM{;H;KYm z&MbHQb>v2v!}s1psJ=!~T(KHwFsLo|!LQ4wfn@>cH)eqG}Gv+-)A)h<0{KtKv* zwCDHZr~B<8`#hCS=In;~w6t;6LHxjN*e-b%C${y4*?^AR*rPnux1-%f;6s&%Hw02# z*MRQpDImor?axI#AREul`5gA~pnPm0Ram;#ksE>Pz4!XaXXBhtv9pd-`F=)uFFxilBv(h=0+ZSCq`wx7iQRaby(q4CSh?J<3CUedM!ou8nD$+}FO{h;KI1 zmCCj@1*Ax{f9m*9#B;bOhw_S+v^&`6u5{!^pnC7UKJwW(*Tx=-{U&>B-#)XnXe%4( zW*1f}1*A|(dxm&cZ1}C17e6`ca(!OcWq%drzd(vZdpapDT4b@Fi_b)SnO`&ars7oH z^0Ix(`ME0TGc=js(;=VrC0jnw(5^Av1ybC|dF@nv0V!0`?!n%F9Y?L=PzSp2b-9W-G_>!X z?4G?jJ=E7n{#Z@f(NtKgAE>Sje^!cNw5X7AIW*F)fcHT4-g`X+&|aT0eJ#|-++V)- z^;vIkk#+?x?ol4<>mzR@?)3v5Cc_1!7)FbFM4`Q2LvafNDQ@Jvb}F5K6wn@2Uc}Uq z8(|LLd#{hYj&TO@ci-y=I!uNONHL5SyZKVQ!_?fKe}5qVK()9G?Zt_qq@e`u$M?aN z477_I(B4JnYn=$BxRLYPsV)LiFtC(TRfA0H#EY4K#Cg~Y0SgDf`Amz9>sp4DekNF1eQ3^!` zXpg}VkYb!OUsUC8ftCDSIXVZh6_kBj23(8QeMy8UanQQmI5hmjyeah6_mHe(Y^m>6h(bZdVv^ffP4#UON?EK#F0`e5Yca zHh}h1iTXDXNO5RSPg0JeMRg)Bd8I>-uwvhPuZKX#I8UQiKhR<3GXW`v(c(03P8%?{ zpGwrffk28IIj@~-J11zGsBe^p(c)aK;}%O^IXx8I$=I9ILw!92M&e#S&~fH7Xg?c2R>z$0z(tXlywZ^y;re{73v2%&U^;#ds%Y59@?*K*8T`+j~WX| zG0vGUs&co$O8%~L4UZE@aU-jka}k-_&owG;0qw<7^=k<2No#0NTGW=7ywWif>|L*i z`g#a-jPo>T^#dJe^$P8$=~CW+xxHMgZY`lbNe%5ui`o)%d#!>kj)(T7Dzqmps!Po6 z)e_1ZfcB&*v?nd9OVC~|p}YZTPntq|(xSQq?bQ;>8-VttDYPdos!PybEup*tXiu6# zd(xt|wB(hJPo}~rZF}$a5a<}^Y0&Bi$|rEyw*`$v{WLA=8!)%mYbb63+M}cbQjBxv zi>ll$K>JCj{E?p&y^hd{?TPlF~gPzAp;cskCK2F&dxDiyY2ZZ8z7XR~_R$GI~F?Sh#H z?Sf)TnS%DDB(x_js!PybEup*tXiu6#d(xt|wB(hJ>)CL9zV}`afsS#W2CaUetmM;8 zx^c8PO`Nj^%Jqe9ODJyu+LNZxp0ubgL3_1?@&=$iX$tL0i|P`zS4${w0NRtL(4MrYEiHMa z8-VttDYPdos!PybEup*tXiu6#d(xu11nt!l${T?8 zq$#u~Evie_Gd0I8W_@_N2VP-2OYtznha(xmmJ9`;U8NC+u5ABWL;gfikz}`2p?A z+2M7x>yP};pP%Y4`~5l_`bT;?L%BgzpWz+$Zk{;p>J<&%M!SKy`$bH3j=OOVkl_+a zKzllonW8;ko!u+D^hsk^S`tZdZ$#<%F1RHk#f1BXP6uvkhIXOA8<_FjNbjA>K8_lM z_Kz{$-Y^jLF6S6awBHVPI?w_GGPDnIpUz1kapv=PF?cxz%+T)c*VG5zXOH@@dneQR z>2ArwNKZo{CWZc}pK2k|zz*%<%yc*X{c%2fBvfl^Lny^Zn9p3WL{5B7<7LSB+h{it z8j-mj+QYnH;0)g>E%nhJeoy-Qz5Xgz^TUJ!uN-HqjFj~Be_UiMzk+?Irr@XF?Id|#SVrrt``-$05 zQxAcrai$o2mFfpN&eNjM{*@m0D_F_j*RI)-(4M4)_M}C1$rSC?k!NnNPMF>Xs$gz^TUJ!uN@E=~UDT z+KXiB+o+y)?#ZW*+sDzOIt!t_T0(gP(4I79ZckcNm!Q2`LU{wwo-~E_q(yZJ+N&j$ zHvsKPQ)o|GRF|N=T0(gP(4I7f_M}C1$rSB;D<-@5bX+v@9MO6HLi@Q!#Vw${SgL*v zncI`r(4MrYE}5deI`T$lJZMjOT^)0toL!5liGuKrx9|6Q2sDi|#o(({KhSYLV}|yx z^tfNa+8-VttDYPdos!PybEup*tXiu6#d(xu11nt!l z${T?8q$#u~Evie~a%G==u0MRf_< zt0j~-0PRUrXir*HmrT*l=k3{@=Hucp_o1P^n(<@++LIP)O3dvk6wscus4hW!wS@8p zpgm~{?MaL3k}2A&BX4AV3+*Ydt7FcybZap+QSki?U8t#tK+`x=48BVB1Law|?6!uH zsDGt_dgz^TUJ!uNJqe9ODJyu+LNZx zp0ubgL3_1?@&=$iX$tL0i|P`zS4${w0NRtL(4MrYEgz^TUJ!uNgz^TUJ!uN6`>X`GK{8~&+6nsA)A8P79nEFWr*Ia`36dhmjCuj#CI5IONeeY40osk6*Ulw}_H&JjTR?lURQ(!4d(s-(lNQybm(gCWR~G}& zo@9phq(yba_V>%mvc0s{k zM!TSL4NWn(Cvl-YX;ED=MSFS}@@C1#)cyzBQ(i-R(xSR#iuUTrLwj|?^fpi(bDpkA zAu~mL{kLM$z%`ec+f#I)J!w&0GDUlJrj|-|zJhu#!)a zH&TLj!9cxiZWmOpp{e@Q+BmnDN8Shm z%m^D^4GOShj2 zI`@n=w;!H*eopotgy{HC_eCpztSQ=SKW{(O@<7SnyO`UP;v%%`ImtUH4_}T*|KI#) zY8lECut&$b^8*_L0t}5(vY?r^&y@k9yuU-R`>DI&5(wAzppuupG_vzC2 zw_!p}H3aU|_w?wB`mY}7@2_9~4e|QQ+pCGMjQ083dBJX`J^Q;{S!XrZhr4~mlngJU z-39;xv>Q3EoyTK*+H*Q{+VgsWvX@#P?Yjx!X6DL4TS1_DpxWlM-I$*5sExTx8BJmL z%ga?3F9=Sx0+C-?Pn*zwo^4OG5Qw>^46l-KJlcJ9(_Qm<4vV9Vg+!@X>1A`f|2V?J z3))kZ1kztq)E(SgujykpcMAB8!wrS-zG8l^-r2co8#ay6zFq@`VU)gW3VX`u?-H~p zO|B*MW+qq(yQ6XW&wCsurs#dBn6&vC=z&gg7uW46uk@4zXt(kOpi|Lfs($5naYf|i zw6j4POv-GHWfs$f9&QCx9N_JS&cc=k>guWo>V|J)r03Gt#@tIWiWcW;Wi7T4fc8uv zEkyy^hskhgznqmb=Ua!FqMhsZtrPZU9keG|hS6dR;HfT+)tBaGpLO|`2|XlM^1FJe z+QHnO!ZF(1{&`2L$XIlK#-C$_VZC zbGwZzxXpOxXXo>>iHLq=;?Bly8lP(rTBy>(u^s$+>Rpu{J7M>y!AgBh4AY;2YX>^e zovC^$bls&}vRGhPw_z}W0kmI;uZf86o!9ev0W4H$QJ=*ZvS7QZDn0U8=jD2xObpWv znYlg1eN5T~Mzu|HNv#PCoRcMEIcVd~7b%Stm!LfbM}P}eQJ?3mIl|M}DuO-D3l{`h zx>BV_iu7lRx&7cow%%nTqMz~I-nx*?YYoD-CM{eNAH9|zCRjCb_vC$zy8-d}nrcJU zbm+X{>YbReJQcFL`I_|D4*KkUh~Le36T_TFdUJs3Fe^jdjds{E9PMFXQ=C1BdBq36 zGp^ZRPw?F0xLtNgEc#yFHn-=}W-2QyRxIuU?;lB|YQ_dtE128Yirn9Z_WRnkI}+NHwj#9KSkk3IH!9hpy*Bdhq2We4 z%9@4tq-Sl+-BQO$)YF(-m2A;ov$XZcS_wO}r+kO@q(yDX7VWi>e-Z6`-kzogD`BsN z`Sf^bKb7c#2F&eA^x8ALp(R}!bfeO{Xty%qL3>JfXir+ymTb|U7H2n0Hg3{|UmdtP z$my_QR=VZa3MA|p*fHe8Yjx}CeTtpoKw+V$fubzU1`dY7Vo z1TE^%1(s+}|C0UfIo^X*FIrm*BCyXtSlHIl9@b@OH}lbJV@XZXejD=az?abOPZ|1$ zdY;^F5LxhU7e|odT&&X;)@YxS`0gK{Ec)XfMmJ5*mwsC`Z#*A;8h_ml+z+hY2K?hd z{Za1{2urjF3@YEd=vvW97+&~6@UZPNHWjChsl{CB{!Rt|Cl=ISw4ABrX?Nq9&RqG} zFO_Ya8{b`@>#x%(VN-fENm& zz}N~v``GbibSRJ%U?rclpo9WrE5O`7c6=Ef3M2)fJ!wG+1;$o@mHgQ8WppTz6ku*o zT2MlPu@!*!vE$3=P#`G)?MVwtC@{7H&^~s28665F1)x1?K?w!MRsh<^jxVD_fusPm zCoL$Uz}N~v``GbibSRJ%fcB&XB@`H20cc<1`xmdbC@?nBN27ywXnzakVVp-ijJ*%q zp*>LWmiMuVJ{ldgL;G7O592)IVeEa-4()-0x4e%{^wH>`9opYQc^Kyr4`c6xc4!Y2 zyybmtqK`%g?a=-f%ELI1co=&hv_pHK;4SZC6MZx~XovQy>N(mrU1c4!}o=%caBwEw>P-*10gw!eM6M?8$ZkGY+>{cZG*P4v;|pdH$w{fLl} z_CY(eL;FZXAB_dtp&iazPq2oeF1zP5kZrjk#+};s164(%!s?C3;Go^;!Wc4+Sinu@M;6WP)`Xoq$c2zGR$B~Q9-Lp!v01WiR(x`}M* z9kfHc3Isbk(UK?KwxJ!`JA$U7E8Rr4^lqlzJxiw`4lT!dWLv?|aiOCEN4YTeKIV4j z_Ku*b=xS`Dk46XW(5?c(j!qo$F!nxZhxU%3spx8KqK`%g?a;0Q!H!NG@i6v2XovQW zpsDC;Y@&}w2kp?V0>O?>9Pu#rK4^#bj-aXNYHXs9MhETCt^&c1P8{(t_C9Eb_Ku*b z=xS`Dk46XW(5?c(j!qo$F!nxZhxU%3spx8KqK`%g?a;0Q!H!NG@i6v2XovQWpsDC; zY@&}w2kp?V0>O?>9Pu#rK4^#bj-aXNYHXs9MhETCt^&c1P8{(t_C9Eb_Ku*b=xS`D zk46XW(5?c(j!qo$F!nxZhxU%3spx8KqK`%g?a;0Q!H!NG@i6v2XovQWpsDC;Y@&}w z2kp?V0>O?>9Pu#rK4^#bj-aXNYHXs9MhETCt^&c1P8{(t_C9Eb_Ku*b=xS`Dk46XW z(5?c(j!qo$F!nxZhxU%3spx8KqK`%g?a;0Q!H!NG@i6v2XovQWpsDC;Y@&}w2kp?V z0>O?>9Pu#rK4^#bj-aXNYHXs9MhETCt^&c1P8{(t_C9Eb_Ku*b=xS`Dk46XW(5?c( zj!qo$F!nxZhxU%3spx8KqK`%g?a;0Q!H!NG@i6v2XovQWpsDC;Y@&}w2kp?V0>O?> z9Pu#rK4^#bj-aXNYHXs9MhETCt^&c1P8{(t_C9Eb_Ku*b=xS`Dk46XW(5?c(j!qo$ zF!nxZhxU%3spx8KqCZCW^Re}3rv3Nly)S52fnY}`{ z-5D~eJJ%3w%*VOP-|4k<;ohc_tM{e~UPX-x>^#_{W1`WO z0tE(0+1s?tw7b94S;Uget_parameter(name_ + "." + "speed_limit_topic", speed_limit_topic); + speed_limit_topic = joinWithParentNamespace(speed_limit_topic); - filter_info_topic_ = filter_info_topic; + filter_info_topic_ = joinWithParentNamespace(filter_info_topic); // Setting new costmap filter info subscriber RCLCPP_INFO( logger_, @@ -139,7 +140,7 @@ void SpeedFilter::filterInfoCallback( return; } - mask_topic_ = msg->filter_mask_topic; + mask_topic_ = joinWithParentNamespace(msg->filter_mask_topic); // Setting new filter mask subscriber RCLCPP_INFO( diff --git a/nav2_system_tests/src/costmap_filters/test_keepout_launch.py b/nav2_system_tests/src/costmap_filters/test_keepout_launch.py index cf2eedb5623..c85b8966709 100755 --- a/nav2_system_tests/src/costmap_filters/test_keepout_launch.py +++ b/nav2_system_tests/src/costmap_filters/test_keepout_launch.py @@ -147,6 +147,7 @@ def generate_launch_description() -> LaunchDescription: 'namespace': '', 'map': map_yaml_file, 'use_keepout_zones': 'False', + 'use_speed_zones': 'False', 'use_sim_time': 'True', 'params_file': new_yaml, 'bt_xml_file': bt_navigator_xml, diff --git a/nav2_system_tests/src/costmap_filters/test_speed_launch.py b/nav2_system_tests/src/costmap_filters/test_speed_launch.py index dae001a4603..cbe30268c92 100755 --- a/nav2_system_tests/src/costmap_filters/test_speed_launch.py +++ b/nav2_system_tests/src/costmap_filters/test_speed_launch.py @@ -144,6 +144,8 @@ def generate_launch_description() -> LaunchDescription: launch_arguments={ 'namespace': '', 'map': map_yaml_file, + 'use_keepout_zones': 'False', + 'use_speed_zones': 'False', 'use_sim_time': 'True', 'params_file': new_yaml, 'bt_xml_file': bt_navigator_xml, From af70b27f4c8929dc46c4536fd6d82dfeebc4e493 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 15 May 2025 17:32:23 -0700 Subject: [PATCH 02/46] suppress rviz logging to warnings and above (#5163) * suppress rviz to warnings and above Signed-off-by: Steve Macenski * Update nav2_bringup/launch/rviz_launch.py Signed-off-by: Steve Macenski --------- Signed-off-by: Steve Macenski --- nav2_bringup/launch/rviz_launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_bringup/launch/rviz_launch.py b/nav2_bringup/launch/rviz_launch.py index 8d5e3e154c0..5ee6a365adc 100644 --- a/nav2_bringup/launch/rviz_launch.py +++ b/nav2_bringup/launch/rviz_launch.py @@ -58,7 +58,7 @@ def generate_launch_description() -> LaunchDescription: package='rviz2', executable='rviz2', namespace=namespace, - arguments=['-d', rviz_config_file], + arguments=['-d', rviz_config_file, '--ros-args', '--log-level', 'warn'], output='screen', parameters=[{'use_sim_time': use_sim_time}], remappings=[ From d7e21cfb6ac9ddd27bfe594f4b4afad97b331d8b Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 15 May 2025 22:10:53 -0700 Subject: [PATCH 03/46] Adding Jazzy build job on Main PRs to automatically test Jazzy compatiblity long term (Kilted to come shortly) (#5164) * Adding jazzy testing job Signed-off-by: Steve Macenski * on PR not push Signed-off-by: Steve Macenski * move dir Signed-off-by: Steve Macenski * more Signed-off-by: Steve Macenski * moar Signed-off-by: Steve Macenski * moar Signed-off-by: Steve Macenski * more Signed-off-by: Steve Macenski * fin Signed-off-by: Steve Macenski --------- Signed-off-by: Steve Macenski --- .../workflows/build_main_against_distros.yml | 49 +++++++++++++++++++ 1 file changed, 49 insertions(+) create mode 100644 .github/workflows/build_main_against_distros.yml diff --git a/.github/workflows/build_main_against_distros.yml b/.github/workflows/build_main_against_distros.yml new file mode 100644 index 00000000000..854795f0281 --- /dev/null +++ b/.github/workflows/build_main_against_distros.yml @@ -0,0 +1,49 @@ +name: Build Against Released Distributions + +on: + workflow_dispatch: + pull_request: + branches: + - main + +jobs: + build-docker: + runs-on: ubuntu-latest + + strategy: + fail-fast: false + matrix: + ros_distro: [jazzy] + + steps: + - name: Checkout repository + uses: actions/checkout@v3 + + - name: Set up Docker build context + run: | + mkdir -p /tmp/docker_context/ws/src + cp -r . /tmp/docker_context/ws/src + echo 'FROM osrf/ros:${{ matrix.ros_distro }}-desktop-full + + RUN apt-get update && apt-get install -y \ + python3-pip \ + python3-colcon-common-extensions \ + python3-vcstool \ + git \ + curl \ + && rm -rf /var/lib/apt/lists/* + + WORKDIR /root/ws + + COPY ws /root/ws + + RUN apt-get update && rosdep update && \ + rosdep install --from-paths src --ignore-src -r -y \ + --skip-keys=slam_toolbox + + RUN . /opt/ros/${{ matrix.ros_distro }}/setup.sh && colcon build' > /tmp/docker_context/Dockerfile + + + - name: Build Docker image + run: | + docker build -t nav2-${{ matrix.ros_distro }}-main-compatibility /tmp/docker_context From 0629d5b2938802affe39d6367e269232d14614e7 Mon Sep 17 00:00:00 2001 From: Leander Stephen D'Souza Date: Fri, 16 May 2025 18:42:57 +0100 Subject: [PATCH 04/46] Prevent MPPI controller from resetting speed limits upon goal execution. (#5165) Signed-off-by: Leander Stephen D'Souza --- .../include/nav2_mppi_controller/optimizer.hpp | 3 ++- nav2_mppi_controller/src/controller.cpp | 2 +- nav2_mppi_controller/src/optimizer.cpp | 6 ++++-- 3 files changed, 7 insertions(+), 4 deletions(-) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp index e0821e548d6..2ced1fe2fc4 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp @@ -120,8 +120,9 @@ class Optimizer /** * @brief Reset the optimization problem to initial conditions + * @param Whether to reset the constraints to its base values */ - void reset(); + void reset(bool reset_dynamic_speed_limits = true); /** * @brief Get the motion model time step diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp index 57effc952ee..81f8aa5c1c0 100644 --- a/nav2_mppi_controller/src/controller.cpp +++ b/nav2_mppi_controller/src/controller.cpp @@ -86,7 +86,7 @@ void MPPIController::deactivate() void MPPIController::reset() { - optimizer_.reset(); + optimizer_.reset(false /*Don't reset zone-based speed limits between requests*/); } geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands( diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index d841699642b..3b944fc9ecd 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -132,7 +132,7 @@ void Optimizer::setOffset(double controller_frequency) } } -void Optimizer::reset() +void Optimizer::reset(bool reset_dynamic_speed_limits) { state_.reset(settings_.batch_size, settings_.time_steps); control_sequence_.reset(settings_.time_steps); @@ -141,7 +141,9 @@ void Optimizer::reset() control_history_[2] = {0.0f, 0.0f, 0.0f}; control_history_[3] = {0.0f, 0.0f, 0.0f}; - settings_.constraints = settings_.base_constraints; + if (reset_dynamic_speed_limits) { + settings_.constraints = settings_.base_constraints; + } costs_.setZero(settings_.batch_size); generated_trajectories_.reset(settings_.batch_size, settings_.time_steps); From 1addc57aa17cd5622ed50c8c0aa51c91e198bf0c Mon Sep 17 00:00:00 2001 From: Alberto Tudela Date: Fri, 16 May 2025 19:59:23 +0200 Subject: [PATCH 05/46] Backward docking without sensors (#5153) * Merge remote-tracking branch 'jakub/backward-docking-without-sensors' into docking_backward Signed-off-by: Jakubach Signed-off-by: Alberto Tudela * Minor fixes and update controller test Signed-off-by: Alberto Tudela * Add more tests Signed-off-by: Alberto Tudela * Move backward_plugin param to plugin Signed-off-by: Alberto Tudela * Fixed rotateToDock and tests Signed-off-by: Alberto Tudela * Update readme Signed-off-by: Alberto Tudela * Added rotate_to_dock_timeout Signed-off-by: Alberto Tudela * Use angular acceleration te acelerate / decelerate Signed-off-by: Alberto Tudela * Added mutex Signed-off-by: Alberto Tudela * Fix exception Signed-off-by: Alberto Tudela * Revert "Added mutex" This reverts commit 29ff010a7bf9a10f8c59287e003af2d4b8d30d14. Signed-off-by: Alberto Tudela * Rename rotate_to_dock Signed-off-by: Alberto Tudela * Rotate after undock and fixes Signed-off-by: Alberto Tudela * Minor fixes Signed-off-by: Alberto Tudela * Log after rotation Signed-off-by: Alberto Tudela --------- Signed-off-by: Jakubach Signed-off-by: Alberto Tudela --- nav2_docking/README.md | 10 ++- .../include/opennav_docking/controller.hpp | 13 +++ .../opennav_docking/docking_server.hpp | 12 +++ .../opennav_docking/src/controller.cpp | 37 +++++++++ .../opennav_docking/src/docking_server.cpp | 82 ++++++++++++++++++- .../src/simple_charging_dock.cpp | 10 ++- .../src/simple_non_charging_dock.cpp | 10 ++- .../opennav_docking/test/CMakeLists.txt | 6 ++ .../opennav_docking/test/docking_params.yaml | 1 + .../opennav_docking/test/test_controller.cpp | 72 +++++++++++++++- .../test/test_docking_server.py | 28 ++++++- .../test/test_docking_server_unit.cpp | 8 +- .../test/test_simple_charging_dock.cpp | 38 +++++++++ .../test/test_simple_non_charging_dock.cpp | 38 +++++++++ .../opennav_docking_core/charging_dock.hpp | 8 ++ 15 files changed, 359 insertions(+), 14 deletions(-) diff --git a/nav2_docking/README.md b/nav2_docking/README.md index b063e72dd75..f246c0c2493 100644 --- a/nav2_docking/README.md +++ b/nav2_docking/README.md @@ -201,12 +201,15 @@ For debugging purposes, there are several publishers which can be used with RVIZ | controller_frequency | Control frequency (Hz) for vision-control loop | double | 50.0 | | initial_perception_timeout | Timeout (s) to wait to obtain initial perception of the dock | double | 5.0 | | wait_charge_timeout | Timeout (s) to wait to see if charging starts after docking | double | 5.0 | -| dock_approach_timeout | timeout (s) to attempt vision-control approach loop | double | 30.0 | +| dock_approach_timeout | Timeout (s) to attempt vision-control approach loop | double | 30.0 | +| rotate_to_dock_timeout | Timeout (s) to attempt rotate-to-dock loop | double | 10.0 | | undock_linear_tolerance | Tolerance (m) to exit the undocking control loop at staging pose | double | 0.05 | -| undock_angular_tolerance | Angular Tolerance (rad) to exist undocking loop at staging pose | double | 0.05 | +| undock_angular_tolerance | Angular tolerance (rad) to exit undocking loop at staging pose | double | 0.05 | | max_retries | Maximum number of retries to attempt | int | 3 | | base_frame | Robot's base frame for control law | string | "base_link" | | fixed_frame | Fixed frame to use, recommended to be a smooth odometry frame **not** map | string | "odom" | +| odom_topic | The topic to use for the odometry data | string | "odom" | +| rotation_angular_tolerance | Angular tolerance (rad) to exit the rotation loop when rotate_to_dock is enabled | double | 0.05 | | dock_prestaging_tolerance | L2 distance in X,Y,Theta from the staging pose to bypass navigation | double | 0.5 | | dock_plugins | A set of dock plugins to load | vector | N/A | | dock_database | The filepath to the dock database to use for this environment | string | N/A | @@ -220,6 +223,8 @@ For debugging purposes, there are several publishers which can be used with RVIZ | controller.v_linear_max | Maximum linear velocity (m/s) | double | 0.25 | | controller.v_angular_max | Maximum angular velocity (rad/s) produced by the control law | double | 0.75 | | controller.slowdown_radius | Radius (m) around the goal pose in which the robot will start to slow down | double | 0.25 | +| controller.rotate_to_heading_angular_vel | Angular velocity (rad/s) to rotate to the goal heading when rotate_to_dock is enabled | double | 1.0 | +| controller.rotate_to_heading_max_angular_accel | Maximum angular acceleration (rad/s^2) to rotate to the goal heading when rotate_to_dock is enabled | double | 3.2 | | controller.use_collision_detection | Whether to use collision detection to avoid obstacles | bool | true | | controller.costmap_topic | The topic to use for the costmap | string | "local_costmap/costmap_raw" | | controller.footprint_topic | The topic to use for the robot's footprint | string | "local_costmap/published_footprint" | @@ -251,6 +256,7 @@ Note: `dock_plugins` and either `docks` or `dock_database` are required. | staging_x_offset | Staging pose offset forward (negative) of dock pose (m) | double | -0.7 | | staging_yaw_offset | Staging pose angle relative to dock pose (rad) | double | 0.0 | | dock_direction | Whether the robot is docking with the dock forward or backward in motion | string | "forward" or "backward" | +| rotate_to_dock | Enables backward docking without requiring a sensor for detection during the final approach. When enabled, the robot approaches the staging pose facing forward with sensor coverage for dock detection; after detection, it rotates and backs into the dock using only the initially detected pose for dead reckoning. | bool | false | Note: The external detection rotation angles are setup to work out of the box with Apriltags detectors in `image_proc` and `isaac_ros`. diff --git a/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp b/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp index 142b4beaf7d..90fb5cea38c 100644 --- a/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp +++ b/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp @@ -67,6 +67,18 @@ class Controller const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::Twist & cmd, bool is_docking, bool backward = false); + /** + * @brief Perform a command for in-place rotation. + * @param angular_distance_to_heading Angular distance to goal. + * @param current_velocity Current angular velocity. + * @param dt Control loop duration [s]. + * @returns TwistStamped command for in-place rotation. + */ + geometry_msgs::msg::Twist computeRotateToHeadingCommand( + const double & angular_distance_to_heading, + const geometry_msgs::msg::Twist & current_velocity, + const double & dt); + protected: /** * @brief Check if a trajectory is collision free. @@ -110,6 +122,7 @@ class Controller std::unique_ptr control_law_; double k_phi_, k_delta_, beta_, lambda_; double slowdown_radius_, v_linear_min_, v_linear_max_, v_angular_max_; + double rotate_to_heading_angular_vel_, rotate_to_heading_max_angular_accel_; // The trajectory of the robot while dock / undock for visualization / debug purposes rclcpp::Publisher::SharedPtr trajectory_pub_; diff --git a/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp b/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp index 74d46f48f17..61edfc85888 100644 --- a/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp +++ b/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp @@ -27,6 +27,7 @@ #include "nav2_util/node_utils.hpp" #include "nav2_util/simple_action_server.hpp" #include "nav2_util/twist_publisher.hpp" +#include "nav_2d_utils/odom_subscriber.hpp" #include "opennav_docking/controller.hpp" #include "opennav_docking/utils.hpp" #include "opennav_docking/types.hpp" @@ -94,6 +95,12 @@ class DockingServer : public nav2_util::LifecycleNode */ bool approachDock(Dock * dock, geometry_msgs::msg::PoseStamped & dock_pose, bool backward); + /** + * @brief Perform a pure rotation to dock orientation. + * @param dock_pose The target pose that will be used to rotate. + */ + void rotateToDock(const geometry_msgs::msg::PoseStamped & dock_pose); + /** * @brief Wait for charging to begin. * @param dock Dock instance, used to query isCharging(). @@ -236,6 +243,8 @@ class DockingServer : public nav2_util::LifecycleNode double wait_charge_timeout_; // Timeout to approach into the dock and reset its approach is retrying double dock_approach_timeout_; + // Timeout to rotate to the dock + double rotate_to_dock_timeout_; // When undocking, these are the tolerances for arriving at the staging pose double undock_linear_tolerance_, undock_angular_tolerance_; // Maximum number of times the robot will return to staging pose and retry docking @@ -248,11 +257,14 @@ class DockingServer : public nav2_util::LifecycleNode std::optional dock_backwards_; // The tolerance to the dock's staging pose not requiring navigation double dock_prestaging_tolerance_; + // Angular tolerance to exit the rotation loop when rotate_to_dock is enabled + double rotation_angular_tolerance_; // This is a class member so it can be accessed in publish feedback rclcpp::Time action_start_time_; std::unique_ptr vel_publisher_; + std::unique_ptr odom_sub_; std::unique_ptr docking_action_server_; std::unique_ptr undocking_action_server_; diff --git a/nav2_docking/opennav_docking/src/controller.cpp b/nav2_docking/opennav_docking/src/controller.cpp index 8128ba82253..1694c8f5375 100644 --- a/nav2_docking/opennav_docking/src/controller.cpp +++ b/nav2_docking/opennav_docking/src/controller.cpp @@ -49,6 +49,10 @@ Controller::Controller( node, "controller.v_angular_max", rclcpp::ParameterValue(0.75)); nav2_util::declare_parameter_if_not_declared( node, "controller.slowdown_radius", rclcpp::ParameterValue(0.25)); + nav2_util::declare_parameter_if_not_declared( + node, "controller.rotate_to_heading_angular_vel", rclcpp::ParameterValue(1.0)); + nav2_util::declare_parameter_if_not_declared( + node, "controller.rotate_to_heading_max_angular_accel", rclcpp::ParameterValue(3.2)); nav2_util::declare_parameter_if_not_declared( node, "controller.use_collision_detection", rclcpp::ParameterValue(true)); nav2_util::declare_parameter_if_not_declared( @@ -95,6 +99,10 @@ Controller::Controller( configureCollisionChecker(node, costmap_topic, footprint_topic, transform_tolerance_); } + node->get_parameter("controller.rotate_to_heading_angular_vel", rotate_to_heading_angular_vel_); + node->get_parameter("controller.rotate_to_heading_max_angular_accel", + rotate_to_heading_max_angular_accel_); + trajectory_pub_ = node->create_publisher("docking_trajectory", 1); } @@ -117,6 +125,31 @@ bool Controller::computeVelocityCommand( return isTrajectoryCollisionFree(pose, is_docking, backward); } +geometry_msgs::msg::Twist Controller::computeRotateToHeadingCommand( + const double & angular_distance_to_heading, + const geometry_msgs::msg::Twist & current_velocity, + const double & dt) +{ + geometry_msgs::msg::Twist cmd_vel; + const double sign = angular_distance_to_heading > 0.0 ? 1.0 : -1.0; + const double angular_vel = sign * rotate_to_heading_angular_vel_; + const double min_feasible_angular_speed = + current_velocity.angular.z - rotate_to_heading_max_angular_accel_ * dt; + const double max_feasible_angular_speed = + current_velocity.angular.z + rotate_to_heading_max_angular_accel_ * dt; + cmd_vel.angular.z = + std::clamp(angular_vel, min_feasible_angular_speed, max_feasible_angular_speed); + + // Check if we need to slow down to avoid overshooting + double max_vel_to_stop = + std::sqrt(2 * rotate_to_heading_max_angular_accel_ * fabs(angular_distance_to_heading)); + if (fabs(cmd_vel.angular.z) > max_vel_to_stop) { + cmd_vel.angular.z = sign * max_vel_to_stop; + } + + return cmd_vel; +} + bool Controller::isTrajectoryCollisionFree( const geometry_msgs::msg::Pose & target_pose, bool is_docking, bool backward) { @@ -228,6 +261,10 @@ Controller::dynamicParametersCallback(std::vector parameters) v_angular_max_ = parameter.as_double(); } else if (name == "controller.slowdown_radius") { slowdown_radius_ = parameter.as_double(); + } else if (name == "controller.rotate_to_heading_angular_vel") { + rotate_to_heading_angular_vel_ = parameter.as_double(); + } else if (name == "controller.rotate_to_heading_max_angular_accel") { + rotate_to_heading_max_angular_accel_ = parameter.as_double(); } else if (name == "controller.projection_time") { projection_time_ = parameter.as_double(); } else if (name == "controller.simulation_time_step") { diff --git a/nav2_docking/opennav_docking/src/docking_server.cpp b/nav2_docking/opennav_docking/src/docking_server.cpp index f0f0dc0f5f6..fee3b6f8f70 100644 --- a/nav2_docking/opennav_docking/src/docking_server.cpp +++ b/nav2_docking/opennav_docking/src/docking_server.cpp @@ -33,6 +33,7 @@ DockingServer::DockingServer(const rclcpp::NodeOptions & options) declare_parameter("initial_perception_timeout", 5.0); declare_parameter("wait_charge_timeout", 5.0); declare_parameter("dock_approach_timeout", 30.0); + declare_parameter("rotate_to_dock_timeout", 10.0); declare_parameter("undock_linear_tolerance", 0.05); declare_parameter("undock_angular_tolerance", 0.05); declare_parameter("max_retries", 3); @@ -40,6 +41,8 @@ DockingServer::DockingServer(const rclcpp::NodeOptions & options) declare_parameter("fixed_frame", "odom"); declare_parameter("dock_backwards", rclcpp::PARAMETER_BOOL); declare_parameter("dock_prestaging_tolerance", 0.5); + declare_parameter("odom_topic", "odom"); + declare_parameter("rotation_angular_tolerance", 0.05); } nav2_util::CallbackReturn @@ -52,14 +55,18 @@ DockingServer::on_configure(const rclcpp_lifecycle::State & state) get_parameter("initial_perception_timeout", initial_perception_timeout_); get_parameter("wait_charge_timeout", wait_charge_timeout_); get_parameter("dock_approach_timeout", dock_approach_timeout_); + get_parameter("rotate_to_dock_timeout", rotate_to_dock_timeout_); get_parameter("undock_linear_tolerance", undock_linear_tolerance_); get_parameter("undock_angular_tolerance", undock_angular_tolerance_); get_parameter("max_retries", max_retries_); get_parameter("base_frame", base_frame_); get_parameter("fixed_frame", fixed_frame_); get_parameter("dock_prestaging_tolerance", dock_prestaging_tolerance_); + get_parameter("rotation_angular_tolerance", rotation_angular_tolerance_); + RCLCPP_INFO(get_logger(), "Controller frequency set to %.4fHz", controller_frequency_); + // Check the dock_backwards deprecated parameter bool dock_backwards = false; try { if (get_parameter("dock_backwards", dock_backwards)) { @@ -73,6 +80,11 @@ DockingServer::on_configure(const rclcpp_lifecycle::State & state) vel_publisher_ = std::make_unique(node, "cmd_vel", 1); tf2_buffer_ = std::make_shared(node->get_clock()); + // Create odom subscriber for backward blind docking + std::string odom_topic; + get_parameter("odom_topic", odom_topic); + odom_sub_ = std::make_unique(node, odom_topic); + double action_server_result_timeout; nav2_util::declare_parameter_if_not_declared( node, "action_server_result_timeout", rclcpp::ParameterValue(10.0)); @@ -165,6 +177,7 @@ DockingServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) controller_.reset(); vel_publisher_.reset(); dock_backwards_.reset(); + odom_sub_.reset(); return nav2_util::CallbackReturn::SUCCESS; } @@ -289,10 +302,22 @@ void DockingServer::dockRobot() dock_backwards_.value() : (dock->plugin->getDockDirection() == opennav_docking_core::DockDirection::BACKWARD); + // If we performed a rotation before docking backward, we must rotate the staging pose + // to match the robot orientation + auto staging_pose = dock->getStagingPose(); + if (dock->plugin->shouldRotateToDock()) { + staging_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis( + tf2::getYaw(staging_pose.pose.orientation) + M_PI); + } + // Docking control loop: while not docked, run controller rclcpp::Time dock_contact_time; while (rclcpp::ok()) { try { + // Perform a 180º to face away from the dock if needed + if (dock->plugin->shouldRotateToDock()) { + rotateToDock(dock_pose); + } // Approach the dock using control law if (approachDock(dock, dock_pose, dock_backward)) { // We are docked, wait for charging to begin @@ -327,7 +352,7 @@ void DockingServer::dockRobot() } // Reset to staging pose to try again - if (!resetApproach(dock->getStagingPose(), dock_backward)) { + if (!resetApproach(staging_pose, dock_backward)) { // Cancelled, preempted, or shutting down stashDockData(goal->use_dock_id, dock, false); publishZeroVelocity(); @@ -424,6 +449,43 @@ void DockingServer::doInitialPerception(Dock * dock, geometry_msgs::msg::PoseSta } } +void DockingServer::rotateToDock(const geometry_msgs::msg::PoseStamped & dock_pose) +{ + const double dt = 1.0 / controller_frequency_; + auto target_pose = dock_pose; + target_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis( + tf2::getYaw(target_pose.pose.orientation) + M_PI); + + rclcpp::Rate loop_rate(controller_frequency_); + auto start = this->now(); + auto timeout = rclcpp::Duration::from_seconds(rotate_to_dock_timeout_); + + while (rclcpp::ok()) { + auto robot_pose = getRobotPoseInFrame(dock_pose.header.frame_id); + auto angular_distance_to_heading = angles::shortest_angular_distance( + tf2::getYaw(robot_pose.pose.orientation), tf2::getYaw(target_pose.pose.orientation)); + if (fabs(angular_distance_to_heading) < rotation_angular_tolerance_) { + break; + } + + auto current_vel = std::make_unique(); + current_vel->twist.angular.z = odom_sub_->getTwist().theta; + + auto command = std::make_unique(); + command->header = robot_pose.header; + command->twist = controller_->computeRotateToHeadingCommand( + angular_distance_to_heading, current_vel->twist, dt); + + vel_publisher_->publish(std::move(command)); + + if (this->now() - start > timeout) { + throw opennav_docking_core::FailedToControl("Timed out rotating to dock"); + } + + loop_rate.sleep(); + } +} + bool DockingServer::approachDock( Dock * dock, geometry_msgs::msg::PoseStamped & dock_pose, bool backward) { @@ -447,7 +509,7 @@ bool DockingServer::approachDock( } // Update perception - if (!dock->plugin->getRefinedPose(dock_pose, dock->id)) { + if (!dock->plugin->getRefinedPose(dock_pose, dock->id) && !dock->plugin->shouldRotateToDock()) { throw opennav_docking_core::FailedToDetectDock("Failed dock detection"); } @@ -652,6 +714,13 @@ void DockingServer::undockRobot() geometry_msgs::msg::PoseStamped staging_pose = dock->getStagingPose(dock_pose.pose, dock_pose.header.frame_id); + // If we performed a rotation before docking backward, we must rotate the staging pose + // to match the robot orientation + if (dock->shouldRotateToDock()) { + staging_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis( + tf2::getYaw(staging_pose.pose.orientation) + M_PI); + } + // Control robot to staging pose rclcpp::Time loop_start = this->now(); while (rclcpp::ok()) { @@ -684,8 +753,13 @@ void DockingServer::undockRobot() command->twist, staging_pose, undock_linear_tolerance_, undock_angular_tolerance_, false, !dock_backward)) { - RCLCPP_INFO(get_logger(), "Robot has reached staging pose"); + // Perform a 180º to the original staging pose + if (dock->shouldRotateToDock()) { + rotateToDock(staging_pose); + } + // Have reached staging_pose + RCLCPP_INFO(get_logger(), "Robot has reached staging pose"); vel_publisher_->publish(std::move(command)); if (!dock->isCharger() || dock->hasStoppedCharging()) { RCLCPP_INFO(get_logger(), "Robot has undocked!"); @@ -775,6 +849,8 @@ DockingServer::dynamicParametersCallback(std::vector paramete undock_linear_tolerance_ = parameter.as_double(); } else if (name == "undock_angular_tolerance") { undock_angular_tolerance_ = parameter.as_double(); + } else if (name == "rotation_angular_tolerance") { + rotation_angular_tolerance_ = parameter.as_double(); } } else if (type == ParameterType::PARAMETER_STRING) { if (name == "base_frame") { diff --git a/nav2_docking/opennav_docking/src/simple_charging_dock.cpp b/nav2_docking/opennav_docking/src/simple_charging_dock.cpp index 10576a2afff..c11d3018a9e 100644 --- a/nav2_docking/opennav_docking/src/simple_charging_dock.cpp +++ b/nav2_docking/opennav_docking/src/simple_charging_dock.cpp @@ -79,9 +79,11 @@ void SimpleChargingDock::configure( nav2_util::declare_parameter_if_not_declared( node_, name + ".staging_yaw_offset", rclcpp::ParameterValue(0.0)); - // Direction of docking + // Direction of docking and if we should rotate to dock nav2_util::declare_parameter_if_not_declared( node_, name + ".dock_direction", rclcpp::ParameterValue(std::string("forward"))); + nav2_util::declare_parameter_if_not_declared( + node_, name + ".rotate_to_dock", rclcpp::ParameterValue(false)); node_->get_parameter(name + ".use_battery_status", use_battery_status_); node_->get_parameter(name + ".use_external_detection_pose", use_external_detection_pose_); @@ -110,6 +112,12 @@ void SimpleChargingDock::configure( throw std::runtime_error{"Dock direction is not valid. Valid options are: forward or backward"}; } + node_->get_parameter(name + ".rotate_to_dock", rotate_to_dock_); + if (rotate_to_dock_ && dock_direction_ != opennav_docking_core::DockDirection::BACKWARD) { + throw std::runtime_error{"Parameter rotate_to_dock is enabled but dock direction is not " + "backward. Please set dock direction to backward."}; + } + // Setup filter double filter_coef; node_->get_parameter(name + ".filter_coef", filter_coef); diff --git a/nav2_docking/opennav_docking/src/simple_non_charging_dock.cpp b/nav2_docking/opennav_docking/src/simple_non_charging_dock.cpp index 54001bfe1b6..36b952b88ee 100644 --- a/nav2_docking/opennav_docking/src/simple_non_charging_dock.cpp +++ b/nav2_docking/opennav_docking/src/simple_non_charging_dock.cpp @@ -70,9 +70,11 @@ void SimpleNonChargingDock::configure( nav2_util::declare_parameter_if_not_declared( node_, name + ".staging_yaw_offset", rclcpp::ParameterValue(0.0)); - // Direction of docking + // Direction of docking and if we should rotate to dock nav2_util::declare_parameter_if_not_declared( node_, name + ".dock_direction", rclcpp::ParameterValue(std::string("forward"))); + nav2_util::declare_parameter_if_not_declared( + node_, name + ".rotate_to_dock", rclcpp::ParameterValue(false)); node_->get_parameter(name + ".use_external_detection_pose", use_external_detection_pose_); node_->get_parameter(name + ".external_detection_timeout", external_detection_timeout_); @@ -99,6 +101,12 @@ void SimpleNonChargingDock::configure( throw std::runtime_error{"Dock direction is not valid. Valid options are: forward or backward"}; } + node_->get_parameter(name + ".rotate_to_dock", rotate_to_dock_); + if (rotate_to_dock_ && dock_direction_ != opennav_docking_core::DockDirection::BACKWARD) { + throw std::runtime_error{"Parameter rotate_to_dock is enabled but dock direction is not " + "backward. Please set dock direction to backward."}; + } + // Setup filter double filter_coef; node_->get_parameter(name + ".filter_coef", filter_coef); diff --git a/nav2_docking/opennav_docking/test/CMakeLists.txt b/nav2_docking/opennav_docking/test/CMakeLists.txt index d0ec76332c0..346d8f59afa 100644 --- a/nav2_docking/opennav_docking/test/CMakeLists.txt +++ b/nav2_docking/opennav_docking/test/CMakeLists.txt @@ -128,3 +128,9 @@ ament_add_pytest_test(test_docking_server_backward test_docking_server.py NON_CHARGING_DOCK=False BACKWARD=True ) + +ament_add_pytest_test(test_docking_server_backward_blind test_docking_server.py + ENV + NON_CHARGING_DOCK=False + BACKWARD_BLIND=True +) diff --git a/nav2_docking/opennav_docking/test/docking_params.yaml b/nav2_docking/opennav_docking/test/docking_params.yaml index 79bc784b292..adcb8f48019 100644 --- a/nav2_docking/opennav_docking/test/docking_params.yaml +++ b/nav2_docking/opennav_docking/test/docking_params.yaml @@ -9,6 +9,7 @@ docking_server: plugin: opennav_docking::SimpleChargingDock use_battery_status: True dock_direction: forward + rotate_to_dock: False staging_yaw_offset: 0.0 docks: [test_dock] test_dock: diff --git a/nav2_docking/opennav_docking/test/test_controller.cpp b/nav2_docking/opennav_docking/test/test_controller.cpp index 9d4f0b9f5ea..3dc51ad60a0 100644 --- a/nav2_docking/opennav_docking/test/test_controller.cpp +++ b/nav2_docking/opennav_docking/test/test_controller.cpp @@ -229,7 +229,9 @@ TEST(ControllerTests, DynamicParameters) { rclcpp::Parameter("controller.slowdown_radius", 8.0), rclcpp::Parameter("controller.projection_time", 9.0), rclcpp::Parameter("controller.simulation_time_step", 10.0), - rclcpp::Parameter("controller.dock_collision_threshold", 11.0)}); + rclcpp::Parameter("controller.dock_collision_threshold", 11.0), + rclcpp::Parameter("controller.rotate_to_heading_angular_vel", 12.0), + rclcpp::Parameter("controller.rotate_to_heading_max_angular_accel", 13.0)}); // Spin rclcpp::spin_until_future_complete(node->get_node_base_interface(), results); @@ -246,6 +248,9 @@ TEST(ControllerTests, DynamicParameters) { EXPECT_EQ(node->get_parameter("controller.projection_time").as_double(), 9.0); EXPECT_EQ(node->get_parameter("controller.simulation_time_step").as_double(), 10.0); EXPECT_EQ(node->get_parameter("controller.dock_collision_threshold").as_double(), 11.0); + EXPECT_EQ(node->get_parameter("controller.rotate_to_heading_angular_vel").as_double(), 12.0); + EXPECT_EQ( + node->get_parameter("controller.rotate_to_heading_max_angular_accel").as_double(), 13.0); } TEST(ControllerTests, TFException) @@ -537,6 +542,71 @@ TEST(ControllerTests, CollisionCheckerUndockForward) { collision_tester->deactivate(); } +TEST(ControllerTests, RotateToHeading) { + auto node = std::make_shared("test"); + + float rotate_to_heading_angular_vel = 1.0; + float rotate_to_heading_max_angular_accel = 3.2; + nav2_util::declare_parameter_if_not_declared( + node, "controller.rotate_to_heading_angular_vel", + rclcpp::ParameterValue(rotate_to_heading_angular_vel)); + nav2_util::declare_parameter_if_not_declared( + node, "controller.rotate_to_heading_max_angular_accel", + rclcpp::ParameterValue(rotate_to_heading_max_angular_accel)); + + auto controller = std::make_unique( + node, nullptr, "test_base_frame", "test_base_frame"); + + geometry_msgs::msg::Twist current_velocity; + double angular_distance_to_heading; + double dt = 0.1; + + // Case 1: Positive angular distance, within feasible range + angular_distance_to_heading = 0.5; + current_velocity.angular.z = 0.1; + auto cmd_vel = + controller->computeRotateToHeadingCommand(angular_distance_to_heading, current_velocity, dt); + EXPECT_DOUBLE_EQ(cmd_vel.linear.x, 0.0); + EXPECT_GE(cmd_vel.angular.z, 0.0); + EXPECT_LE(cmd_vel.angular.z, rotate_to_heading_angular_vel); + + // Case 2: Negative angular distance, within feasible range + angular_distance_to_heading = -0.5; + current_velocity.angular.z = -0.1; + cmd_vel = + controller->computeRotateToHeadingCommand(angular_distance_to_heading, current_velocity, dt); + EXPECT_DOUBLE_EQ(cmd_vel.linear.x, 0.0); + EXPECT_LE(cmd_vel.angular.z, 0.0); + EXPECT_GE(cmd_vel.angular.z, -rotate_to_heading_angular_vel); + + // Case 3: Positive angular distance, exceeding max feasible speed + angular_distance_to_heading = 1.0; + current_velocity.angular.z = 0.5; + cmd_vel = + controller->computeRotateToHeadingCommand(angular_distance_to_heading, current_velocity, dt); + EXPECT_DOUBLE_EQ(cmd_vel.linear.x, 0.0); + EXPECT_DOUBLE_EQ(cmd_vel.angular.z, + current_velocity.angular.z + rotate_to_heading_max_angular_accel * dt); + + // Case 4: Negative angular distance, exceeding max feasible speed + angular_distance_to_heading = -1.0; + current_velocity.angular.z = -0.5; + cmd_vel = + controller->computeRotateToHeadingCommand(angular_distance_to_heading, current_velocity, dt); + EXPECT_DOUBLE_EQ(cmd_vel.linear.x, 0.0); + EXPECT_DOUBLE_EQ(cmd_vel.angular.z, + current_velocity.angular.z - rotate_to_heading_max_angular_accel * dt); + + // Case 5: Zero angular distance + angular_distance_to_heading = 0.0; + current_velocity.angular.z = 0.0; + cmd_vel = + controller->computeRotateToHeadingCommand(angular_distance_to_heading, current_velocity, dt); + EXPECT_DOUBLE_EQ(cmd_vel.linear.x, 0.0); + EXPECT_DOUBLE_EQ(cmd_vel.angular.z, 0.0); + + controller.reset(); +} } // namespace opennav_docking diff --git a/nav2_docking/opennav_docking/test/test_docking_server.py b/nav2_docking/opennav_docking/test/test_docking_server.py index 6d1a8d6db4d..086d76c6b3a 100644 --- a/nav2_docking/opennav_docking/test/test_docking_server.py +++ b/nav2_docking/opennav_docking/test/test_docking_server.py @@ -31,6 +31,7 @@ import launch_testing.util from nav2_common.launch import RewrittenYaml from nav2_msgs.action import DockRobot, NavigateToPose, UndockRobot +from nav_msgs.msg import Odometry import pytest import rclpy from rclpy.action.client import ActionClient @@ -67,6 +68,10 @@ def generate_test_description() -> LaunchDescription: param_substitutions.update({'dock_direction': 'backward'}) param_substitutions.update({'staging_yaw_offset': '3.14'}) + if os.getenv('BACKWARD_BLIND') == 'True': + param_substitutions.update({'dock_direction': 'backward'}) + param_substitutions.update({'rotate_to_dock': 'True'}) + configured_params = RewrittenYaml( source_file=params_file, root_key='', @@ -122,6 +127,8 @@ def setUp(self) -> None: # Latest command velocity self.command = Twist() self.node = rclpy.create_node('test_docking_server') + # Publish odometry + self.odom_pub = self.node.create_publisher(Odometry, 'odom', 10) def tearDown(self) -> None: self.node.destroy_node() @@ -150,6 +157,7 @@ def publish(self) -> None: t.transform.rotation.z = sin(self.theta / 2.0) t.transform.rotation.w = cos(self.theta / 2.0) self.tf_broadcaster.sendTransform(t) + self.publish_odometry(t) # Publish the battery state if we are using a charging dock if os.getenv('NON_CHARGING_DOCK') == 'False': b = BatteryState() @@ -159,6 +167,17 @@ def publish(self) -> None: b.current = -1.0 self.battery_state_pub.publish(b) + def publish_odometry(self, odom_to_base_link: TransformStamped) -> None: + odom = Odometry() + odom.header.stamp = self.node.get_clock().now().to_msg() + odom.header.frame_id = 'odom' + odom.child_frame_id = 'base_link' + odom.pose.pose.position.x = odom_to_base_link.transform.translation.x + odom.pose.pose.position.y = odom_to_base_link.transform.translation.y + odom.pose.pose.orientation = odom_to_base_link.transform.rotation + odom.twist.twist = self.command + self.odom_pub.publish(odom) + def action_feedback_callback(self, msg: DockRobot.Feedback) -> None: # Force the docking action to run a full recovery loop and then # make contact with the dock (based on pose of robot) before @@ -193,8 +212,13 @@ def test_docking_server(self) -> None: self.timer = self.node.create_timer(0.05, self.timer_callback) # Create action client - self.dock_action_client = ActionClient(self.node, DockRobot, 'dock_robot') - self.undock_action_client = ActionClient(self.node, UndockRobot, 'undock_robot') + self.dock_action_client: ActionClient[ + DockRobot.Goal, DockRobot.Result, DockRobot.Feedback + ] = ActionClient(self.node, DockRobot, 'dock_robot') + + self.undock_action_client: ActionClient[ + UndockRobot.Goal, UndockRobot.Result, UndockRobot.Feedback + ] = ActionClient(self.node, UndockRobot, 'undock_robot') # Subscribe to command velocity self.node.create_subscription( diff --git a/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp b/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp index a97448b2d7d..f72b4aaa242 100644 --- a/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp +++ b/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp @@ -247,11 +247,10 @@ TEST(DockingServerTests, testDynamicParams) rclcpp::Parameter("undock_angular_tolerance", 0.125), rclcpp::Parameter("base_frame", std::string("hi")), rclcpp::Parameter("fixed_frame", std::string("hi")), - rclcpp::Parameter("max_retries", 7)}); + rclcpp::Parameter("max_retries", 7), + rclcpp::Parameter("rotation_angular_tolerance", 0.42)}); - rclcpp::spin_until_future_complete( - node->get_node_base_interface(), - results); + rclcpp::spin_until_future_complete(node->get_node_base_interface(), results); EXPECT_EQ(node->get_parameter("controller_frequency").as_double(), 0.2); EXPECT_EQ(node->get_parameter("initial_perception_timeout").as_double(), 1.0); @@ -261,6 +260,7 @@ TEST(DockingServerTests, testDynamicParams) EXPECT_EQ(node->get_parameter("base_frame").as_string(), std::string("hi")); EXPECT_EQ(node->get_parameter("fixed_frame").as_string(), std::string("hi")); EXPECT_EQ(node->get_parameter("max_retries").as_int(), 7); + EXPECT_EQ(node->get_parameter("rotation_angular_tolerance").as_double(), 0.42); node->on_deactivate(rclcpp_lifecycle::State()); node->on_cleanup(rclcpp_lifecycle::State()); diff --git a/nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp b/nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp index ccd4825863b..14747ef1742 100644 --- a/nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp +++ b/nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp @@ -363,6 +363,44 @@ TEST(SimpleChargingDockTests, GetDockDirection) dock.reset(); } +TEST(SimpleChargingDockTests, ShouldRotateToDock) +{ + auto node = std::make_shared("test"); + + // Case 1: Direction to BACKWARD and rotate_to_dock to true + node->declare_parameter("my_dock.dock_direction", rclcpp::ParameterValue("backward")); + node->declare_parameter("my_dock.rotate_to_dock", rclcpp::ParameterValue(true)); + + auto dock = std::make_unique(); + EXPECT_NO_THROW(dock->configure(node, "my_dock", nullptr)); + EXPECT_EQ(dock->shouldRotateToDock(), true); + dock->cleanup(); + + // Case 2: Direction to BACKWARD and rotate_to_dock to false + node->set_parameter( + rclcpp::Parameter("my_dock.rotate_to_dock", rclcpp::ParameterValue(false))); + EXPECT_NO_THROW(dock->configure(node, "my_dock", nullptr)); + EXPECT_EQ(dock->shouldRotateToDock(), false); + + // Case 3: Direction to FORWARD and rotate_to_dock to true + node->set_parameter( + rclcpp::Parameter("my_dock.dock_direction", rclcpp::ParameterValue("forward"))); + node->set_parameter( + rclcpp::Parameter("my_dock.rotate_to_dock", rclcpp::ParameterValue(true))); + EXPECT_THROW(dock->configure(node, "my_dock", nullptr), std::runtime_error); + EXPECT_EQ(dock->shouldRotateToDock(), true); + dock->cleanup(); + + // Case 4: Direction to FORWARD and rotate_to_dock to false + node->set_parameter( + rclcpp::Parameter("my_dock.rotate_to_dock", rclcpp::ParameterValue(false))); + EXPECT_NO_THROW(dock->configure(node, "my_dock", nullptr)); + EXPECT_EQ(dock->shouldRotateToDock(), false); + + dock->cleanup(); + dock.reset(); +} + } // namespace opennav_docking int main(int argc, char ** argv) diff --git a/nav2_docking/opennav_docking/test/test_simple_non_charging_dock.cpp b/nav2_docking/opennav_docking/test/test_simple_non_charging_dock.cpp index 68bfc193379..fe274c756c9 100644 --- a/nav2_docking/opennav_docking/test/test_simple_non_charging_dock.cpp +++ b/nav2_docking/opennav_docking/test/test_simple_non_charging_dock.cpp @@ -324,6 +324,44 @@ TEST(SimpleNonChargingDockTests, GetDockDirection) dock.reset(); } +TEST(SimpleChargingDockTests, ShouldRotateToDock) +{ + auto node = std::make_shared("test"); + + // Case 1: Direction to BACKWARD and rotate_to_dock to true + node->declare_parameter("my_dock.dock_direction", rclcpp::ParameterValue("backward")); + node->declare_parameter("my_dock.rotate_to_dock", rclcpp::ParameterValue(true)); + + auto dock = std::make_unique(); + EXPECT_NO_THROW(dock->configure(node, "my_dock", nullptr)); + EXPECT_EQ(dock->shouldRotateToDock(), true); + dock->cleanup(); + + // Case 2: Direction to BACKWARD and rotate_to_dock to false + node->set_parameter( + rclcpp::Parameter("my_dock.rotate_to_dock", rclcpp::ParameterValue(false))); + EXPECT_NO_THROW(dock->configure(node, "my_dock", nullptr)); + EXPECT_EQ(dock->shouldRotateToDock(), false); + + // Case 3: Direction to FORWARD and rotate_to_dock to true + node->set_parameter( + rclcpp::Parameter("my_dock.dock_direction", rclcpp::ParameterValue("forward"))); + node->set_parameter( + rclcpp::Parameter("my_dock.rotate_to_dock", rclcpp::ParameterValue(true))); + EXPECT_THROW(dock->configure(node, "my_dock", nullptr), std::runtime_error); + EXPECT_EQ(dock->shouldRotateToDock(), true); + dock->cleanup(); + + // Case 4: Direction to FORWARD and rotate_to_dock to false + node->set_parameter( + rclcpp::Parameter("my_dock.rotate_to_dock", rclcpp::ParameterValue(false))); + EXPECT_NO_THROW(dock->configure(node, "my_dock", nullptr)); + EXPECT_EQ(dock->shouldRotateToDock(), false); + + dock->cleanup(); + dock.reset(); +} + } // namespace opennav_docking int main(int argc, char ** argv) diff --git a/nav2_docking/opennav_docking_core/include/opennav_docking_core/charging_dock.hpp b/nav2_docking/opennav_docking_core/include/opennav_docking_core/charging_dock.hpp index ada25c9f474..8d4ace00eaa 100644 --- a/nav2_docking/opennav_docking_core/include/opennav_docking_core/charging_dock.hpp +++ b/nav2_docking/opennav_docking_core/include/opennav_docking_core/charging_dock.hpp @@ -136,11 +136,19 @@ class ChargingDock */ DockDirection getDockDirection() {return dock_direction_;} + /** + * @brief Determines whether the robot should rotate 180º to face away from the dock. + * For example, to perform a backward docking without detections. + * @return bool If the robot should rotate to face away from the dock. + */ + bool shouldRotateToDock() {return rotate_to_dock_;} + std::string getName() {return name_;} protected: std::string name_; DockDirection dock_direction_{DockDirection::UNKNOWN}; + bool rotate_to_dock_{false}; }; } // namespace opennav_docking_core From 68c130fa04343446bb0fa2b6540d40048ace2790 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 16 May 2025 11:32:00 -0700 Subject: [PATCH 06/46] Speed up CI builds for released distros (#5168) Signed-off-by: Steve Macenski --- .github/workflows/build_main_against_distros.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/build_main_against_distros.yml b/.github/workflows/build_main_against_distros.yml index 854795f0281..18ee47949ca 100644 --- a/.github/workflows/build_main_against_distros.yml +++ b/.github/workflows/build_main_against_distros.yml @@ -23,7 +23,7 @@ jobs: run: | mkdir -p /tmp/docker_context/ws/src cp -r . /tmp/docker_context/ws/src - echo 'FROM osrf/ros:${{ matrix.ros_distro }}-desktop-full + echo 'FROM ghcr.io/ros-navigation/nav2_docker:${{ matrix.ros_distro }}-nightly RUN apt-get update && apt-get install -y \ python3-pip \ From 130449df667fcaa59e6fce44adf4d06ab6e3f65c Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 16 May 2025 13:10:45 -0700 Subject: [PATCH 07/46] Save 2 minutes in Main-Jazzy build times to align with other CI job lengths (#5169) * Save 2 minutes in Main-Jazzy build times to align with other CI job lengths Signed-off-by: Steve Macenski * Update build_main_against_distros.yml Signed-off-by: Steve Macenski * Update build_main_against_distros.yml Signed-off-by: Steve Macenski --------- Signed-off-by: Steve Macenski --- .github/workflows/build_main_against_distros.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/build_main_against_distros.yml b/.github/workflows/build_main_against_distros.yml index 18ee47949ca..6cc0ba96e24 100644 --- a/.github/workflows/build_main_against_distros.yml +++ b/.github/workflows/build_main_against_distros.yml @@ -41,7 +41,7 @@ jobs: rosdep install --from-paths src --ignore-src -r -y \ --skip-keys=slam_toolbox - RUN . /opt/ros/${{ matrix.ros_distro }}/setup.sh && colcon build' > /tmp/docker_context/Dockerfile + RUN . /opt/ros/${{ matrix.ros_distro }}/setup.sh && colcon build --packages-skip nav2_system_tests nav2_bringup nav2_simple_commander nav2_loopback_sim navigation2' > /tmp/docker_context/Dockerfile - name: Build Docker image From faf189ad9756dc001712d72547e478fcbe7d6719 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 16 May 2025 13:48:22 -0700 Subject: [PATCH 08/46] Fixing docking server when already docked at the requeste ddock (#5171) Signed-off-by: Steve Macenski --- nav2_docking/opennav_docking/src/docking_server.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/nav2_docking/opennav_docking/src/docking_server.cpp b/nav2_docking/opennav_docking/src/docking_server.cpp index fee3b6f8f70..4e864d77abc 100644 --- a/nav2_docking/opennav_docking/src/docking_server.cpp +++ b/nav2_docking/opennav_docking/src/docking_server.cpp @@ -265,6 +265,8 @@ void DockingServer::dockRobot() if (dock->plugin->isCharger() && (dock->plugin->isDocked() || dock->plugin->isCharging())) { RCLCPP_INFO( get_logger(), "Robot is already docked and/or charging (if applicable), no need to dock"); + result->success = true; + docking_action_server_->succeeded_current(result); return; } From 2b679b026cbcf7e5d8b36d2ada51985ce98dc5a3 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 19 May 2025 11:26:14 -0700 Subject: [PATCH 09/46] Update mergify.yml Signed-off-by: Steve Macenski --- .github/mergify.yml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/mergify.yml b/.github/mergify.yml index f91d5df4d38..bf71c93c9f0 100644 --- a/.github/mergify.yml +++ b/.github/mergify.yml @@ -8,14 +8,14 @@ pull_request_rules: branches: - jazzy - - name: backport to iron at reviewers discretion + - name: backport to humble-main at reviewers discretion conditions: - base=main - - "label=backport-iron" + - "label=backport-humble-main" actions: backport: branches: - - iron + - humble_main - name: backport to humble at reviewers discretion conditions: From 630fd450d535f66cbfb13f5ddf7c1fc20604f4a9 Mon Sep 17 00:00:00 2001 From: Brayan Pallares Date: Mon, 19 May 2025 13:38:08 -0500 Subject: [PATCH 10/46] fix MPPI goal critic inversion (#5088) (#5105) * fix MPPI goal critic inversion (#5088) Signed-off-by: brayanpa * Support path inversion in all critics (#5088) Signed-off-by: brayanpa * Fix code style issues and formatting Signed-off-by: brayanpa * Remove trailing whitespaces Signed-off-by: brayanpa * Fix goalAngleCritic tests Signed-off-by: brayanpa * Normalize code formatting Signed-off-by: brayanpa * Add getLastPathPose test Signed-off-by: brayanpa * Abstract getCriticGoal in nav2_mppi critics Signed-off-by: brayanpa * Fix whitespace issues Signed-off-by: brayanpa * Fix getCriticGoal test Signed-off-by: brayanpa --------- Signed-off-by: brayanpa Signed-off-by: Brayan Pallares --- nav2_mppi_controller/README.md | 2 +- .../critics/cost_critic.hpp | 1 + .../critics/goal_angle_critic.hpp | 1 + .../critics/goal_critic.hpp | 1 + .../critics/obstacles_critic.hpp | 1 + .../critics/path_align_critic.hpp | 1 + .../critics/path_angle_critic.hpp | 1 + .../critics/path_follow_critic.hpp | 1 + .../critics/prefer_forward_critic.hpp | 1 + .../critics/twirling_critic.hpp | 1 + .../nav2_mppi_controller/tools/utils.hpp | 43 +++++++++++ .../src/critics/cost_critic.cpp | 7 +- .../src/critics/goal_angle_critic.cpp | 18 +++-- .../src/critics/goal_critic.cpp | 17 +++- .../src/critics/obstacles_critic.cpp | 7 +- .../src/critics/path_align_critic.cpp | 13 +++- .../src/critics/path_angle_critic.cpp | 11 ++- .../src/critics/path_follow_critic.cpp | 14 +++- .../src/critics/prefer_forward_critic.cpp | 13 +++- .../src/critics/twirling_critic.cpp | 13 +++- nav2_mppi_controller/test/critics_tests.cpp | 5 ++ nav2_mppi_controller/test/utils_test.cpp | 77 +++++++++++++++++++ 22 files changed, 226 insertions(+), 23 deletions(-) diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md index 3b11644e19f..68db5b73719 100644 --- a/nav2_mppi_controller/README.md +++ b/nav2_mppi_controller/README.md @@ -73,7 +73,7 @@ This process is then repeated a number of times and returns a converged solution | max_robot_pose_search_dist | double | Default: Costmap half-size. Max integrated distance ahead of robot pose to search for nearest path point in case of path looping. | | prune_distance | double | Default: 1.5. Distance ahead of nearest point on path to robot to prune path to. | | transform_tolerance | double | Default: 0.1. Time tolerance for data transformations with TF. | - | enforce_path_inversion | double | Default: False. If true, it will prune paths containing cusping points for segments changing directions (e.g. path inversions) such that the controller will be forced to change directions at or very near the planner's requested inversion point. This is targeting Smac Planner users with feasible paths who need their robots to switch directions where specifically requested. | + | enforce_path_inversion | double | Default: False. If true, it will prune paths containing cusping points for segments changing directions (e.g. path inversions) such that the controller will be forced to change directions at or very near the planner's requested inversion point. In addition, these cusping points will also be treated by the critics as local goals that the robot will attempt to reach. This is targeting Smac Planner users with feasible paths who need their robots to switch directions where specifically requested. | | inversion_xy_tolerance | double | Default: 0.2. Cartesian proximity (m) to path inversion point to be considered "achieved" to pass on the rest of the path after path inversion. | | inversion_yaw_tolerance | double | Default: 0.4. Angular proximity (radians) to path inversion point to be considered "achieved" to pass on the rest of the path after path inversion. 0.4 rad = 23 deg. | diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp index c285a14b664..2646c98fd9e 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp @@ -144,6 +144,7 @@ class CostCritic : public CriticFunction std::string inflation_layer_name_; unsigned int power_{0}; + bool enforce_path_inversion_{false}; }; } // namespace mppi::critics diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_angle_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_angle_critic.hpp index 08ec354cf3b..58c108b21cd 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_angle_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_angle_critic.hpp @@ -46,6 +46,7 @@ class GoalAngleCritic : public CriticFunction float threshold_to_consider_{0}; unsigned int power_{0}; float weight_{0}; + bool enforce_path_inversion_{false}; }; } // namespace mppi::critics diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_critic.hpp index 8fb8fb688ae..6b248f29945 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_critic.hpp @@ -46,6 +46,7 @@ class GoalCritic : public CriticFunction unsigned int power_{0}; float weight_{0}; float threshold_to_consider_{0}; + bool enforce_path_inversion_{false}; }; } // namespace mppi::critics diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp index 31b6a3d0df8..23fb3db2ca0 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp @@ -97,6 +97,7 @@ class ObstaclesCritic : public CriticFunction unsigned int power_{0}; float repulsion_weight_, critical_weight_{0}; + bool enforce_path_inversion_{false}; std::string inflation_layer_name_; }; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp index f7ad2fda6a9..133447a8774 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp @@ -52,6 +52,7 @@ class PathAlignCritic : public CriticFunction bool use_path_orientations_{false}; unsigned int power_{0}; float weight_{0}; + bool enforce_path_inversion_{false}; }; } // namespace mppi::critics diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp index b137270aba9..85b9d48eb83 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp @@ -81,6 +81,7 @@ class PathAngleCritic : public CriticFunction unsigned int power_{0}; float weight_{0}; + bool enforce_path_inversion_{false}; }; } // namespace mppi::critics diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp index 1ccd08c32fe..aacec055ccf 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp @@ -53,6 +53,7 @@ class PathFollowCritic : public CriticFunction unsigned int power_{0}; float weight_{0}; + bool enforce_path_inversion_{false}; }; } // namespace mppi::critics diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/prefer_forward_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/prefer_forward_critic.hpp index e17ad235e5c..811ba9ae9a6 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/prefer_forward_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/prefer_forward_critic.hpp @@ -45,6 +45,7 @@ class PreferForwardCritic : public CriticFunction unsigned int power_{0}; float weight_{0}; float threshold_to_consider_{0}; + bool enforce_path_inversion_{false}; }; } // namespace mppi::critics diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/twirling_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/twirling_critic.hpp index 3e316a567e0..bc40b194404 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/twirling_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/twirling_critic.hpp @@ -44,6 +44,7 @@ class TwirlingCritic : public CriticFunction protected: unsigned int power_{0}; float weight_{0}; + bool enforce_path_inversion_{false}; }; } // namespace mppi::critics diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp index 2ca44fd0137..40f825fd69c 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -218,6 +218,49 @@ inline models::Path toTensor(const nav_msgs::msg::Path & path) return result; } +/** + * @brief Get the last pose from a path + * @param path Reference to the path + * @return geometry_msgs::msg::Pose Last pose in the path + */ +inline geometry_msgs::msg::Pose getLastPathPose(const models::Path & path) +{ + const unsigned int path_last_idx = path.x.size() - 1; + + auto last_orientation = path.yaws(path_last_idx); + + tf2::Quaternion pose_orientation; + pose_orientation.setRPY(0.0, 0.0, last_orientation); + + geometry_msgs::msg::Pose pathPose; + pathPose.position.x = path.x(path_last_idx); + pathPose.position.y = path.y(path_last_idx); + pathPose.orientation.x = pose_orientation.x(); + pathPose.orientation.y = pose_orientation.y(); + pathPose.orientation.z = pose_orientation.z(); + pathPose.orientation.w = pose_orientation.w(); + + return pathPose; +} + +/** + * @brief Get the target pose to be evaluated by the critic + * @param data Data to use + * @param enforce_path_inversion True to return the cusp point (last pose of the path) + * instead of the original goal + * @return geometry_msgs::msg::Pose Target pose for the critic + */ +inline geometry_msgs::msg::Pose getCriticGoal( + const CriticData & data, + bool enforce_path_inversion) +{ + if (enforce_path_inversion) { + return getLastPathPose(data.path); + } else { + return data.goal; + } +} + /** * @brief Check if the robot pose is within the Goal Checker's tolerances to goal * @param global_checker Pointer to the goal checker diff --git a/nav2_mppi_controller/src/critics/cost_critic.cpp b/nav2_mppi_controller/src/critics/cost_critic.cpp index 0efbb7b24bb..cb7b39e856f 100644 --- a/nav2_mppi_controller/src/critics/cost_critic.cpp +++ b/nav2_mppi_controller/src/critics/cost_critic.cpp @@ -22,6 +22,9 @@ namespace mppi::critics void CostCritic::initialize() { + auto getParentParam = parameters_handler_->getParamGetter(parent_name_); + getParentParam(enforce_path_inversion_, "enforce_path_inversion", false); + auto getParam = parameters_handler_->getParamGetter(name_); getParam(consider_footprint_, "consider_footprint", false); getParam(power_, "cost_power", 1); @@ -132,6 +135,8 @@ void CostCritic::score(CriticData & data) return; } + geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_); + // Setup cost information for various parts of the critic is_tracking_unknown_ = costmap_ros_->getLayeredCostmap()->isTrackingUnknown(); auto * costmap = collision_checker_.getCostmap(); @@ -148,7 +153,7 @@ void CostCritic::score(CriticData & data) // If near the goal, don't apply the preferential term since the goal is near obstacles bool near_goal = false; - if (utils::withinPositionGoalTolerance(near_goal_distance_, data.state.pose.pose, data.goal)) { + if (utils::withinPositionGoalTolerance(near_goal_distance_, data.state.pose.pose, goal)) { near_goal = true; } diff --git a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp index aada6ff4920..161e2a4fddf 100644 --- a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp @@ -19,11 +19,12 @@ namespace mppi::critics void GoalAngleCritic::initialize() { - auto getParam = parameters_handler_->getParamGetter(name_); + auto getParentParam = parameters_handler_->getParamGetter(parent_name_); + getParentParam(enforce_path_inversion_, "enforce_path_inversion", false); + auto getParam = parameters_handler_->getParamGetter(name_); getParam(power_, "cost_power", 1); getParam(weight_, "cost_weight", 3.0f); - getParam(threshold_to_consider_, "threshold_to_consider", 0.5f); RCLCPP_INFO( @@ -35,14 +36,19 @@ void GoalAngleCritic::initialize() void GoalAngleCritic::score(CriticData & data) { - if (!enabled_ || !utils::withinPositionGoalTolerance( - threshold_to_consider_, data.state.pose.pose, data.goal)) + if (!enabled_) { + return; + } + + geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_); + + if (!utils::withinPositionGoalTolerance( + threshold_to_consider_, data.state.pose.pose, goal)) { return; } - const auto goal_idx = data.path.x.size() - 1; - const float goal_yaw = data.path.yaws(goal_idx); + double goal_yaw = tf2::getYaw(goal.orientation); if (power_ > 1u) { data.costs += (((utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw).abs()). diff --git a/nav2_mppi_controller/src/critics/goal_critic.cpp b/nav2_mppi_controller/src/critics/goal_critic.cpp index 282ddc62850..7aa5f51aeff 100644 --- a/nav2_mppi_controller/src/critics/goal_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_critic.cpp @@ -21,6 +21,9 @@ namespace mppi::critics void GoalCritic::initialize() { + auto getParentParam = parameters_handler_->getParamGetter(parent_name_); + getParentParam(enforce_path_inversion_, "enforce_path_inversion", false); + auto getParam = parameters_handler_->getParamGetter(name_); getParam(power_, "cost_power", 1); @@ -34,14 +37,20 @@ void GoalCritic::initialize() void GoalCritic::score(CriticData & data) { - if (!enabled_ || !utils::withinPositionGoalTolerance( - threshold_to_consider_, data.state.pose.pose, data.goal)) + if (!enabled_) { + return; + } + + geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_); + + if (!utils::withinPositionGoalTolerance( + threshold_to_consider_, data.state.pose.pose, goal)) { return; } - const auto & goal_x = data.goal.position.x; - const auto & goal_y = data.goal.position.y; + auto goal_x = goal.position.x; + auto goal_y = goal.position.y; const auto delta_x = data.trajectories.x - goal_x; const auto delta_y = data.trajectories.y - goal_y; diff --git a/nav2_mppi_controller/src/critics/obstacles_critic.cpp b/nav2_mppi_controller/src/critics/obstacles_critic.cpp index ee660a75815..deba6009421 100644 --- a/nav2_mppi_controller/src/critics/obstacles_critic.cpp +++ b/nav2_mppi_controller/src/critics/obstacles_critic.cpp @@ -23,6 +23,9 @@ namespace mppi::critics void ObstaclesCritic::initialize() { + auto getParentParam = parameters_handler_->getParamGetter(parent_name_); + getParentParam(enforce_path_inversion_, "enforce_path_inversion1", false); + auto getParam = parameters_handler_->getParamGetter(name_); getParam(consider_footprint_, "consider_footprint", false); getParam(power_, "cost_power", 1); @@ -127,9 +130,11 @@ void ObstaclesCritic::score(CriticData & data) possible_collision_cost_ = findCircumscribedCost(costmap_ros_); } + geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_); + // If near the goal, don't apply the preferential term since the goal is near obstacles bool near_goal = false; - if (utils::withinPositionGoalTolerance(near_goal_distance_, data.state.pose.pose, data.goal)) { + if (utils::withinPositionGoalTolerance(near_goal_distance_, data.state.pose.pose, goal)) { near_goal = true; } diff --git a/nav2_mppi_controller/src/critics/path_align_critic.cpp b/nav2_mppi_controller/src/critics/path_align_critic.cpp index 71b05e0a7da..03da09b7bb5 100644 --- a/nav2_mppi_controller/src/critics/path_align_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_align_critic.cpp @@ -19,6 +19,9 @@ namespace mppi::critics void PathAlignCritic::initialize() { + auto getParentParam = parameters_handler_->getParamGetter(parent_name_); + getParentParam(enforce_path_inversion_, "enforce_path_inversion", false); + auto getParam = parameters_handler_->getParamGetter(name_); getParam(power_, "cost_power", 1); getParam(weight_, "cost_weight", 10.0f); @@ -39,9 +42,15 @@ void PathAlignCritic::initialize() void PathAlignCritic::score(CriticData & data) { + if (!enabled_) { + return; + } + + geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_); + // Don't apply close to goal, let the goal critics take over - if (!enabled_ || utils::withinPositionGoalTolerance( - threshold_to_consider_, data.state.pose.pose, data.goal)) + if (utils::withinPositionGoalTolerance( + threshold_to_consider_, data.state.pose.pose, goal)) { return; } diff --git a/nav2_mppi_controller/src/critics/path_angle_critic.cpp b/nav2_mppi_controller/src/critics/path_angle_critic.cpp index a312b1e50ae..1374c8e749d 100644 --- a/nav2_mppi_controller/src/critics/path_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_angle_critic.cpp @@ -23,6 +23,7 @@ namespace mppi::critics void PathAngleCritic::initialize() { auto getParentParam = parameters_handler_->getParamGetter(parent_name_); + getParentParam(enforce_path_inversion_, "enforce_path_inversion", false); float vx_min; getParentParam(vx_min, "vx_min", -0.35); if (fabs(vx_min) < 1e-6f) { // zero @@ -61,8 +62,14 @@ void PathAngleCritic::initialize() void PathAngleCritic::score(CriticData & data) { - if (!enabled_ || - utils::withinPositionGoalTolerance(threshold_to_consider_, data.state.pose.pose, data.goal)) + if (!enabled_) { + return; + } + + geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_); + + if (utils::withinPositionGoalTolerance( + threshold_to_consider_, data.state.pose.pose, goal)) { return; } diff --git a/nav2_mppi_controller/src/critics/path_follow_critic.cpp b/nav2_mppi_controller/src/critics/path_follow_critic.cpp index 56059cc2811..4f8c1c70eec 100644 --- a/nav2_mppi_controller/src/critics/path_follow_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_follow_critic.cpp @@ -21,6 +21,9 @@ namespace mppi::critics void PathFollowCritic::initialize() { + auto getParentParam = parameters_handler_->getParamGetter(parent_name_); + getParentParam(enforce_path_inversion_, "enforce_path_inversion", false); + auto getParam = parameters_handler_->getParamGetter(name_); getParam( @@ -33,8 +36,15 @@ void PathFollowCritic::initialize() void PathFollowCritic::score(CriticData & data) { - if (!enabled_ || data.path.x.size() < 2 || - utils::withinPositionGoalTolerance(threshold_to_consider_, data.state.pose.pose, data.goal)) + if (!enabled_) { + return; + } + + geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_); + + if (data.path.x.size() < 2 || + utils::withinPositionGoalTolerance( + threshold_to_consider_, data.state.pose.pose, goal)) { return; } diff --git a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp index b5fb3880c14..a220cbe1fd9 100644 --- a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp +++ b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp @@ -21,6 +21,9 @@ namespace mppi::critics void PreferForwardCritic::initialize() { + auto getParentParam = parameters_handler_->getParamGetter(parent_name_); + getParentParam(enforce_path_inversion_, "enforce_path_inversion", false); + auto getParam = parameters_handler_->getParamGetter(name_); getParam(power_, "cost_power", 1); getParam(weight_, "cost_weight", 5.0f); @@ -34,8 +37,14 @@ void PreferForwardCritic::initialize() void PreferForwardCritic::score(CriticData & data) { - if (!enabled_ || utils::withinPositionGoalTolerance( - threshold_to_consider_, data.state.pose.pose, data.goal)) + if (!enabled_) { + return; + } + + geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_); + + if (utils::withinPositionGoalTolerance( + threshold_to_consider_, data.state.pose.pose, goal)) { return; } diff --git a/nav2_mppi_controller/src/critics/twirling_critic.cpp b/nav2_mppi_controller/src/critics/twirling_critic.cpp index 06b7b70fc5f..c9cfc79233c 100644 --- a/nav2_mppi_controller/src/critics/twirling_critic.cpp +++ b/nav2_mppi_controller/src/critics/twirling_critic.cpp @@ -21,6 +21,9 @@ namespace mppi::critics void TwirlingCritic::initialize() { + auto getParentParam = parameters_handler_->getParamGetter(parent_name_); + getParentParam(enforce_path_inversion_, "enforce_path_inversion", false); + auto getParam = parameters_handler_->getParamGetter(name_); getParam(power_, "cost_power", 1); @@ -32,8 +35,14 @@ void TwirlingCritic::initialize() void TwirlingCritic::score(CriticData & data) { - if (!enabled_ || - utils::withinPositionGoalTolerance(data.goal_checker, data.state.pose.pose, data.goal)) + if (!enabled_) { + return; + } + + geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_); + + if (utils::withinPositionGoalTolerance( + data.goal_checker, data.state.pose.pose, goal)) { return; } diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp index 563009976e3..f02376af598 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -240,6 +240,11 @@ TEST(CriticTests, GoalAngleCritic) path.y(9) = 0.0; path.yaws(9) = 3.14; goal.position.x = 10.0; + goal.position.y = 0.0; + goal.orientation.x = 0.0; + goal.orientation.y = 0.0; + goal.orientation.z = 1.0; + goal.orientation.w = 0.0; critic.score(data); EXPECT_NEAR(costs.sum(), 0, 1e-6); diff --git a/nav2_mppi_controller/test/utils_test.cpp b/nav2_mppi_controller/test/utils_test.cpp index 9c5903fccd8..5734db444e4 100644 --- a/nav2_mppi_controller/test/utils_test.cpp +++ b/nav2_mppi_controller/test/utils_test.cpp @@ -633,6 +633,83 @@ TEST(UtilsTests, toTrajectoryMsgTest) EXPECT_EQ(trajectory_msg->points[4].time_from_start, rclcpp::Duration(4, 0)); } +TEST(UtilsTests, getLastPathPoseTest) +{ + nav_msgs::msg::Path path; + path.poses.resize(10); + path.poses[9].pose.position.x = 5.0; + path.poses[9].pose.position.y = 50.0; + path.poses[9].pose.orientation.x = 0.0; + path.poses[9].pose.orientation.y = 0.0; + path.poses[9].pose.orientation.z = 1.0; + path.poses[9].pose.orientation.w = 0.0; + + models::Path path_t = toTensor(path); + geometry_msgs::msg::Pose last_path_pose = utils::getLastPathPose(path_t); + + EXPECT_EQ(last_path_pose.position.x, 5); + EXPECT_EQ(last_path_pose.position.y, 50); + EXPECT_NEAR(last_path_pose.orientation.x, 0.0, 1e-3); + EXPECT_NEAR(last_path_pose.orientation.y, 0.0, 1e-3); + EXPECT_NEAR(last_path_pose.orientation.z, 1.0, 1e-3); + EXPECT_NEAR(last_path_pose.orientation.w, 0.0, 1e-3); +} + +TEST(UtilsTests, getCriticGoalTest) +{ + geometry_msgs::msg::Pose pose; + pose.position.x = 10.0; + pose.position.y = 1.0; + + nav_msgs::msg::Path path; + path.poses.resize(10); + path.poses[9].pose.position.x = 5.0; + path.poses[9].pose.position.y = 50.0; + path.poses[9].pose.orientation.x = 0.0; + path.poses[9].pose.orientation.y = 0.0; + path.poses[9].pose.orientation.z = 1.0; + path.poses[9].pose.orientation.w = 0.0; + + geometry_msgs::msg::Pose goal; + goal.position.x = 6.0; + goal.position.y = 60.0; + goal.orientation.x = 0.0; + goal.orientation.y = 0.0; + goal.orientation.z = 0.0; + goal.orientation.w = 1.0; + + // Create CriticData with state and goal initialized + models::State state; + state.pose.pose = pose; + models::Trajectories generated_trajectories; + models::Path path_t = toTensor(path); + Eigen::ArrayXf costs; + float model_dt; + CriticData data = { + state, generated_trajectories, path_t, goal, + costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; + + bool enforce_path_inversion = true; + geometry_msgs::msg::Pose target_goal = utils::getCriticGoal(data, enforce_path_inversion); + + EXPECT_EQ(target_goal.position.x, 5); + EXPECT_EQ(target_goal.position.y, 50); + EXPECT_NEAR(target_goal.orientation.x, 0.0, 1e-3); + EXPECT_NEAR(target_goal.orientation.y, 0.0, 1e-3); + EXPECT_NEAR(target_goal.orientation.z, 1.0, 1e-3); + EXPECT_NEAR(target_goal.orientation.w, 0.0, 1e-3); + + enforce_path_inversion = false; + target_goal = utils::getCriticGoal(data, enforce_path_inversion); + + EXPECT_EQ(target_goal.position.x, 6); + EXPECT_EQ(target_goal.position.y, 60); + EXPECT_NEAR(target_goal.orientation.x, 0.0, 1e-3); + EXPECT_NEAR(target_goal.orientation.y, 0.0, 1e-3); + EXPECT_NEAR(target_goal.orientation.z, 0.0, 1e-3); + EXPECT_NEAR(target_goal.orientation.w, 1.0, 1e-3); +} + int main(int argc, char **argv) { ::testing::InitGoogleTest(&argc, argv); From 83c9c71320c4e63f57b7c5e4136d070da69ea1fb Mon Sep 17 00:00:00 2001 From: Jad Haj Mustafa Date: Mon, 19 May 2025 21:07:21 +0200 Subject: [PATCH 11/46] Add stateful to regulated pure pursuit controller (#5167) * add_stateful_to_regulated_pure_pursuit_controller Signed-off-by: Jad haj mustafa * fix naming Signed-off-by: Jad haj mustafa * fix naming left over Signed-off-by: Jad haj mustafa * resolve comments Signed-off-by: Jad haj mustafa * typo Signed-off-by: Jad haj mustafa * add unit test for both cases stateful and not stateful Signed-off-by: Jad haj mustafa --------- Signed-off-by: Jad haj mustafa --- .../parameter_handler.hpp | 1 + .../regulated_pure_pursuit_controller.hpp | 1 + .../src/parameter_handler.cpp | 5 +++ .../src/regulated_pure_pursuit_controller.cpp | 19 ++++++++++-- .../test/test_regulated_pp.cpp | 31 ++++++++++++++++++- 5 files changed, 54 insertions(+), 3 deletions(-) diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp index 765a0e85dc0..da5950d9736 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp @@ -61,6 +61,7 @@ struct Parameters bool interpolate_curvature_after_goal; bool use_collision_detection; double transform_tolerance; + bool stateful; }; /** diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp index df3e5ea587e..2d766ea967f 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp @@ -220,6 +220,7 @@ class RegulatedPurePursuitController : public nav2_core::Controller bool cancelling_ = false; bool finished_cancelling_ = false; bool is_rotating_to_heading_ = false; + bool has_reached_xy_tolerance_ = false; std::shared_ptr> global_path_pub_; std::shared_ptr> diff --git a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp index 984d3f31af6..91e151d1534 100644 --- a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp @@ -101,6 +101,8 @@ ParameterHandler::ParameterHandler( declare_parameter_if_not_declared( node, plugin_name_ + ".use_collision_detection", rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".stateful", rclcpp::ParameterValue(true)); node->get_parameter(plugin_name_ + ".desired_linear_vel", params_.desired_linear_vel); params_.base_desired_linear_vel = params_.desired_linear_vel; @@ -181,6 +183,7 @@ ParameterHandler::ParameterHandler( node->get_parameter( plugin_name_ + ".use_collision_detection", params_.use_collision_detection); + node->get_parameter(plugin_name_ + ".stateful", params_.stateful); if (params_.inflation_cost_scaling_factor <= 0.0) { RCLCPP_WARN( @@ -307,6 +310,8 @@ ParameterHandler::updateParametersCallback( params_.use_cost_regulated_linear_velocity_scaling = parameter.as_bool(); } else if (name == plugin_name_ + ".use_collision_detection") { params_.use_collision_detection = parameter.as_bool(); + } else if (name == plugin_name_ + ".stateful") { + params_.stateful = parameter.as_bool(); } else if (name == plugin_name_ + ".use_rotate_to_heading") { params_.use_rotate_to_heading = parameter.as_bool(); } else if (name == plugin_name_ + ".use_cancel_deceleration") { diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index 49617ea558e..2f43f427f2f 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -320,8 +320,21 @@ bool RegulatedPurePursuitController::shouldRotateToGoalHeading( const geometry_msgs::msg::PoseStamped & carrot_pose) { // Whether we should rotate robot to goal heading - double dist_to_goal = std::hypot(carrot_pose.pose.position.x, carrot_pose.pose.position.y); - return params_->use_rotate_to_heading && dist_to_goal < goal_dist_tol_; + if (!params_->use_rotate_to_heading) { + return false; + } + + double dist_to_goal = std::hypot( + carrot_pose.pose.position.x, carrot_pose.pose.position.y); + + if (params_->stateful) { + if (!has_reached_xy_tolerance_ && dist_to_goal < goal_dist_tol_) { + has_reached_xy_tolerance_ = true; + } + return has_reached_xy_tolerance_; + } + + return dist_to_goal < goal_dist_tol_; } void RegulatedPurePursuitController::rotateToHeading( @@ -466,6 +479,7 @@ void RegulatedPurePursuitController::applyConstraints( void RegulatedPurePursuitController::setPlan(const nav_msgs::msg::Path & path) { + has_reached_xy_tolerance_ = false; path_handler_->setPlan(path); } @@ -493,6 +507,7 @@ void RegulatedPurePursuitController::reset() { cancelling_ = false; finished_cancelling_ = false; + has_reached_xy_tolerance_ = false; } double RegulatedPurePursuitController::findVelocitySignChange( diff --git a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp index a46f0e741f1..0ca4527c2ee 100644 --- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp +++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp @@ -498,8 +498,14 @@ TEST(RegulatedPurePursuitTest, lookaheadAPI) TEST(RegulatedPurePursuitTest, rotateTests) { + // -------------------------- + // Non-Stateful Configuration + // -------------------------- auto ctrl = std::make_shared(); auto node = std::make_shared("testRPP"); + nav2_util::declare_parameter_if_not_declared( + node, "PathFollower.stateful", rclcpp::ParameterValue(false)); + std::string name = "PathFollower"; auto tf = std::make_shared(node->get_clock()); auto costmap = std::make_shared("fake_costmap"); @@ -565,6 +571,27 @@ TEST(RegulatedPurePursuitTest, rotateTests) curr_speed.angular.z = 1.0; ctrl->rotateToHeadingWrapper(lin_v, ang_v, angle_to_path, curr_speed); EXPECT_NEAR(ang_v, 0.84, 0.01); + + // ----------------------- + // Stateful Configuration + // ----------------------- + node->set_parameter( + rclcpp::Parameter("PathFollower.stateful", true)); + + ctrl->configure(node, name, tf, costmap); + + // Start just outside tolerance + carrot.pose.position.x = 0.0; + carrot.pose.position.y = 0.26; + EXPECT_EQ(ctrl->shouldRotateToGoalHeadingWrapper(carrot), false); + + // Enter tolerance (should set internal flag) + carrot.pose.position.y = 0.24; + EXPECT_EQ(ctrl->shouldRotateToGoalHeadingWrapper(carrot), true); + + // Move outside tolerance again - still expect true (due to persistent state) + carrot.pose.position.y = 0.26; + EXPECT_EQ(ctrl->shouldRotateToGoalHeadingWrapper(carrot), true); } TEST(RegulatedPurePursuitTest, applyConstraints) @@ -705,7 +732,8 @@ TEST(RegulatedPurePursuitTest, testDynamicParameter) rclcpp::Parameter("test.use_cost_regulated_linear_velocity_scaling", false), rclcpp::Parameter("test.inflation_cost_scaling_factor", 1.0), rclcpp::Parameter("test.allow_reversing", false), - rclcpp::Parameter("test.use_rotate_to_heading", false)}); + rclcpp::Parameter("test.use_rotate_to_heading", false), + rclcpp::Parameter("test.stateful", false)}); rclcpp::spin_until_future_complete( node->get_node_base_interface(), @@ -736,6 +764,7 @@ TEST(RegulatedPurePursuitTest, testDynamicParameter) "test.use_cost_regulated_linear_velocity_scaling").as_bool(), false); EXPECT_EQ(node->get_parameter("test.allow_reversing").as_bool(), false); EXPECT_EQ(node->get_parameter("test.use_rotate_to_heading").as_bool(), false); + EXPECT_EQ(node->get_parameter("test.stateful").as_bool(), false); // Should fail auto results2 = rec_param->set_parameters_atomically( From 8c153ef8f5c5d2a19426197bbb179e3bbe8e8dca Mon Sep 17 00:00:00 2001 From: Tim Clephas Date: Tue, 20 May 2025 18:13:51 +0200 Subject: [PATCH 12/46] tf2 uses hpp headers in rolling (and is backported) (#5180) Signed-off-by: Tim Clephas --- .../plugins/edge_cost_functions/goal_orientation_scorer.hpp | 2 +- .../edge_cost_functions/start_pose_orientation_scorer.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_route/include/nav2_route/plugins/edge_cost_functions/goal_orientation_scorer.hpp b/nav2_route/include/nav2_route/plugins/edge_cost_functions/goal_orientation_scorer.hpp index 562f5b67339..a2886347f8b 100644 --- a/nav2_route/include/nav2_route/plugins/edge_cost_functions/goal_orientation_scorer.hpp +++ b/nav2_route/include/nav2_route/plugins/edge_cost_functions/goal_orientation_scorer.hpp @@ -26,7 +26,7 @@ #include "nav2_util/node_utils.hpp" #include "nav2_costmap_2d/costmap_subscriber.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "tf2/utils.h" +#include "tf2/utils.hpp" #include "angles/angles.h" namespace nav2_route diff --git a/nav2_route/include/nav2_route/plugins/edge_cost_functions/start_pose_orientation_scorer.hpp b/nav2_route/include/nav2_route/plugins/edge_cost_functions/start_pose_orientation_scorer.hpp index 41d92970704..a0b2e132d02 100644 --- a/nav2_route/include/nav2_route/plugins/edge_cost_functions/start_pose_orientation_scorer.hpp +++ b/nav2_route/include/nav2_route/plugins/edge_cost_functions/start_pose_orientation_scorer.hpp @@ -27,7 +27,7 @@ #include "nav2_util/robot_utils.hpp" #include "nav2_costmap_2d/costmap_subscriber.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "tf2/utils.h" +#include "tf2/utils.hpp" #include "angles/angles.h" namespace nav2_route From 4783190270f02b26ce1850dab248608e0f85c543 Mon Sep 17 00:00:00 2001 From: Raman <52356167+RamanRobotics@users.noreply.github.com> Date: Tue, 20 May 2025 21:48:24 +0530 Subject: [PATCH 13/46] added config for laserscan in lb-sim (#5174) * added config for laserscan in lb-sim Signed-off-by: RamanaBotta * fixing ament_flake8 errors Signed-off-by: RamanaBotta * review: use_inf is default:true and added parameters on readme #4992 Signed-off-by: RamanaBotta * refactor: meaningfull value for scan_angle_increment Signed-off-by: RamanaBotta --------- Signed-off-by: RamanaBotta Co-authored-by: RamanaBotta --- nav2_bringup/params/nav2_params.yaml | 6 +++ nav2_loopback_sim/README.md | 7 +++ .../nav2_loopback_sim/loopback_simulator.py | 46 ++++++++++++++++--- 3 files changed, 52 insertions(+), 7 deletions(-) diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index d9cf87e5cb0..bf148f60b3d 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -565,3 +565,9 @@ loopback_simulator: map_frame_id: "map" scan_frame_id: "base_scan" # tb4_loopback_simulator.launch.py remaps to 'rplidar_link' update_duration: 0.02 + scan_range_min: 0.05 + scan_range_max: 30.0 + scan_angle_min: -3.1415 + scan_angle_max: 3.1415 + scan_angle_increment: 0.02617 + scan_use_inf: true diff --git a/nav2_loopback_sim/README.md b/nav2_loopback_sim/README.md index fc160d91070..62104e05e9b 100644 --- a/nav2_loopback_sim/README.md +++ b/nav2_loopback_sim/README.md @@ -42,6 +42,13 @@ ros2 launch nav2_bringup tb4_loopback_simulation.launch.py # Nav2 integrated na - `scan_publish_dur`: : The duration between publishing scan (default 0.1s -- 10hz) - `publish_map_odom_tf`: Whether or not to publish tf from `map_frame_id` to `odom_frame_id` (default `true`) - `publish_clock`: Whether or not to publish simulated clock to `/clock` (default `true`) +- `scan_range_min`: Minimum measurable distance from the scan in meters. Values below this are considered invalid (default: `0.05`) +- `scan_range_max`: Maximum measurable distance from the scan in meters. Values beyond this are out of range (default: `30.0`) +- `scan_angle_min`: Starting angle of the scan in radians (leftmost angle) (default: `-Ï€` / `-3.1415`) +- `scan_angle_max`: Ending angle of the scan in radians (rightmost angle) (default: `Ï€` / `3.1415`) +- `scan_angle_increment`: Angular resolution of the scan in radians (angle between consecutive measurements) (default: `0.02617`) +- `scan_use_inf`: Whether to use `inf` for out-of-range values. If `false`, uses `scan_range_max - 0.1` instead (default: `True`) + ### Topics diff --git a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py index 53d67540214..77a69143a9c 100644 --- a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py +++ b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py @@ -86,6 +86,30 @@ def __init__(self) -> None: self.declare_parameter('publish_clock', True) self.publish_clock = self.get_parameter('publish_clock').get_parameter_value().bool_value + self.declare_parameter('scan_range_min', 0.05) + self.scan_range_min = \ + self.get_parameter('scan_range_min').get_parameter_value().double_value + + self.declare_parameter('scan_range_max', 30.0) + self.scan_range_max = \ + self.get_parameter('scan_range_max').get_parameter_value().double_value + + self.declare_parameter('scan_angle_min', -math.pi) + self.scan_angle_min = \ + self.get_parameter('scan_angle_min').get_parameter_value().double_value + + self.declare_parameter('scan_angle_max', math.pi) + self.scan_angle_max = \ + self.get_parameter('scan_angle_max').get_parameter_value().double_value + + self.declare_parameter('scan_angle_increment', 0.0261) # 0.0261 rad = 1.5 degrees + self.scan_angle_increment = \ + self.get_parameter('scan_angle_increment').get_parameter_value().double_value + + self.declare_parameter('scan_use_inf', True) + self.use_inf = \ + self.get_parameter('scan_use_inf').get_parameter_value().bool_value + self.t_map_to_odom = TransformStamped() self.t_map_to_odom.header.frame_id = self.map_frame_id self.t_map_to_odom.child_frame_id = self.odom_frame_id @@ -235,14 +259,14 @@ def publishLaserScan(self) -> None: self.scan_msg = LaserScan() self.scan_msg.header.stamp = (self.get_clock().now()).to_msg() self.scan_msg.header.frame_id = self.scan_frame_id - self.scan_msg.angle_min = -math.pi - self.scan_msg.angle_max = math.pi + self.scan_msg.angle_min = self.scan_angle_min + self.scan_msg.angle_max = self.scan_angle_max # 1.5 degrees - self.scan_msg.angle_increment = 0.0261799 + self.scan_msg.angle_increment = self.scan_angle_increment self.scan_msg.time_increment = 0.0 self.scan_msg.scan_time = 0.1 - self.scan_msg.range_min = 0.05 - self.scan_msg.range_max = 30.0 + self.scan_msg.range_min = self.scan_range_min + self.scan_msg.range_max = self.scan_range_max num_samples = int( (self.scan_msg.angle_max - self.scan_msg.angle_min) / self.scan_msg.angle_increment) @@ -323,7 +347,10 @@ def getLaserPose(self) -> tuple[float, float, float]: def getLaserScan(self, num_samples: int) -> None: if self.map is None or self.initial_pose is None or self.mat_base_to_laser is None: - self.scan_msg.ranges = [self.scan_msg.range_max - 0.1] * num_samples + if self.use_inf: + self.scan_msg.ranges = [float('inf')] * num_samples + else: + self.scan_msg.ranges = [self.scan_msg.range_max - 0.1] * num_samples return x0, y0, theta = self.getLaserPose() @@ -332,7 +359,10 @@ def getLaserScan(self, num_samples: int) -> None: if not 0 < mx0 < self.map.info.width or not 0 < my0 < self.map.info.height: # outside map - self.scan_msg.ranges = [self.scan_msg.range_max - 0.1] * num_samples + if self.use_inf: + self.scan_msg.ranges = [float('inf')] * num_samples + else: + self.scan_msg.ranges = [self.scan_msg.range_max - 0.1] * num_samples return for i in range(num_samples): @@ -361,6 +391,8 @@ def getLaserScan(self, num_samples: int) -> None: break line_iterator.advance() + if self.scan_msg.ranges[i] == 0.0 and self.use_inf: + self.scan_msg.ranges[i] = float('inf') def main() -> None: From a278acf5332b29da92b8f83deed4a3d4612fba2c Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 20 May 2025 18:42:12 +0200 Subject: [PATCH 14/46] Publish planned footprints after smoothing (#5155) * Publish planned footprints after smoothing Signed-off-by: Tony Najjar * Revert "Publish planned footprints after smoothing" This reverts commit c9b349acc6f784c790c3e0566addc21dcbbea541. * Add smoothed footprints publishing Signed-off-by: Tony Najjar * fix formatting Signed-off-by: Tony Najjar * Fix indentation for publisher creation in SmacPlannerHybrid and SmacPlannerLattice Signed-off-by: Tony Najjar * address PR comments Signed-off-by: Tony Najjar * fixes Signed-off-by: Tony Najjar * fix build error Signed-off-by: Tony Najjar --------- Signed-off-by: Tony Najjar --- .../nav2_smac_planner/smac_planner_hybrid.hpp | 2 + .../smac_planner_lattice.hpp | 2 + nav2_smac_planner/src/smac_planner_hybrid.cpp | 53 +++++++++++++---- .../src/smac_planner_lattice.cpp | 57 +++++++++++++++---- 4 files changed, 92 insertions(+), 22 deletions(-) diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp index 96f40f47664..0b50c0e15fd 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp @@ -128,6 +128,8 @@ class SmacPlannerHybrid : public nav2_core::GlobalPlanner rclcpp_lifecycle::LifecyclePublisher::SharedPtr _raw_plan_publisher; rclcpp_lifecycle::LifecyclePublisher::SharedPtr _planned_footprints_publisher; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr + _smoothed_footprints_publisher; rclcpp_lifecycle::LifecyclePublisher::SharedPtr _expansions_publisher; std::mutex _mutex; diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp index 6db78ddccaf..8cead0a0145 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp @@ -118,6 +118,8 @@ class SmacPlannerLattice : public nav2_core::GlobalPlanner bool _debug_visualizations; rclcpp_lifecycle::LifecyclePublisher::SharedPtr _planned_footprints_publisher; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr + _smoothed_footprints_publisher; rclcpp_lifecycle::LifecyclePublisher::SharedPtr _expansions_publisher; GoalHeadingMode _goal_heading_mode; diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index 22feccffabd..d0e5441ddfd 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -294,6 +294,9 @@ void SmacPlannerHybrid::configure( _expansions_publisher = node->create_publisher("expansions", 1); _planned_footprints_publisher = node->create_publisher( "planned_footprints", 1); + _smoothed_footprints_publisher = + node->create_publisher( + "smoothed_footprints", 1); } RCLCPP_INFO( @@ -314,6 +317,7 @@ void SmacPlannerHybrid::activate() if (_debug_visualizations) { _expansions_publisher->on_activate(); _planned_footprints_publisher->on_activate(); + _smoothed_footprints_publisher->on_activate(); } if (_costmap_downsampler) { _costmap_downsampler->on_activate(); @@ -342,6 +346,7 @@ void SmacPlannerHybrid::deactivate() if (_debug_visualizations) { _expansions_publisher->on_deactivate(); _planned_footprints_publisher->on_deactivate(); + _smoothed_footprints_publisher->on_deactivate(); } if (_costmap_downsampler) { _costmap_downsampler->on_deactivate(); @@ -367,8 +372,11 @@ void SmacPlannerHybrid::cleanup() _costmap_downsampler.reset(); } _raw_plan_publisher.reset(); - _expansions_publisher.reset(); - _planned_footprints_publisher.reset(); + if (_debug_visualizations) { + _expansions_publisher.reset(); + _planned_footprints_publisher.reset(); + _smoothed_footprints_publisher.reset(); + } } nav_msgs::msg::Path SmacPlannerHybrid::createPlan( @@ -522,9 +530,10 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( if (_debug_visualizations) { // Publish expansions for debug + auto now = _clock->now(); geometry_msgs::msg::PoseArray msg; geometry_msgs::msg::Pose msg_pose; - msg.header.stamp = _clock->now(); + msg.header.stamp = now; msg.header.frame_id = _global_frame; for (auto & e : *expansions) { msg_pose.position.x = std::get<0>(e); @@ -534,19 +543,20 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( } _expansions_publisher->publish(msg); - // plot footprint path planned for debug if (_planned_footprints_publisher->get_subscription_count() > 0) { + // Clear all markers first auto marker_array = std::make_unique(); + visualization_msgs::msg::Marker clear_all_marker; + clear_all_marker.action = visualization_msgs::msg::Marker::DELETEALL; + marker_array->markers.push_back(clear_all_marker); + _planned_footprints_publisher->publish(std::move(marker_array)); + + // Publish smoothed footprints for debug + marker_array = std::make_unique(); for (size_t i = 0; i < plan.poses.size(); i++) { const std::vector edge = transformFootprintToEdges(plan.poses[i].pose, _costmap_ros->getRobotFootprint()); - marker_array->markers.push_back(createMarker(edge, i, _global_frame, _clock->now())); - } - - if (marker_array->markers.empty()) { - visualization_msgs::msg::Marker clear_all_marker; - clear_all_marker.action = visualization_msgs::msg::Marker::DELETEALL; - marker_array->markers.push_back(clear_all_marker); + marker_array->markers.push_back(createMarker(edge, i, _global_frame, now)); } _planned_footprints_publisher->publish(std::move(marker_array)); } @@ -574,6 +584,27 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( " milliseconds to smooth path." << std::endl; #endif + if (_debug_visualizations) { + if (_smoothed_footprints_publisher->get_subscription_count() > 0) { + // Clear all markers first + auto marker_array = std::make_unique(); + visualization_msgs::msg::Marker clear_all_marker; + clear_all_marker.action = visualization_msgs::msg::Marker::DELETEALL; + marker_array->markers.push_back(clear_all_marker); + _smoothed_footprints_publisher->publish(std::move(marker_array)); + + // Publish smoothed footprints for debug + marker_array = std::make_unique(); + auto now = _clock->now(); + for (size_t i = 0; i < plan.poses.size(); i++) { + const std::vector edge = + transformFootprintToEdges(plan.poses[i].pose, _costmap_ros->getRobotFootprint()); + marker_array->markers.push_back(createMarker(edge, i, _global_frame, now)); + } + _smoothed_footprints_publisher->publish(std::move(marker_array)); + } + } + return plan; } diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index b771c2cd3dd..d86d245d8e0 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -56,7 +56,6 @@ void SmacPlannerLattice::configure( _costmap_ros = costmap_ros; _name = name; _global_frame = costmap_ros->getGlobalFrameID(); - _raw_plan_publisher = node->create_publisher("unsmoothed_plan", 1); RCLCPP_INFO(_logger, "Configuring %s of type SmacPlannerLattice", name.c_str()); @@ -238,10 +237,15 @@ void SmacPlannerLattice::configure( _smoother->initialize(_metadata.min_turning_radius); } + _raw_plan_publisher = node->create_publisher("unsmoothed_plan", 1); + if (_debug_visualizations) { _expansions_publisher = node->create_publisher("expansions", 1); _planned_footprints_publisher = node->create_publisher( "planned_footprints", 1); + _smoothed_footprints_publisher = + node->create_publisher( + "smoothed_footprints", 1); } RCLCPP_INFO( @@ -262,6 +266,7 @@ void SmacPlannerLattice::activate() if (_debug_visualizations) { _expansions_publisher->on_activate(); _planned_footprints_publisher->on_activate(); + _smoothed_footprints_publisher->on_activate(); } auto node = _node.lock(); // Add callback for dynamic parameters @@ -278,6 +283,7 @@ void SmacPlannerLattice::deactivate() if (_debug_visualizations) { _expansions_publisher->on_deactivate(); _planned_footprints_publisher->on_deactivate(); + _smoothed_footprints_publisher->on_deactivate(); } // shutdown dyn_param_handler auto node = _node.lock(); @@ -296,6 +302,11 @@ void SmacPlannerLattice::cleanup() _a_star.reset(); _smoother.reset(); _raw_plan_publisher.reset(); + if (_debug_visualizations) { + _expansions_publisher.reset(); + _planned_footprints_publisher.reset(); + _smoothed_footprints_publisher.reset(); + } } nav_msgs::msg::Path SmacPlannerLattice::createPlan( @@ -390,9 +401,10 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( _tolerance / static_cast(_costmap->getResolution()), cancel_checker, expansions.get())) { if (_debug_visualizations) { + auto now = _clock->now(); geometry_msgs::msg::PoseArray msg; geometry_msgs::msg::Pose msg_pose; - msg.header.stamp = _clock->now(); + msg.header.stamp = now; msg.header.frame_id = _global_frame; for (auto & e : *expansions) { msg_pose.position.x = std::get<0>(e); @@ -441,10 +453,11 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( } if (_debug_visualizations) { + auto now = _clock->now(); // Publish expansions for debug geometry_msgs::msg::PoseArray msg; geometry_msgs::msg::Pose msg_pose; - msg.header.stamp = _clock->now(); + msg.header.stamp = now; msg.header.frame_id = _global_frame; for (auto & e : *expansions) { msg_pose.position.x = std::get<0>(e); @@ -454,19 +467,20 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( } _expansions_publisher->publish(msg); - // plot footprint path planned for debug if (_planned_footprints_publisher->get_subscription_count() > 0) { + // Clear all markers first auto marker_array = std::make_unique(); + visualization_msgs::msg::Marker clear_all_marker; + clear_all_marker.action = visualization_msgs::msg::Marker::DELETEALL; + marker_array->markers.push_back(clear_all_marker); + _planned_footprints_publisher->publish(std::move(marker_array)); + + // Publish smoothed footprints for debug + marker_array = std::make_unique(); for (size_t i = 0; i < plan.poses.size(); i++) { const std::vector edge = transformFootprintToEdges(plan.poses[i].pose, _costmap_ros->getRobotFootprint()); - marker_array->markers.push_back(createMarker(edge, i, _global_frame, _clock->now())); - } - - if (marker_array->markers.empty()) { - visualization_msgs::msg::Marker clear_all_marker; - clear_all_marker.action = visualization_msgs::msg::Marker::DELETEALL; - marker_array->markers.push_back(clear_all_marker); + marker_array->markers.push_back(createMarker(edge, i, _global_frame, now)); } _planned_footprints_publisher->publish(std::move(marker_array)); } @@ -494,6 +508,27 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( " milliseconds to smooth path." << std::endl; #endif + if (_debug_visualizations) { + if (_smoothed_footprints_publisher->get_subscription_count() > 0) { + // Clear all markers first + auto marker_array = std::make_unique(); + visualization_msgs::msg::Marker clear_all_marker; + clear_all_marker.action = visualization_msgs::msg::Marker::DELETEALL; + marker_array->markers.push_back(clear_all_marker); + _smoothed_footprints_publisher->publish(std::move(marker_array)); + + // Publish smoothed footprints for debug + marker_array = std::make_unique(); + auto now = _clock->now(); + for (size_t i = 0; i < plan.poses.size(); i++) { + const std::vector edge = + transformFootprintToEdges(plan.poses[i].pose, _costmap_ros->getRobotFootprint()); + marker_array->markers.push_back(createMarker(edge, i, _global_frame, now)); + } + _smoothed_footprints_publisher->publish(std::move(marker_array)); + } + } + return plan; } From 6edb7e7fad4af1deb110632d128078cfa4537bb1 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 20 May 2025 12:16:12 -0700 Subject: [PATCH 15/46] fixing deprecation warning (#5182) Signed-off-by: Steve Macenski --- nav2_docking/opennav_docking/test/CMakeLists.txt | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/nav2_docking/opennav_docking/test/CMakeLists.txt b/nav2_docking/opennav_docking/test/CMakeLists.txt index 346d8f59afa..149e401b9f6 100644 --- a/nav2_docking/opennav_docking/test/CMakeLists.txt +++ b/nav2_docking/opennav_docking/test/CMakeLists.txt @@ -63,8 +63,11 @@ target_link_libraries(test_simple_charging_dock ament_add_gtest(test_simple_non_charging_dock test_simple_non_charging_dock.cpp ) -ament_target_dependencies(test_simple_non_charging_dock - ${dependencies} +target_link_libraries(test_simple_non_charging_dock + ${geometry_msgs_TARGETS} + ${library_name} + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle ) target_link_libraries(test_simple_non_charging_dock ${library_name} simple_non_charging_dock From bd8b024a4998fe2cb92ff5eb078826578e80923d Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 20 May 2025 15:52:28 -0700 Subject: [PATCH 16/46] Removing action server timeout duration after fixes to ROS 2, Reverts 3787 (#5183) * Removing action server timeout duration after fixes to ROS 2 Signed-off-by: Steve Macenski * fix build warning Signed-off-by: Steve Macenski --------- Signed-off-by: Steve Macenski --- .../nav2_behavior_tree/bt_action_server_impl.hpp | 11 +---------- .../include/nav2_behaviors/timed_behavior.hpp | 11 +---------- nav2_bringup/params/nav2_params.yaml | 2 -- nav2_controller/src/controller_server.cpp | 10 ++-------- nav2_docking/opennav_docking/src/docking_server.cpp | 11 ++--------- nav2_planner/src/planner_server.cpp | 10 ++-------- nav2_smoother/src/nav2_smoother.cpp | 11 ++--------- nav2_system_tests/src/system/nav2_system_params.yaml | 2 -- nav2_waypoint_follower/src/waypoint_follower.cpp | 10 ++-------- 9 files changed, 12 insertions(+), 66 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index 4a6f43b925f..be5a5ea7912 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -66,9 +66,6 @@ BtActionServer::BtActionServer( if (!node->has_parameter("default_server_timeout")) { node->declare_parameter("default_server_timeout", 20); } - if (!node->has_parameter("action_server_result_timeout")) { - node->declare_parameter("action_server_result_timeout", 900.0); - } if (!node->has_parameter("always_reload_bt_xml")) { node->declare_parameter("always_reload_bt_xml", false); } @@ -167,19 +164,13 @@ bool BtActionServer::on_configure() node, "transform_tolerance", rclcpp::ParameterValue(0.1)); nav2_util::copy_all_parameter_values(node, client_node_); - // set the timeout in seconds for the action server to discard goal handles if not finished - double action_server_result_timeout = - node->get_parameter("action_server_result_timeout").as_double(); - rcl_action_server_options_t server_options = rcl_action_server_get_default_options(); - server_options.result_timeout.nanoseconds = RCL_S_TO_NS(action_server_result_timeout); - action_server_ = std::make_shared( node->get_node_base_interface(), node->get_node_clock_interface(), node->get_node_logging_interface(), node->get_node_waitables_interface(), action_name_, std::bind(&BtActionServer::executeCallback, this), - nullptr, std::chrono::milliseconds(500), false, server_options); + nullptr, std::chrono::milliseconds(500), false); // Get parameters for BT timeouts int bt_loop_duration; diff --git a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp index 987575354bd..dee4107ca45 100644 --- a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp +++ b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp @@ -135,19 +135,10 @@ class TimedBehavior : public nav2_core::Behavior node->get_parameter("robot_base_frame", robot_base_frame_); node->get_parameter("transform_tolerance", transform_tolerance_); - if (!node->has_parameter("action_server_result_timeout")) { - node->declare_parameter("action_server_result_timeout", 10.0); - } - - double action_server_result_timeout; - node->get_parameter("action_server_result_timeout", action_server_result_timeout); - rcl_action_server_options_t server_options = rcl_action_server_get_default_options(); - server_options.result_timeout.nanoseconds = RCL_S_TO_NS(action_server_result_timeout); - action_server_ = std::make_shared( node, behavior_name_, std::bind(&TimedBehavior::execute, this), nullptr, std::chrono::milliseconds( - 500), false, server_options); + 500), false); local_collision_checker_ = local_collision_checker; global_collision_checker_ = global_collision_checker; diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index bf148f60b3d..19c1edbf591 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -48,7 +48,6 @@ bt_navigator: filter_duration: 0.3 default_server_timeout: 20 wait_for_service_timeout: 1000 - action_server_result_timeout: 900.0 service_introspection_mode: "disabled" navigators: ["navigate_to_pose", "navigate_through_poses"] navigate_to_pose: @@ -430,7 +429,6 @@ waypoint_follower: loop_rate: 20 stop_on_failure: false service_introspection_mode: "disabled" - action_server_result_timeout: 900.0 waypoint_task_executor_plugin: "wait_at_waypoint" wait_at_waypoint: plugin: "nav2_waypoint_follower::WaitAtWaypoint" diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 06fd66ff364..220dec1d73f 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -51,8 +51,6 @@ ControllerServer::ControllerServer(const rclcpp::NodeOptions & options) declare_parameter("controller_frequency", 20.0); - declare_parameter("action_server_result_timeout", 10.0); - declare_parameter("progress_checker_plugins", default_progress_checker_ids_); declare_parameter("goal_checker_plugins", default_goal_checker_ids_); declare_parameter("controller_plugins", default_ids_); @@ -224,11 +222,6 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) odom_sub_ = std::make_unique(node); vel_publisher_ = std::make_unique(node, "cmd_vel", 1); - double action_server_result_timeout; - get_parameter("action_server_result_timeout", action_server_result_timeout); - rcl_action_server_options_t server_options = rcl_action_server_get_default_options(); - server_options.result_timeout.nanoseconds = RCL_S_TO_NS(action_server_result_timeout); - double costmap_update_timeout_dbl; get_parameter("costmap_update_timeout", costmap_update_timeout_dbl); costmap_update_timeout_ = rclcpp::Duration::from_seconds(costmap_update_timeout_dbl); @@ -242,7 +235,8 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) std::bind(&ControllerServer::computeControl, this), nullptr, std::chrono::milliseconds(500), - true /*spin thread*/, server_options, use_realtime_priority_ /*soft realtime*/); + true /*spin thread*/, rcl_action_server_get_default_options(), + use_realtime_priority_ /*soft realtime*/); } catch (const std::runtime_error & e) { RCLCPP_ERROR(get_logger(), "Error creating action server! %s", e.what()); on_cleanup(state); diff --git a/nav2_docking/opennav_docking/src/docking_server.cpp b/nav2_docking/opennav_docking/src/docking_server.cpp index 4e864d77abc..d20e23401c2 100644 --- a/nav2_docking/opennav_docking/src/docking_server.cpp +++ b/nav2_docking/opennav_docking/src/docking_server.cpp @@ -85,25 +85,18 @@ DockingServer::on_configure(const rclcpp_lifecycle::State & state) get_parameter("odom_topic", odom_topic); odom_sub_ = std::make_unique(node, odom_topic); - double action_server_result_timeout; - nav2_util::declare_parameter_if_not_declared( - node, "action_server_result_timeout", rclcpp::ParameterValue(10.0)); - get_parameter("action_server_result_timeout", action_server_result_timeout); - rcl_action_server_options_t server_options = rcl_action_server_get_default_options(); - server_options.result_timeout.nanoseconds = RCL_S_TO_NS(action_server_result_timeout); - // Create the action servers for dock / undock docking_action_server_ = std::make_unique( node, "dock_robot", std::bind(&DockingServer::dockRobot, this), nullptr, std::chrono::milliseconds(500), - true, server_options); + true); undocking_action_server_ = std::make_unique( node, "undock_robot", std::bind(&DockingServer::undockRobot, this), nullptr, std::chrono::milliseconds(500), - true, server_options); + true); // Create composed utilities mutex_ = std::make_shared(); diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 7d29119f31b..09154e75df4 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -52,7 +52,6 @@ PlannerServer::PlannerServer(const rclcpp::NodeOptions & options) // Declare this node's parameters declare_parameter("planner_plugins", default_ids_); declare_parameter("expected_planner_frequency", 1.0); - declare_parameter("action_server_result_timeout", 10.0); declare_parameter("costmap_update_timeout", 1.0); get_parameter("planner_plugins", planner_ids_); @@ -148,11 +147,6 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & state) // Initialize pubs & subs plan_publisher_ = create_publisher("plan", 1); - double action_server_result_timeout; - get_parameter("action_server_result_timeout", action_server_result_timeout); - rcl_action_server_options_t server_options = rcl_action_server_get_default_options(); - server_options.result_timeout.nanoseconds = RCL_S_TO_NS(action_server_result_timeout); - double costmap_update_timeout_dbl; get_parameter("costmap_update_timeout", costmap_update_timeout_dbl); costmap_update_timeout_ = rclcpp::Duration::from_seconds(costmap_update_timeout_dbl); @@ -164,7 +158,7 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & state) std::bind(&PlannerServer::computePlan, this), nullptr, std::chrono::milliseconds(500), - true, server_options); + true); action_server_poses_ = std::make_unique( shared_from_this(), @@ -172,7 +166,7 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & state) std::bind(&PlannerServer::computePlanThroughPoses, this), nullptr, std::chrono::milliseconds(500), - true, server_options); + true); return nav2_util::CallbackReturn::SUCCESS; } diff --git a/nav2_smoother/src/nav2_smoother.cpp b/nav2_smoother/src/nav2_smoother.cpp index af3697d8856..b09b3171eb4 100644 --- a/nav2_smoother/src/nav2_smoother.cpp +++ b/nav2_smoother/src/nav2_smoother.cpp @@ -53,8 +53,6 @@ SmootherServer::SmootherServer(const rclcpp::NodeOptions & options) rclcpp::ParameterValue(std::string("base_link"))); declare_parameter("transform_tolerance", rclcpp::ParameterValue(0.1)); declare_parameter("smoother_plugins", default_ids_); - - declare_parameter("action_server_result_timeout", 10.0); } SmootherServer::~SmootherServer() @@ -85,7 +83,7 @@ SmootherServer::on_configure(const rclcpp_lifecycle::State & state) transform_listener_ = std::make_shared(*tf_); std::string costmap_topic, footprint_topic, robot_base_frame; - double transform_tolerance; + double transform_tolerance = 0.1; this->get_parameter("costmap_topic", costmap_topic); this->get_parameter("footprint_topic", footprint_topic); this->get_parameter("transform_tolerance", transform_tolerance); @@ -107,11 +105,6 @@ SmootherServer::on_configure(const rclcpp_lifecycle::State & state) // Initialize pubs & subs plan_publisher_ = create_publisher("plan_smoothed", 1); - double action_server_result_timeout; - get_parameter("action_server_result_timeout", action_server_result_timeout); - rcl_action_server_options_t server_options = rcl_action_server_get_default_options(); - server_options.result_timeout.nanoseconds = RCL_S_TO_NS(action_server_result_timeout); - // Create the action server that we implement with our smoothPath method action_server_ = std::make_unique( shared_from_this(), @@ -119,7 +112,7 @@ SmootherServer::on_configure(const rclcpp_lifecycle::State & state) std::bind(&SmootherServer::smoothPlan, this), nullptr, std::chrono::milliseconds(500), - true, server_options); + true); return nav2_util::CallbackReturn::SUCCESS; } diff --git a/nav2_system_tests/src/system/nav2_system_params.yaml b/nav2_system_tests/src/system/nav2_system_params.yaml index 311081aaa26..3ee779d90a0 100644 --- a/nav2_system_tests/src/system/nav2_system_params.yaml +++ b/nav2_system_tests/src/system/nav2_system_params.yaml @@ -46,7 +46,6 @@ bt_navigator: bt_loop_duration: 10 filter_duration: 0.3 default_server_timeout: 20 - action_server_result_timeout: 900.0 navigators: ["navigate_to_pose", "navigate_through_poses"] navigate_to_pose: plugin: "nav2_bt_navigator::NavigateToPoseNavigator" @@ -291,7 +290,6 @@ waypoint_follower: ros__parameters: loop_rate: 20 stop_on_failure: false - action_server_result_timeout: 900.0 waypoint_task_executor_plugin: "wait_at_waypoint" wait_at_waypoint: plugin: "nav2_waypoint_follower::WaitAtWaypoint" diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index dcf025aa3ce..099d0f98c9c 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -37,8 +37,6 @@ WaypointFollower::WaypointFollower(const rclcpp::NodeOptions & options) declare_parameter("stop_on_failure", true); declare_parameter("loop_rate", 20); - declare_parameter("action_server_result_timeout", 900.0); - declare_parameter("global_frame_id", "map"); nav2_util::declare_parameter_if_not_declared( @@ -78,10 +76,6 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & state) get_node_waitables_interface(), "navigate_to_pose", callback_group_); - double action_server_result_timeout = get_parameter("action_server_result_timeout").as_double(); - rcl_action_server_options_t server_options = rcl_action_server_get_default_options(); - server_options.result_timeout.nanoseconds = RCL_S_TO_NS(action_server_result_timeout); - xyz_action_server_ = std::make_unique( get_node_base_interface(), get_node_clock_interface(), @@ -90,7 +84,7 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & state) "follow_waypoints", std::bind( &WaypointFollower::followWaypointsCallback, this), nullptr, std::chrono::milliseconds( - 500), false, server_options); + 500), false); from_ll_to_map_client_ = std::make_unique< nav2_util::ServiceClient Date: Tue, 20 May 2025 18:02:29 -0700 Subject: [PATCH 17/46] action timeout in BT client edits error code and string (#5184) Signed-off-by: Steve Macenski --- .../include/nav2_behavior_tree/bt_action_node.hpp | 11 +++++++++++ .../plugins/action/assisted_teleop_action.hpp | 6 ++++++ .../plugins/action/back_up_action.hpp | 6 ++++++ .../plugins/action/compute_and_track_route_action.hpp | 6 ++++++ .../action/compute_path_through_poses_action.hpp | 6 ++++++ .../plugins/action/compute_path_to_pose_action.hpp | 6 ++++++ .../plugins/action/compute_route_action.hpp | 6 ++++++ .../plugins/action/drive_on_heading_action.hpp | 6 ++++++ .../plugins/action/follow_path_action.hpp | 6 ++++++ .../plugins/action/navigate_through_poses_action.hpp | 6 ++++++ .../plugins/action/navigate_to_pose_action.hpp | 6 ++++++ .../plugins/action/smooth_path_action.hpp | 6 ++++++ .../nav2_behavior_tree/plugins/action/spin_action.hpp | 6 ++++++ .../nav2_behavior_tree/plugins/action/wait_action.hpp | 6 ++++++ .../plugins/action/assisted_teleop_action.cpp | 6 ++++++ nav2_behavior_tree/plugins/action/back_up_action.cpp | 6 ++++++ .../plugins/action/compute_and_track_route_action.cpp | 6 ++++++ .../action/compute_path_through_poses_action.cpp | 6 ++++++ .../plugins/action/compute_path_to_pose_action.cpp | 6 ++++++ .../plugins/action/compute_route_action.cpp | 6 ++++++ .../plugins/action/drive_on_heading_action.cpp | 6 ++++++ .../plugins/action/follow_path_action.cpp | 6 ++++++ .../plugins/action/navigate_through_poses_action.cpp | 5 +++++ .../plugins/action/navigate_to_pose_action.cpp | 6 ++++++ .../plugins/action/smooth_path_action.cpp | 6 ++++++ nav2_behavior_tree/plugins/action/spin_action.cpp | 6 ++++++ nav2_behavior_tree/plugins/action/wait_action.cpp | 6 ++++++ .../include/opennav_docking_bt/dock_robot.hpp | 6 ++++++ .../include/opennav_docking_bt/undock_robot.hpp | 6 ++++++ nav2_docking/opennav_docking_bt/src/dock_robot.cpp | 6 ++++++ nav2_docking/opennav_docking_bt/src/undock_robot.cpp | 6 ++++++ nav2_msgs/action/DockRobot.action | 1 + nav2_msgs/action/NavigateThroughPoses.action | 1 + nav2_msgs/action/NavigateToPose.action | 1 + nav2_msgs/action/UndockRobot.action | 1 + nav2_msgs/action/Wait.action | 1 + 36 files changed, 195 insertions(+) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp index 32e65a7b5b7..c99cbf2b9ac 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp @@ -187,6 +187,15 @@ class BtActionNode : public BT::ActionNodeBase return BT::NodeStatus::SUCCESS; } + /** + * @brief Function to perform work in a BT Node when the action server times out + * Such as setting the error code ID status to timed out for action clients. + */ + virtual void on_timeout() + { + return; + } + /** * @brief The main override required by a BT action * @return BT::NodeStatus Status of tick execution @@ -231,6 +240,7 @@ class BtActionNode : public BT::ActionNodeBase "Timed out while waiting for action server to acknowledge goal request for %s", action_name_.c_str()); future_goal_handle_.reset(); + on_timeout(); return BT::NodeStatus::FAILURE; } } @@ -261,6 +271,7 @@ class BtActionNode : public BT::ActionNodeBase "Timed out while waiting for action server to acknowledge goal request for %s", action_name_.c_str()); future_goal_handle_.reset(); + on_timeout(); return BT::NodeStatus::FAILURE; } } diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp index f3f7e0794c0..f4a740abc84 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp @@ -66,6 +66,12 @@ class AssistedTeleopAction : public BtActionNode */ BT::NodeStatus on_cancelled() override; + /** + * @brief Function to perform work in a BT Node when the action server times out + * Such as setting the error code ID status to timed out for action clients. + */ + void on_timeout() override; + /** * @brief Function to read parameters and initialize class variables */ diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_and_track_route_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_and_track_route_action.hpp index c56da81de5d..6d258552a7f 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_and_track_route_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_and_track_route_action.hpp @@ -64,6 +64,12 @@ class ComputeAndTrackRouteAction : public BtActionNode */ BT::NodeStatus on_cancelled() override; + /** + * @brief Function to perform work in a BT Node when the action server times out + * Such as setting the error code ID status to timed out for action clients. + */ + void on_timeout() override; + /** * \brief Override required by the a BT action. Cancel the action and set the path output */ diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp index 55a30e74c3b..2a2ac3948ca 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp @@ -88,6 +88,12 @@ class DriveOnHeadingAction : public BtActionNode */ BT::NodeStatus on_cancelled() override; + /** + * @brief Function to perform work in a BT Node when the action server times out + * Such as setting the error code ID status to timed out for action clients. + */ + void on_timeout() override; + /** * @brief Function to perform some user-defined operation after a timeout * waiting for a result that hasn't been received yet diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp index fa175002a15..71c6597683f 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp @@ -67,6 +67,12 @@ class NavigateThroughPosesAction : public BtActionNode */ void on_tick() override; + /** + * @brief Function to perform work in a BT Node when the action server times out + * Such as setting the error code ID status to timed out for action clients. + */ + void on_timeout() override; + /** * @brief Function to read parameters and initialize class variables */ diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_action.hpp index f4e65a7bf82..866dc130901 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_action.hpp @@ -50,6 +50,12 @@ class WaitAction : public BtActionNode */ void on_tick() override; + /** + * @brief Function to perform work in a BT Node when the action server times out + * Such as setting the error code ID status to timed out for action clients. + */ + void on_timeout() override; + /** * @brief Function to read parameters and initialize class variables */ diff --git a/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp b/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp index 2c95dda33a7..3f293f588bf 100644 --- a/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp +++ b/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp @@ -69,6 +69,12 @@ BT::NodeStatus AssistedTeleopAction::on_cancelled() return BT::NodeStatus::SUCCESS; } +void AssistedTeleopAction::on_timeout() +{ + setOutput("error_code_id", ActionResult::TIMEOUT); + setOutput("error_msg", "Behavior Tree action client timed out waiting."); +} + } // namespace nav2_behavior_tree #include "behaviortree_cpp/bt_factory.h" diff --git a/nav2_behavior_tree/plugins/action/back_up_action.cpp b/nav2_behavior_tree/plugins/action/back_up_action.cpp index 5e29107c9e4..d31ff9aecb1 100644 --- a/nav2_behavior_tree/plugins/action/back_up_action.cpp +++ b/nav2_behavior_tree/plugins/action/back_up_action.cpp @@ -78,6 +78,12 @@ BT::NodeStatus BackUpAction::on_cancelled() return BT::NodeStatus::SUCCESS; } +void BackUpAction::on_timeout() +{ + setOutput("error_code_id", ActionResult::TIMEOUT); + setOutput("error_msg", "Behavior Tree action client timed out waiting."); +} + } // namespace nav2_behavior_tree #include "behaviortree_cpp/bt_factory.h" diff --git a/nav2_behavior_tree/plugins/action/compute_and_track_route_action.cpp b/nav2_behavior_tree/plugins/action/compute_and_track_route_action.cpp index cc6db6fbe8b..eb2edc19b76 100644 --- a/nav2_behavior_tree/plugins/action/compute_and_track_route_action.cpp +++ b/nav2_behavior_tree/plugins/action/compute_and_track_route_action.cpp @@ -75,6 +75,12 @@ BT::NodeStatus ComputeAndTrackRouteAction::on_cancelled() return BT::NodeStatus::SUCCESS; } +void ComputeAndTrackRouteAction::on_timeout() +{ + setOutput("error_code_id", ActionResult::TIMEOUT); + setOutput("error_msg", "Behavior Tree action client timed out waiting."); +} + void ComputeAndTrackRouteAction::on_wait_for_result( std::shared_ptr/*feedback*/) { diff --git a/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp b/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp index 3b778903df5..72a62258599 100644 --- a/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp +++ b/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp @@ -66,6 +66,12 @@ BT::NodeStatus ComputePathThroughPosesAction::on_cancelled() return BT::NodeStatus::SUCCESS; } +void ComputePathThroughPosesAction::on_timeout() +{ + setOutput("error_code_id", ActionResult::TIMEOUT); + setOutput("error_msg", "Behavior Tree action client timed out waiting."); +} + void ComputePathThroughPosesAction::halt() { nav_msgs::msg::Path empty_path; diff --git a/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp b/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp index 41772aca79b..0d80527c913 100644 --- a/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp +++ b/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp @@ -82,6 +82,12 @@ BT::NodeStatus ComputePathToPoseAction::on_cancelled() return BT::NodeStatus::SUCCESS; } +void ComputePathToPoseAction::on_timeout() +{ + setOutput("error_code_id", ActionResult::TIMEOUT); + setOutput("error_msg", "Behavior Tree action client timed out waiting."); +} + void ComputePathToPoseAction::halt() { nav_msgs::msg::Path empty_path; diff --git a/nav2_behavior_tree/plugins/action/compute_route_action.cpp b/nav2_behavior_tree/plugins/action/compute_route_action.cpp index cc16125c74b..6613d3e7b91 100644 --- a/nav2_behavior_tree/plugins/action/compute_route_action.cpp +++ b/nav2_behavior_tree/plugins/action/compute_route_action.cpp @@ -87,6 +87,12 @@ BT::NodeStatus ComputeRouteAction::on_cancelled() return BT::NodeStatus::SUCCESS; } +void ComputeRouteAction::on_timeout() +{ + setOutput("error_code_id", ActionResult::TIMEOUT); + setOutput("error_msg", "Behavior Tree action client timed out waiting."); +} + void ComputeRouteAction::halt() { resetPorts(); diff --git a/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp b/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp index e3a0fc02765..b612c16f86b 100644 --- a/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp +++ b/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp @@ -76,6 +76,12 @@ BT::NodeStatus DriveOnHeadingAction::on_cancelled() return BT::NodeStatus::SUCCESS; } +void DriveOnHeadingAction::on_timeout() +{ + setOutput("error_code_id", ActionResult::TIMEOUT); + setOutput("error_msg", "Behavior Tree action client timed out waiting."); +} + } // namespace nav2_behavior_tree #include "behaviortree_cpp/bt_factory.h" diff --git a/nav2_behavior_tree/plugins/action/follow_path_action.cpp b/nav2_behavior_tree/plugins/action/follow_path_action.cpp index 875c1b71c44..e89a23e282a 100644 --- a/nav2_behavior_tree/plugins/action/follow_path_action.cpp +++ b/nav2_behavior_tree/plugins/action/follow_path_action.cpp @@ -58,6 +58,12 @@ BT::NodeStatus FollowPathAction::on_cancelled() return BT::NodeStatus::SUCCESS; } +void FollowPathAction::on_timeout() +{ + setOutput("error_code_id", ActionResult::CONTROLLER_TIMED_OUT); + setOutput("error_msg", "Behavior Tree action client timed out waiting."); +} + void FollowPathAction::on_wait_for_result( std::shared_ptr/*feedback*/) { diff --git a/nav2_behavior_tree/plugins/action/navigate_through_poses_action.cpp b/nav2_behavior_tree/plugins/action/navigate_through_poses_action.cpp index be6272317ab..3b362d55300 100644 --- a/nav2_behavior_tree/plugins/action/navigate_through_poses_action.cpp +++ b/nav2_behavior_tree/plugins/action/navigate_through_poses_action.cpp @@ -61,6 +61,11 @@ BT::NodeStatus NavigateThroughPosesAction::on_cancelled() return BT::NodeStatus::SUCCESS; } +void NavigateThroughPosesAction::on_timeout() +{ + setOutput("error_code_id", ActionResult::TIMEOUT); + setOutput("error_msg", "Behavior Tree action client timed out waiting."); +} } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp b/nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp index 4bfac9e8829..775314babc4 100644 --- a/nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp +++ b/nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp @@ -60,6 +60,12 @@ BT::NodeStatus NavigateToPoseAction::on_cancelled() return BT::NodeStatus::SUCCESS; } +void NavigateToPoseAction::on_timeout() +{ + setOutput("error_code_id", ActionResult::TIMEOUT); + setOutput("error_msg", "Behavior Tree action client timed out waiting."); +} + } // namespace nav2_behavior_tree #include "behaviortree_cpp/bt_factory.h" diff --git a/nav2_behavior_tree/plugins/action/smooth_path_action.cpp b/nav2_behavior_tree/plugins/action/smooth_path_action.cpp index ebd72cbf137..cb420e0508e 100644 --- a/nav2_behavior_tree/plugins/action/smooth_path_action.cpp +++ b/nav2_behavior_tree/plugins/action/smooth_path_action.cpp @@ -65,6 +65,12 @@ BT::NodeStatus SmoothPathAction::on_cancelled() return BT::NodeStatus::SUCCESS; } +void SmoothPathAction::on_timeout() +{ + setOutput("error_code_id", ActionResult::TIMEOUT); + setOutput("error_msg", "Behavior Tree action client timed out waiting."); +} + } // namespace nav2_behavior_tree #include "behaviortree_cpp/bt_factory.h" diff --git a/nav2_behavior_tree/plugins/action/spin_action.cpp b/nav2_behavior_tree/plugins/action/spin_action.cpp index c41d2a97744..f5b1cb07fa9 100644 --- a/nav2_behavior_tree/plugins/action/spin_action.cpp +++ b/nav2_behavior_tree/plugins/action/spin_action.cpp @@ -68,6 +68,12 @@ BT::NodeStatus SpinAction::on_cancelled() return BT::NodeStatus::SUCCESS; } +void SpinAction::on_timeout() +{ + setOutput("error_code_id", ActionResult::TIMEOUT); + setOutput("error_msg", "Behavior Tree action client timed out waiting."); +} + } // namespace nav2_behavior_tree #include "behaviortree_cpp/bt_factory.h" diff --git a/nav2_behavior_tree/plugins/action/wait_action.cpp b/nav2_behavior_tree/plugins/action/wait_action.cpp index 4a357fd1b94..8d22313e04b 100644 --- a/nav2_behavior_tree/plugins/action/wait_action.cpp +++ b/nav2_behavior_tree/plugins/action/wait_action.cpp @@ -73,6 +73,12 @@ BT::NodeStatus WaitAction::on_cancelled() return BT::NodeStatus::SUCCESS; } +void WaitAction::on_timeout() +{ + setOutput("error_code_id", ActionResult::TIMEOUT); + setOutput("error_msg", "Behavior Tree action client timed out waiting."); +} + } // namespace nav2_behavior_tree #include "behaviortree_cpp/bt_factory.h" diff --git a/nav2_docking/opennav_docking_bt/include/opennav_docking_bt/dock_robot.hpp b/nav2_docking/opennav_docking_bt/include/opennav_docking_bt/dock_robot.hpp index 0a07e12e7f7..8f4a4afeff2 100644 --- a/nav2_docking/opennav_docking_bt/include/opennav_docking_bt/dock_robot.hpp +++ b/nav2_docking/opennav_docking_bt/include/opennav_docking_bt/dock_robot.hpp @@ -68,6 +68,12 @@ class DockRobotAction */ BT::NodeStatus on_cancelled() override; + /** + * @brief Function to perform work in a BT Node when the action server times out + * Such as setting the error code ID status to timed out for action clients. + */ + void on_timeout() override; + /** * \brief Override required by the a BT action. Cancel the action and set the path output */ diff --git a/nav2_docking/opennav_docking_bt/include/opennav_docking_bt/undock_robot.hpp b/nav2_docking/opennav_docking_bt/include/opennav_docking_bt/undock_robot.hpp index 8674e477791..d0e78e3611f 100644 --- a/nav2_docking/opennav_docking_bt/include/opennav_docking_bt/undock_robot.hpp +++ b/nav2_docking/opennav_docking_bt/include/opennav_docking_bt/undock_robot.hpp @@ -68,6 +68,12 @@ class UndockRobotAction */ BT::NodeStatus on_cancelled() override; + /** + * @brief Function to perform work in a BT Node when the action server times out + * Such as setting the error code ID status to timed out for action clients. + */ + void on_timeout() override; + /** * \brief Override required by the a BT action. Cancel the action and set the path output */ diff --git a/nav2_docking/opennav_docking_bt/src/dock_robot.cpp b/nav2_docking/opennav_docking_bt/src/dock_robot.cpp index be3e58b8c8c..35cea456b4f 100644 --- a/nav2_docking/opennav_docking_bt/src/dock_robot.cpp +++ b/nav2_docking/opennav_docking_bt/src/dock_robot.cpp @@ -67,6 +67,12 @@ BT::NodeStatus DockRobotAction::on_cancelled() return BT::NodeStatus::SUCCESS; } +void DockRobotAction::on_timeout() +{ + setOutput("error_code_id", ActionResult::TIMEOUT); + setOutput("error_msg", "Behavior Tree action client timed out waiting."); +} + void DockRobotAction::halt() { BtActionNode::halt(); diff --git a/nav2_docking/opennav_docking_bt/src/undock_robot.cpp b/nav2_docking/opennav_docking_bt/src/undock_robot.cpp index 0669bad92bd..400615e6f0d 100644 --- a/nav2_docking/opennav_docking_bt/src/undock_robot.cpp +++ b/nav2_docking/opennav_docking_bt/src/undock_robot.cpp @@ -58,6 +58,12 @@ BT::NodeStatus UndockRobotAction::on_cancelled() return BT::NodeStatus::SUCCESS; } +void UndockRobotAction::on_timeout() +{ + setOutput("error_code_id", ActionResult::TIMEOUT); + setOutput("error_msg", "Behavior Tree action client timed out waiting."); +} + void UndockRobotAction::halt() { BtActionNode::halt(); diff --git a/nav2_msgs/action/DockRobot.action b/nav2_msgs/action/DockRobot.action index e3e2fbf78cb..ae70c33413a 100644 --- a/nav2_msgs/action/DockRobot.action +++ b/nav2_msgs/action/DockRobot.action @@ -21,6 +21,7 @@ uint16 FAILED_TO_STAGE=903 uint16 FAILED_TO_DETECT_DOCK=904 uint16 FAILED_TO_CONTROL=905 uint16 FAILED_TO_CHARGE=906 +uint16 TIMEOUT=907 uint16 UNKNOWN=999 bool success True # docking success status diff --git a/nav2_msgs/action/NavigateThroughPoses.action b/nav2_msgs/action/NavigateThroughPoses.action index 8bbaa361e73..1d7a1951578 100644 --- a/nav2_msgs/action/NavigateThroughPoses.action +++ b/nav2_msgs/action/NavigateThroughPoses.action @@ -11,6 +11,7 @@ uint16 NONE=0 uint16 UNKNOWN=9100 uint16 FAILED_TO_LOAD_BEHAVIOR_TREE=9101 uint16 TF_ERROR=9102 +uint16 TIMEOUT=9103 uint16 error_code string error_msg diff --git a/nav2_msgs/action/NavigateToPose.action b/nav2_msgs/action/NavigateToPose.action index 6da35cee510..14272c5663a 100644 --- a/nav2_msgs/action/NavigateToPose.action +++ b/nav2_msgs/action/NavigateToPose.action @@ -10,6 +10,7 @@ uint16 NONE=0 uint16 UNKNOWN=9000 uint16 FAILED_TO_LOAD_BEHAVIOR_TREE=9001 uint16 TF_ERROR=9002 +uint16 TIMEOUT=9003 uint16 error_code string error_msg diff --git a/nav2_msgs/action/UndockRobot.action b/nav2_msgs/action/UndockRobot.action index 61747830607..5bb882a2e41 100644 --- a/nav2_msgs/action/UndockRobot.action +++ b/nav2_msgs/action/UndockRobot.action @@ -16,6 +16,7 @@ float32 max_undocking_time 30.0 # Maximum time to undock uint16 NONE=0 uint16 DOCK_NOT_VALID=902 uint16 FAILED_TO_CONTROL=905 +uint16 TIMEOUT=907 uint16 UNKNOWN=999 bool success True # docking success status diff --git a/nav2_msgs/action/Wait.action b/nav2_msgs/action/Wait.action index 1496e3371ed..c6b20d4f93d 100644 --- a/nav2_msgs/action/Wait.action +++ b/nav2_msgs/action/Wait.action @@ -8,6 +8,7 @@ string error_msg uint16 NONE=0 uint16 UNKNOWN=740 +uint16 TIMEOUT=741 --- #feedback definition builtin_interfaces/Duration time_left From e89b092615125c05a9d9c020afdf8ff4e478a829 Mon Sep 17 00:00:00 2001 From: Nils-Christian Iseke <48475933+Nils-ChristianIseke@users.noreply.github.com> Date: Thu, 22 May 2025 19:37:56 +0200 Subject: [PATCH 18/46] Dynamic Parameters Only validating params that are part of the plugin (#5106) * Only validating params that are part of the plugin Signed-off-by: Nils-ChristianIseke * review Signed-off-by: Nils-Christian Iseke * Refactoring type with param_type and name with param_name to get more consistency. Signed-off-by: Nils-Christian Iseke * Check if plugin_name is part of param_name Signed-off-by: Nils-Christian Iseke * Check if param_name contains name_ Signed-off-by: Nils-Christian Iseke * Uncrustify Signed-off-by: Nils-Christian Iseke * Add check param name in dynamic parameter upate. Signed-off-by: Nils-Christian Iseke * fix Signed-off-by: Nils-Christian Iseke * Merge remote-tracking branch 'upstream/main' into FixNamespaceCheck Signed-off-by: Nils-Christian Iseke * Remove controller_frequency as dynamic parameter Signed-off-by: Nils-Christian Iseke * Revert "Merge remote-tracking branch 'origin/main' into FixNamespaceCheck" This reverts commit 7632e41c21ef53f809228b3d5c04d803a1e292b1, reversing changes made to 19afc9ea8df633f61c801131973f02c212435948. Signed-off-by: Nils-Christian Iseke * Merge remote-tracking branch 'upstream/main' into FixNamespaceCheck Signed-off-by: Nils-Christian Iseke * Fix merge errors. Signed-off-by: Nils-Christian Iseke * Add missing check to simple_goal_checker Signed-off-by: Nils-Christian Iseke * Handel param_name resolution for smac_planner_hybrid Signed-off-by: Nils-Christian Iseke * fix typo Signed-off-by: Nils-Christian Iseke * uncrustify Signed-off-by: Nils-Christian Iseke * fix Signed-off-by: Nils-Christian Iseke * Revert "uncrustify" This reverts commit 43749c272e060fcdb1566bcffb795a162ea62725. Signed-off-by: Nils-Christian Iseke * uncrustify Signed-off-by: Nils-Christian Iseke * Revert "uncrustify" This reverts commit e18f7048d5fbea6bbaec38c3e8e1ed4ea7f19f43. Signed-off-by: Nils-Christian Iseke * Revert "fix" This reverts commit a0a7892f26b8217036b57f498a851ac8760b365f. Signed-off-by: Nils-Christian Iseke * Merge remote-tracking branch 'upstream/main' into FixNamespaceCheck Signed-off-by: Nils-Christian Iseke * MPPI Check Namespace. Signed-off-by: Nils-Christian Iseke * fixing parameter_handler tests. Signed-off-by: Nils-Christian Iseke * Fix optimizer Signed-off-by: Nils-Christian Iseke * Fix indentation Signed-off-by: Nils-Christian Iseke * mppi param handler only execute post_callbacks if a param of mppi was updated. Signed-off-by: Nils-Christian Iseke * Update nav2_rotation_shim_controller.hpp Signed-off-by: Nils-Christian Iseke <48475933+Nils-ChristianIseke@users.noreply.github.com> * Review Signed-off-by: Nils-Christian Iseke * Update nav2_mppi_controller/src/parameters_handler.cpp Signed-off-by: Steve Macenski * Update nav2_mppi_controller/src/parameters_handler.cpp Signed-off-by: Steve Macenski * Update nav2_mppi_controller/src/parameters_handler.cpp Signed-off-by: Steve Macenski --------- Signed-off-by: Nils-ChristianIseke Signed-off-by: Nils-Christian Iseke Signed-off-by: Nils-Christian Iseke <48475933+Nils-ChristianIseke@users.noreply.github.com> Signed-off-by: Steve Macenski Co-authored-by: Steve Macenski --- nav2_amcl/src/amcl_node.cpp | 3 + nav2_collision_monitor/src/polygon.cpp | 4 +- nav2_collision_monitor/src/source.cpp | 4 +- .../plugins/pose_progress_checker.cpp | 11 ++- .../plugins/simple_goal_checker.cpp | 18 ++-- .../plugins/simple_progress_checker.cpp | 13 ++- .../plugins/stopped_goal_checker.cpp | 13 ++- nav2_controller/src/controller_server.cpp | 18 ++-- .../test/test_dynamic_parameters.cpp | 4 +- nav2_costmap_2d/plugins/inflation_layer.cpp | 3 + nav2_costmap_2d/plugins/obstacle_layer.cpp | 3 + .../plugins/plugin_container_layer.cpp | 3 + nav2_costmap_2d/plugins/static_layer.cpp | 3 + nav2_costmap_2d/plugins/voxel_layer.cpp | 3 + nav2_costmap_2d/src/costmap_2d_ros.cpp | 36 +++---- .../opennav_docking/src/controller.cpp | 36 +++---- .../opennav_docking/src/docking_server.cpp | 31 +++--- .../test/test_docking_server_unit.cpp | 1 - .../dwb_plugins/src/kinematic_parameters.cpp | 37 ++++---- .../src/parameter_handler.cpp | 49 +++++----- .../benchmark/optimizer_benchmark.cpp | 3 +- .../tools/parameters_handler.hpp | 5 +- nav2_mppi_controller/src/controller.cpp | 2 +- .../src/parameters_handler.cpp | 36 ++++--- .../test/critic_manager_test.cpp | 6 +- nav2_mppi_controller/test/critics_tests.cpp | 39 +++++--- .../test/motion_model_tests.cpp | 6 +- .../test/noise_generator_test.cpp | 9 +- .../test/optimizer_smoke_test.cpp | 3 +- .../test/optimizer_unit_tests.cpp | 39 +++++--- .../test/parameter_handler_test.cpp | 90 +++++++++--------- .../test/path_handler_test.cpp | 6 +- .../test/trajectory_visualizer_tests.cpp | 15 ++- nav2_navfn_planner/src/navfn_planner.cpp | 20 ++-- nav2_planner/src/planner_server.cpp | 11 ++- .../src/parameter_handler.cpp | 94 +++++++++---------- .../src/nav2_rotation_shim_controller.cpp | 29 +++--- nav2_smac_planner/src/smac_planner_2d.cpp | 34 +++---- nav2_smac_planner/src/smac_planner_hybrid.cpp | 68 +++++++------- .../src/smac_planner_lattice.cpp | 60 ++++++------ .../src/theta_star_planner.cpp | 26 ++--- .../src/velocity_smoother.cpp | 36 +++---- .../src/waypoint_follower.cpp | 15 +-- 43 files changed, 534 insertions(+), 411 deletions(-) diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 5dcc83eb93a..71b50bdcc79 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -1182,6 +1182,9 @@ AmclNode::dynamicParametersCallback( for (auto parameter : parameters) { const auto & param_type = parameter.get_type(); const auto & param_name = parameter.get_name(); + if (param_name.find('.') != std::string::npos) { + continue; + } if (param_type == ParameterType::PARAMETER_DOUBLE) { if (param_name == "alpha1") { diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index 1e087070191..aeda547fd46 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -578,7 +578,9 @@ Polygon::dynamicParametersCallback( for (auto parameter : parameters) { const auto & param_type = parameter.get_type(); const auto & param_name = parameter.get_name(); - + if(param_name.find(polygon_name_ + ".") != 0) { + continue; + } if (param_type == rcl_interfaces::msg::ParameterType::PARAMETER_BOOL) { if (param_name == polygon_name_ + "." + "enabled") { enabled_ = parameter.as_bool(); diff --git a/nav2_collision_monitor/src/source.cpp b/nav2_collision_monitor/src/source.cpp index e6e10b08639..50dc6e9ef96 100644 --- a/nav2_collision_monitor/src/source.cpp +++ b/nav2_collision_monitor/src/source.cpp @@ -121,7 +121,9 @@ Source::dynamicParametersCallback( for (auto parameter : parameters) { const auto & param_type = parameter.get_type(); const auto & param_name = parameter.get_name(); - + if(param_name.find(source_name_ + ".") != 0) { + continue; + } if (param_type == rcl_interfaces::msg::ParameterType::PARAMETER_BOOL) { if (param_name == source_name_ + "." + "enabled") { enabled_ = parameter.as_bool(); diff --git a/nav2_controller/plugins/pose_progress_checker.cpp b/nav2_controller/plugins/pose_progress_checker.cpp index 271e5635c07..1ac4d20e026 100644 --- a/nav2_controller/plugins/pose_progress_checker.cpp +++ b/nav2_controller/plugins/pose_progress_checker.cpp @@ -79,11 +79,14 @@ PoseProgressChecker::dynamicParametersCallback(std::vector pa { rcl_interfaces::msg::SetParametersResult result; for (auto parameter : parameters) { - const auto & type = parameter.get_type(); - const auto & name = parameter.get_name(); + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + if (param_name.find(plugin_name_ + ".") != 0) { + continue; + } - if (type == ParameterType::PARAMETER_DOUBLE) { - if (name == plugin_name_ + ".required_movement_angle") { + if (param_type == ParameterType::PARAMETER_DOUBLE) { + if (param_name == plugin_name_ + ".required_movement_angle") { required_movement_angle_ = parameter.as_double(); } } diff --git a/nav2_controller/plugins/simple_goal_checker.cpp b/nav2_controller/plugins/simple_goal_checker.cpp index d49b94c2873..f4bd8173ac2 100644 --- a/nav2_controller/plugins/simple_goal_checker.cpp +++ b/nav2_controller/plugins/simple_goal_checker.cpp @@ -145,18 +145,20 @@ SimpleGoalChecker::dynamicParametersCallback(std::vector para { rcl_interfaces::msg::SetParametersResult result; for (auto & parameter : parameters) { - const auto & type = parameter.get_type(); - const auto & name = parameter.get_name(); - - if (type == ParameterType::PARAMETER_DOUBLE) { - if (name == plugin_name_ + ".xy_goal_tolerance") { + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + if (param_name.find(plugin_name_ + ".") != 0) { + continue; + } + if (param_type == ParameterType::PARAMETER_DOUBLE) { + if (param_name == plugin_name_ + ".xy_goal_tolerance") { xy_goal_tolerance_ = parameter.as_double(); xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_; - } else if (name == plugin_name_ + ".yaw_goal_tolerance") { + } else if (param_name == plugin_name_ + ".yaw_goal_tolerance") { yaw_goal_tolerance_ = parameter.as_double(); } - } else if (type == ParameterType::PARAMETER_BOOL) { - if (name == plugin_name_ + ".stateful") { + } else if (param_type == ParameterType::PARAMETER_BOOL) { + if (param_name == plugin_name_ + ".stateful") { stateful_ = parameter.as_bool(); } } diff --git a/nav2_controller/plugins/simple_progress_checker.cpp b/nav2_controller/plugins/simple_progress_checker.cpp index 120b4320da1..001edbf5d62 100644 --- a/nav2_controller/plugins/simple_progress_checker.cpp +++ b/nav2_controller/plugins/simple_progress_checker.cpp @@ -98,13 +98,16 @@ SimpleProgressChecker::dynamicParametersCallback(std::vector { rcl_interfaces::msg::SetParametersResult result; for (auto parameter : parameters) { - const auto & type = parameter.get_type(); - const auto & name = parameter.get_name(); + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + if (param_name.find(plugin_name_ + ".") != 0) { + continue; + } - if (type == ParameterType::PARAMETER_DOUBLE) { - if (name == plugin_name_ + ".required_movement_radius") { + if (param_type == ParameterType::PARAMETER_DOUBLE) { + if (param_name == plugin_name_ + ".required_movement_radius") { radius_ = parameter.as_double(); - } else if (name == plugin_name_ + ".movement_time_allowance") { + } else if (param_name == plugin_name_ + ".movement_time_allowance") { time_allowance_ = rclcpp::Duration::from_seconds(parameter.as_double()); } } diff --git a/nav2_controller/plugins/stopped_goal_checker.cpp b/nav2_controller/plugins/stopped_goal_checker.cpp index 179c698bd46..7408d4db78a 100644 --- a/nav2_controller/plugins/stopped_goal_checker.cpp +++ b/nav2_controller/plugins/stopped_goal_checker.cpp @@ -119,13 +119,16 @@ StoppedGoalChecker::dynamicParametersCallback(std::vector par { rcl_interfaces::msg::SetParametersResult result; for (auto parameter : parameters) { - const auto & type = parameter.get_type(); - const auto & name = parameter.get_name(); + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + if (param_name.find(plugin_name_ + ".") != 0) { + continue; + } - if (type == ParameterType::PARAMETER_DOUBLE) { - if (name == plugin_name_ + ".rot_stopped_velocity") { + if (param_type == ParameterType::PARAMETER_DOUBLE) { + if (param_name == plugin_name_ + ".rot_stopped_velocity") { rot_stopped_velocity_ = parameter.as_double(); - } else if (name == plugin_name_ + ".trans_stopped_velocity") { + } else if (param_name == plugin_name_ + ".trans_stopped_velocity") { trans_stopped_velocity_ = parameter.as_double(); } } diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 220dec1d73f..f370f5437e8 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -836,12 +836,12 @@ ControllerServer::dynamicParametersCallback(std::vector param rcl_interfaces::msg::SetParametersResult result; for (auto parameter : parameters) { - const auto & type = parameter.get_type(); - const auto & name = parameter.get_name(); + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); // If we are trying to change the parameter of a plugin we can just skip it at this point // as they handle parameter changes themselves and don't need to lock the mutex - if (name.find('.') != std::string::npos) { + if (param_name.find('.') != std::string::npos) { continue; } @@ -855,16 +855,14 @@ ControllerServer::dynamicParametersCallback(std::vector param return result; } - if (type == ParameterType::PARAMETER_DOUBLE) { - if (name == "controller_frequency") { - controller_frequency_ = parameter.as_double(); - } else if (name == "min_x_velocity_threshold") { + if (param_type == ParameterType::PARAMETER_DOUBLE) { + if (param_name == "min_x_velocity_threshold") { min_x_velocity_threshold_ = parameter.as_double(); - } else if (name == "min_y_velocity_threshold") { + } else if (param_name == "min_y_velocity_threshold") { min_y_velocity_threshold_ = parameter.as_double(); - } else if (name == "min_theta_velocity_threshold") { + } else if (param_name == "min_theta_velocity_threshold") { min_theta_velocity_threshold_ = parameter.as_double(); - } else if (name == "failure_tolerance") { + } else if (param_name == "failure_tolerance") { failure_tolerance_ = parameter.as_double(); } } diff --git a/nav2_controller/test/test_dynamic_parameters.cpp b/nav2_controller/test/test_dynamic_parameters.cpp index b8f03e19991..523991a8e12 100644 --- a/nav2_controller/test/test_dynamic_parameters.cpp +++ b/nav2_controller/test/test_dynamic_parameters.cpp @@ -62,8 +62,7 @@ TEST(WPTest, test_dynamic_parameters) controller->get_node_services_interface()); auto results = rec_param->set_parameters_atomically( - {rclcpp::Parameter("controller_frequency", 100.0), - rclcpp::Parameter("min_x_velocity_threshold", 100.0), + {rclcpp::Parameter("min_x_velocity_threshold", 100.0), rclcpp::Parameter("min_y_velocity_threshold", 100.0), rclcpp::Parameter("min_theta_velocity_threshold", 100.0), rclcpp::Parameter("failure_tolerance", 5.0)}); @@ -72,7 +71,6 @@ TEST(WPTest, test_dynamic_parameters) controller->get_node_base_interface(), results); - EXPECT_EQ(controller->get_parameter("controller_frequency").as_double(), 100.0); EXPECT_EQ(controller->get_parameter("min_x_velocity_threshold").as_double(), 100.0); EXPECT_EQ(controller->get_parameter("min_y_velocity_threshold").as_double(), 100.0); EXPECT_EQ(controller->get_parameter("min_theta_velocity_threshold").as_double(), 100.0); diff --git a/nav2_costmap_2d/plugins/inflation_layer.cpp b/nav2_costmap_2d/plugins/inflation_layer.cpp index 6ce187a0b23..90dfbab319e 100644 --- a/nav2_costmap_2d/plugins/inflation_layer.cpp +++ b/nav2_costmap_2d/plugins/inflation_layer.cpp @@ -444,6 +444,9 @@ InflationLayer::dynamicParametersCallback( for (auto parameter : parameters) { const auto & param_type = parameter.get_type(); const auto & param_name = parameter.get_name(); + if (param_name.find(name_ + ".") != 0) { + continue; + } if (param_type == ParameterType::PARAMETER_DOUBLE) { if (param_name == name_ + "." + "inflation_radius" && diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp index 75dc2bcf217..fcfa9e180cc 100644 --- a/nav2_costmap_2d/plugins/obstacle_layer.cpp +++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp @@ -326,6 +326,9 @@ ObstacleLayer::dynamicParametersCallback( for (auto parameter : parameters) { const auto & param_type = parameter.get_type(); const auto & param_name = parameter.get_name(); + if (param_name.find(name_ + ".") != 0) { + continue; + } if (param_type == ParameterType::PARAMETER_DOUBLE) { if (param_name == name_ + "." + "min_obstacle_height") { diff --git a/nav2_costmap_2d/plugins/plugin_container_layer.cpp b/nav2_costmap_2d/plugins/plugin_container_layer.cpp index 743a4751e1a..b8f50d01dee 100644 --- a/nav2_costmap_2d/plugins/plugin_container_layer.cpp +++ b/nav2_costmap_2d/plugins/plugin_container_layer.cpp @@ -217,6 +217,9 @@ rcl_interfaces::msg::SetParametersResult PluginContainerLayer::dynamicParameters for (auto parameter : parameters) { const auto & param_type = parameter.get_type(); const auto & param_name = parameter.get_name(); + if (param_name.find(name_ + ".") != 0) { + continue; + } if (param_type == ParameterType::PARAMETER_INTEGER) { if (param_name == name_ + "." + "combination_method") { diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp index 4ef50c1f0d4..4d58dc2914f 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -485,6 +485,9 @@ StaticLayer::dynamicParametersCallback( for (auto parameter : parameters) { const auto & param_type = parameter.get_type(); const auto & param_name = parameter.get_name(); + if (param_name.find(name_ + ".") != 0) { + continue; + } if (param_name == name_ + "." + "map_subscribe_transient_local" || param_name == name_ + "." + "map_topic" || diff --git a/nav2_costmap_2d/plugins/voxel_layer.cpp b/nav2_costmap_2d/plugins/voxel_layer.cpp index afd1ad06459..af1ac0a016f 100644 --- a/nav2_costmap_2d/plugins/voxel_layer.cpp +++ b/nav2_costmap_2d/plugins/voxel_layer.cpp @@ -496,6 +496,9 @@ VoxelLayer::dynamicParametersCallback( for (auto parameter : parameters) { const auto & param_type = parameter.get_type(); const auto & param_name = parameter.get_name(); + if (param_name.find(name_ + ".") != 0) { + continue; + } if (param_type == ParameterType::PARAMETER_DOUBLE) { if (param_name == name_ + "." + "max_obstacle_height") { diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index ae4de8f0c34..87a67a0758f 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -725,42 +725,46 @@ Costmap2DROS::dynamicParametersCallback(std::vector parameter std::lock_guard lock_reinit(_dynamic_parameter_mutex); for (auto parameter : parameters) { - const auto & type = parameter.get_type(); - const auto & name = parameter.get_name(); + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + if (param_name.find('.') != std::string::npos) { + continue; + } + - if (type == ParameterType::PARAMETER_DOUBLE) { - if (name == "robot_radius") { + if (param_type == ParameterType::PARAMETER_DOUBLE) { + if (param_name == "robot_radius") { robot_radius_ = parameter.as_double(); // Set the footprint if (use_radius_) { setRobotFootprint(makeFootprintFromRadius(robot_radius_)); } - } else if (name == "footprint_padding") { + } else if (param_name == "footprint_padding") { footprint_padding_ = parameter.as_double(); padded_footprint_ = unpadded_footprint_; padFootprint(padded_footprint_, footprint_padding_); layered_costmap_->setFootprint(padded_footprint_); - } else if (name == "transform_tolerance") { + } else if (param_name == "transform_tolerance") { transform_tolerance_ = parameter.as_double(); - } else if (name == "publish_frequency") { + } else if (param_name == "publish_frequency") { map_publish_frequency_ = parameter.as_double(); if (map_publish_frequency_ > 0) { publish_cycle_ = rclcpp::Duration::from_seconds(1 / map_publish_frequency_); } else { publish_cycle_ = rclcpp::Duration(-1s); } - } else if (name == "resolution") { + } else if (param_name == "resolution") { resize_map = true; resolution_ = parameter.as_double(); - } else if (name == "origin_x") { + } else if (param_name == "origin_x") { resize_map = true; origin_x_ = parameter.as_double(); - } else if (name == "origin_y") { + } else if (param_name == "origin_y") { resize_map = true; origin_y_ = parameter.as_double(); } - } else if (type == ParameterType::PARAMETER_INTEGER) { - if (name == "width") { + } else if (param_type == ParameterType::PARAMETER_INTEGER) { + if (param_name == "width") { if (parameter.as_int() > 0) { resize_map = true; map_width_meters_ = parameter.as_int(); @@ -771,7 +775,7 @@ Costmap2DROS::dynamicParametersCallback(std::vector parameter result.successful = false; return result; } - } else if (name == "height") { + } else if (param_name == "height") { if (parameter.as_int() > 0) { resize_map = true; map_height_meters_ = parameter.as_int(); @@ -783,14 +787,14 @@ Costmap2DROS::dynamicParametersCallback(std::vector parameter return result; } } - } else if (type == ParameterType::PARAMETER_STRING) { - if (name == "footprint") { + } else if (param_type == ParameterType::PARAMETER_STRING) { + if (param_name == "footprint") { footprint_ = parameter.as_string(); std::vector new_footprint; if (makeFootprintFromString(footprint_, new_footprint)) { setRobotFootprint(new_footprint); } - } else if (name == "robot_base_frame") { + } else if (param_name == "robot_base_frame") { // First, make sure that the transform between the robot base frame // and the global frame is available std::string tf_error; diff --git a/nav2_docking/opennav_docking/src/controller.cpp b/nav2_docking/opennav_docking/src/controller.cpp index 1694c8f5375..62a87651bea 100644 --- a/nav2_docking/opennav_docking/src/controller.cpp +++ b/nav2_docking/opennav_docking/src/controller.cpp @@ -241,35 +241,37 @@ Controller::dynamicParametersCallback(std::vector parameters) rcl_interfaces::msg::SetParametersResult result; for (auto parameter : parameters) { - const auto & type = parameter.get_type(); - const auto & name = parameter.get_name(); - - if (type == rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE) { - if (name == "controller.k_phi") { + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + if (param_name.find("controller.") != 0) { + continue; + } + if (param_type == rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE) { + if (param_name == "controller.k_phi") { k_phi_ = parameter.as_double(); - } else if (name == "controller.k_delta") { + } else if (param_name == "controller.k_delta") { k_delta_ = parameter.as_double(); - } else if (name == "controller.beta") { + } else if (param_name == "controller.beta") { beta_ = parameter.as_double(); - } else if (name == "controller.lambda") { + } else if (param_name == "controller.lambda") { lambda_ = parameter.as_double(); - } else if (name == "controller.v_linear_min") { + } else if (param_name == "controller.v_linear_min") { v_linear_min_ = parameter.as_double(); - } else if (name == "controller.v_linear_max") { + } else if (param_name == "controller.v_linear_max") { v_linear_max_ = parameter.as_double(); - } else if (name == "controller.v_angular_max") { + } else if (param_name == "controller.v_angular_max") { v_angular_max_ = parameter.as_double(); - } else if (name == "controller.slowdown_radius") { + } else if (param_name == "controller.slowdown_radius") { slowdown_radius_ = parameter.as_double(); - } else if (name == "controller.rotate_to_heading_angular_vel") { + } else if (param_name == "controller.rotate_to_heading_angular_vel") { rotate_to_heading_angular_vel_ = parameter.as_double(); - } else if (name == "controller.rotate_to_heading_max_angular_accel") { + } else if (param_name == "controller.rotate_to_heading_max_angular_accel") { rotate_to_heading_max_angular_accel_ = parameter.as_double(); - } else if (name == "controller.projection_time") { + } else if (param_name == "controller.projection_time") { projection_time_ = parameter.as_double(); - } else if (name == "controller.simulation_time_step") { + } else if (param_name == "controller.simulation_time_step") { simulation_time_step_ = parameter.as_double(); - } else if (name == "controller.dock_collision_threshold") { + } else if (param_name == "controller.dock_collision_threshold") { dock_collision_threshold_ = parameter.as_double(); } diff --git a/nav2_docking/opennav_docking/src/docking_server.cpp b/nav2_docking/opennav_docking/src/docking_server.cpp index d20e23401c2..c14686ee1a5 100644 --- a/nav2_docking/opennav_docking/src/docking_server.cpp +++ b/nav2_docking/opennav_docking/src/docking_server.cpp @@ -830,31 +830,34 @@ DockingServer::dynamicParametersCallback(std::vector paramete rcl_interfaces::msg::SetParametersResult result; for (auto parameter : parameters) { - const auto & type = parameter.get_type(); - const auto & name = parameter.get_name(); + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + if (param_name.find('.') != std::string::npos) { + continue; + } - if (type == ParameterType::PARAMETER_DOUBLE) { - if (name == "controller_frequency") { + if (param_type == ParameterType::PARAMETER_DOUBLE) { + if (param_name == "controller_frequency") { controller_frequency_ = parameter.as_double(); - } else if (name == "initial_perception_timeout") { + } else if (param_name == "initial_perception_timeout") { initial_perception_timeout_ = parameter.as_double(); - } else if (name == "wait_charge_timeout") { + } else if (param_name == "wait_charge_timeout") { wait_charge_timeout_ = parameter.as_double(); - } else if (name == "undock_linear_tolerance") { + } else if (param_name == "undock_linear_tolerance") { undock_linear_tolerance_ = parameter.as_double(); - } else if (name == "undock_angular_tolerance") { + } else if (param_name == "undock_angular_tolerance") { undock_angular_tolerance_ = parameter.as_double(); - } else if (name == "rotation_angular_tolerance") { + } else if (param_name == "rotation_angular_tolerance") { rotation_angular_tolerance_ = parameter.as_double(); } - } else if (type == ParameterType::PARAMETER_STRING) { - if (name == "base_frame") { + } else if (param_type == ParameterType::PARAMETER_STRING) { + if (param_name == "base_frame") { base_frame_ = parameter.as_string(); - } else if (name == "fixed_frame") { + } else if (param_name == "fixed_frame") { fixed_frame_ = parameter.as_string(); } - } else if (type == ParameterType::PARAMETER_INTEGER) { - if (name == "max_retries") { + } else if (param_type == ParameterType::PARAMETER_INTEGER) { + if (param_name == "max_retries") { max_retries_ = parameter.as_int(); } } diff --git a/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp b/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp index f72b4aaa242..e0ace25ce47 100644 --- a/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp +++ b/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp @@ -252,7 +252,6 @@ TEST(DockingServerTests, testDynamicParams) rclcpp::spin_until_future_complete(node->get_node_base_interface(), results); - EXPECT_EQ(node->get_parameter("controller_frequency").as_double(), 0.2); EXPECT_EQ(node->get_parameter("initial_perception_timeout").as_double(), 1.0); EXPECT_EQ(node->get_parameter("wait_charge_timeout").as_double(), 1.2); EXPECT_EQ(node->get_parameter("undock_linear_tolerance").as_double(), 0.25); diff --git a/nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp b/nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp index dd7d5ff1e64..d40f7f1b66e 100644 --- a/nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp +++ b/nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp @@ -177,43 +177,46 @@ KinematicsHandler::dynamicParametersCallback(std::vector para KinematicParameters kinematics(*kinematics_.load()); for (auto parameter : parameters) { - const auto & type = parameter.get_type(); - const auto & name = parameter.get_name(); + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + if (param_name.find(plugin_name_ + ".") != 0) { + continue; + } - if (type == ParameterType::PARAMETER_DOUBLE) { - if (name == plugin_name_ + ".min_vel_x") { + if (param_type == ParameterType::PARAMETER_DOUBLE) { + if (param_name == plugin_name_ + ".min_vel_x") { kinematics.min_vel_x_ = parameter.as_double(); - } else if (name == plugin_name_ + ".min_vel_y") { + } else if (param_name == plugin_name_ + ".min_vel_y") { kinematics.min_vel_y_ = parameter.as_double(); - } else if (name == plugin_name_ + ".max_vel_x") { + } else if (param_name == plugin_name_ + ".max_vel_x") { kinematics.max_vel_x_ = parameter.as_double(); kinematics.base_max_vel_x_ = kinematics.max_vel_x_; - } else if (name == plugin_name_ + ".max_vel_y") { + } else if (param_name == plugin_name_ + ".max_vel_y") { kinematics.max_vel_y_ = parameter.as_double(); kinematics.base_max_vel_y_ = kinematics.max_vel_y_; - } else if (name == plugin_name_ + ".max_vel_theta") { + } else if (param_name == plugin_name_ + ".max_vel_theta") { kinematics.max_vel_theta_ = parameter.as_double(); kinematics.base_max_vel_theta_ = kinematics.max_vel_theta_; - } else if (name == plugin_name_ + ".min_speed_xy") { + } else if (param_name == plugin_name_ + ".min_speed_xy") { kinematics.min_speed_xy_ = parameter.as_double(); kinematics.min_speed_xy_sq_ = kinematics.min_speed_xy_ * kinematics.min_speed_xy_; - } else if (name == plugin_name_ + ".max_speed_xy") { + } else if (param_name == plugin_name_ + ".max_speed_xy") { kinematics.max_speed_xy_ = parameter.as_double(); kinematics.base_max_speed_xy_ = kinematics.max_speed_xy_; - } else if (name == plugin_name_ + ".min_speed_theta") { + } else if (param_name == plugin_name_ + ".min_speed_theta") { kinematics.min_speed_theta_ = parameter.as_double(); kinematics.max_speed_xy_sq_ = kinematics.max_speed_xy_ * kinematics.max_speed_xy_; - } else if (name == plugin_name_ + ".acc_lim_x") { + } else if (param_name == plugin_name_ + ".acc_lim_x") { kinematics.acc_lim_x_ = parameter.as_double(); - } else if (name == plugin_name_ + ".acc_lim_y") { + } else if (param_name == plugin_name_ + ".acc_lim_y") { kinematics.acc_lim_y_ = parameter.as_double(); - } else if (name == plugin_name_ + ".acc_lim_theta") { + } else if (param_name == plugin_name_ + ".acc_lim_theta") { kinematics.acc_lim_theta_ = parameter.as_double(); - } else if (name == plugin_name_ + ".decel_lim_x") { + } else if (param_name == plugin_name_ + ".decel_lim_x") { kinematics.decel_lim_x_ = parameter.as_double(); - } else if (name == plugin_name_ + ".decel_lim_y") { + } else if (param_name == plugin_name_ + ".decel_lim_y") { kinematics.decel_lim_y_ = parameter.as_double(); - } else if (name == plugin_name_ + ".decel_lim_theta") { + } else if (param_name == plugin_name_ + ".decel_lim_theta") { kinematics.decel_lim_theta_ = parameter.as_double(); } } diff --git a/nav2_graceful_controller/src/parameter_handler.cpp b/nav2_graceful_controller/src/parameter_handler.cpp index 7b2f7c42936..65fc260eb5f 100644 --- a/nav2_graceful_controller/src/parameter_handler.cpp +++ b/nav2_graceful_controller/src/parameter_handler.cpp @@ -135,45 +135,48 @@ ParameterHandler::dynamicParametersCallback(std::vector param std::lock_guard lock_reinit(mutex_); for (auto parameter : parameters) { - const auto & type = parameter.get_type(); - const auto & name = parameter.get_name(); + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + if (param_name.find(plugin_name_ + ".") != 0) { + continue; + } - if (type == ParameterType::PARAMETER_DOUBLE) { - if (name == plugin_name_ + ".transform_tolerance") { + if (param_type == ParameterType::PARAMETER_DOUBLE) { + if (param_name == plugin_name_ + ".transform_tolerance") { params_.transform_tolerance = parameter.as_double(); - } else if (name == plugin_name_ + ".min_lookahead") { + } else if (param_name == plugin_name_ + ".min_lookahead") { params_.min_lookahead = parameter.as_double(); - } else if (name == plugin_name_ + ".max_lookahead") { + } else if (param_name == plugin_name_ + ".max_lookahead") { params_.max_lookahead = parameter.as_double(); - } else if (name == plugin_name_ + ".k_phi") { + } else if (param_name == plugin_name_ + ".k_phi") { params_.k_phi = parameter.as_double(); - } else if (name == plugin_name_ + ".k_delta") { + } else if (param_name == plugin_name_ + ".k_delta") { params_.k_delta = parameter.as_double(); - } else if (name == plugin_name_ + ".beta") { + } else if (param_name == plugin_name_ + ".beta") { params_.beta = parameter.as_double(); - } else if (name == plugin_name_ + ".lambda") { + } else if (param_name == plugin_name_ + ".lambda") { params_.lambda = parameter.as_double(); - } else if (name == plugin_name_ + ".v_linear_min") { + } else if (param_name == plugin_name_ + ".v_linear_min") { params_.v_linear_min = parameter.as_double(); - } else if (name == plugin_name_ + ".v_linear_max") { + } else if (param_name == plugin_name_ + ".v_linear_max") { params_.v_linear_max = parameter.as_double(); params_.v_linear_max_initial = params_.v_linear_max; - } else if (name == plugin_name_ + ".v_angular_max") { + } else if (param_name == plugin_name_ + ".v_angular_max") { params_.v_angular_max = parameter.as_double(); params_.v_angular_max_initial = params_.v_angular_max; - } else if (name == plugin_name_ + ".v_angular_min_in_place") { + } else if (param_name == plugin_name_ + ".v_angular_min_in_place") { params_.v_angular_min_in_place = parameter.as_double(); - } else if (name == plugin_name_ + ".slowdown_radius") { + } else if (param_name == plugin_name_ + ".slowdown_radius") { params_.slowdown_radius = parameter.as_double(); - } else if (name == plugin_name_ + ".initial_rotation_tolerance") { + } else if (param_name == plugin_name_ + ".initial_rotation_tolerance") { params_.initial_rotation_tolerance = parameter.as_double(); - } else if (name == plugin_name_ + ".rotation_scaling_factor") { + } else if (param_name == plugin_name_ + ".rotation_scaling_factor") { params_.rotation_scaling_factor = parameter.as_double(); - } else if (name == plugin_name_ + ".in_place_collision_resolution") { + } else if (param_name == plugin_name_ + ".in_place_collision_resolution") { params_.in_place_collision_resolution = parameter.as_double(); } - } else if (type == ParameterType::PARAMETER_BOOL) { - if (name == plugin_name_ + ".initial_rotation") { + } else if (param_type == ParameterType::PARAMETER_BOOL) { + if (param_name == plugin_name_ + ".initial_rotation") { if (parameter.as_bool() && params_.allow_backward) { RCLCPP_WARN( logger_, "Initial rotation and allow backward parameters are both true, " @@ -181,9 +184,9 @@ ParameterHandler::dynamicParametersCallback(std::vector param continue; } params_.initial_rotation = parameter.as_bool(); - } else if (name == plugin_name_ + ".prefer_final_rotation") { + } else if (param_name == plugin_name_ + ".prefer_final_rotation") { params_.prefer_final_rotation = parameter.as_bool(); - } else if (name == plugin_name_ + ".allow_backward") { + } else if (param_name == plugin_name_ + ".allow_backward") { if (params_.initial_rotation && parameter.as_bool()) { RCLCPP_WARN( logger_, "Initial rotation and allow backward parameters are both true, " @@ -191,7 +194,7 @@ ParameterHandler::dynamicParametersCallback(std::vector param continue; } params_.allow_backward = parameter.as_bool(); - } else if (name == plugin_name_ + ".use_collision_detection") { + } else if (param_name == plugin_name_ + ".use_collision_detection") { params_.use_collision_detection = parameter.as_bool(); } } diff --git a/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp b/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp index 478bfd23492..42e98190d9a 100644 --- a/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp +++ b/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp @@ -79,7 +79,8 @@ void prepareAndRunBenchmark( printInfo(optimizer_settings, path_settings, critics); auto node = getDummyNode(optimizer_settings, critics); - auto parameters_handler = std::make_unique(node); + std::string name = "test"; + auto parameters_handler = std::make_unique(node, name); auto optimizer = getDummyOptimizer(node, costmap_ros, parameters_handler.get()); // evalControl args diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/parameters_handler.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/parameters_handler.hpp index 4445186c24c..28f0eaf3997 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/parameters_handler.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/parameters_handler.hpp @@ -45,7 +45,6 @@ class ParametersHandler rcl_interfaces::msg::SetParametersResult & result); using post_callback_t = void (); using pre_callback_t = void (); - /** * @brief Constructor for mppi::ParametersHandler */ @@ -56,7 +55,7 @@ class ParametersHandler * @param parent Weak ptr to node */ explicit ParametersHandler( - const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent); + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, std::string & name); /** * @brief Destructor for mppi::ParametersHandler @@ -168,10 +167,12 @@ class ParametersHandler on_set_param_handler_; rclcpp_lifecycle::LifecycleNode::WeakPtr node_; std::string node_name_; + std::string name_; bool verbose_{false}; std::unordered_map> get_param_callbacks_; + std::unordered_map> get_pre_callbacks_; std::vector> pre_callbacks_; std::vector> post_callbacks_; diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp index 81f8aa5c1c0..9c6c7c209a5 100644 --- a/nav2_mppi_controller/src/controller.cpp +++ b/nav2_mppi_controller/src/controller.cpp @@ -31,7 +31,7 @@ void MPPIController::configure( costmap_ros_ = costmap_ros; tf_buffer_ = tf; name_ = name; - parameters_handler_ = std::make_unique(parent); + parameters_handler_ = std::make_unique(parent, name_); auto node = parent_.lock(); // Get high-level controller parameters diff --git a/nav2_mppi_controller/src/parameters_handler.cpp b/nav2_mppi_controller/src/parameters_handler.cpp index 34d7764e695..4d540ef1cae 100644 --- a/nav2_mppi_controller/src/parameters_handler.cpp +++ b/nav2_mppi_controller/src/parameters_handler.cpp @@ -18,12 +18,13 @@ namespace mppi { ParametersHandler::ParametersHandler( - const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent) + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, std::string & name) { node_ = parent; auto node = node_.lock(); node_name_ = node->get_name(); logger_ = node->get_logger(); + name_ = name; } ParametersHandler::~ParametersHandler() @@ -57,23 +58,32 @@ ParametersHandler::dynamicParamsCallback( result.reason = ""; std::lock_guard lock(parameters_change_mutex_); - - for (auto & pre_cb : pre_callbacks_) { - pre_cb(); - } - + std::vector plugin_params; for (auto & param : parameters) { const std::string & param_name = param.get_name(); - - if (auto callback = get_param_callbacks_.find(param_name); - callback != get_param_callbacks_.end()) - { - callback->second(param, result); + if (param_name.find(name_ + ".") != 0) { + continue; } + plugin_params.push_back(param); } - for (auto & post_cb : post_callbacks_) { - post_cb(); + if (!plugin_params.empty()) { + for (auto & pre_cb : pre_callbacks_) { + pre_cb(); + } + + for (auto & param : plugin_params) { + const std::string & param_name = param.get_name(); + if (auto callback = get_param_callbacks_.find(param_name); + callback != get_param_callbacks_.end()) + { + callback->second(param, result); + } + } + + for (auto & post_cb : post_callbacks_) { + post_cb(); + } } if (!result.successful) { diff --git a/nav2_mppi_controller/test/critic_manager_test.cpp b/nav2_mppi_controller/test/critic_manager_test.cpp index 0c83a492e63..ae1362ff5f1 100644 --- a/nav2_mppi_controller/test/critic_manager_test.cpp +++ b/nav2_mppi_controller/test/critic_manager_test.cpp @@ -95,7 +95,8 @@ TEST(CriticManagerTests, BasicCriticOperations) auto node = std::make_shared("my_node"); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); @@ -134,7 +135,8 @@ TEST(CriticManagerTests, CriticLoadingTest) rclcpp::ParameterValue(std::vector{"ConstraintCritic", "PreferForwardCritic"})); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); rclcpp_lifecycle::State state; costmap_ros->on_configure(state); diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp index f02376af598..7e0f61ccf9b 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -62,7 +62,8 @@ TEST(CriticTests, ConstraintsCritic) auto node = std::make_shared("my_node"); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); @@ -130,7 +131,8 @@ TEST(CriticTests, ObstacleCriticMisalignedParams) { auto node = std::make_shared("my_node"); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); auto getParam = param_handler.getParamGetter("critic"); bool consider_footprint; getParam(consider_footprint, "consider_footprint", true); @@ -150,7 +152,8 @@ TEST(CriticTests, ObstacleCriticAlignedParams) { auto node = std::make_shared("my_node"); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); auto getParam = param_handler.getParamGetter("critic"); bool consider_footprint; getParam(consider_footprint, "consider_footprint", false); @@ -169,7 +172,8 @@ TEST(CriticTests, CostCriticMisAlignedParams) { auto node = std::make_shared("my_node"); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); rclcpp_lifecycle::State lstate; auto getParam = param_handler.getParamGetter("critic"); bool consider_footprint; @@ -189,7 +193,8 @@ TEST(CriticTests, CostCriticAlignedParams) { auto node = std::make_shared("my_node"); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); rclcpp_lifecycle::State lstate; auto getParam = param_handler.getParamGetter("critic"); bool consider_footprint; @@ -207,7 +212,8 @@ TEST(CriticTests, GoalAngleCritic) auto node = std::make_shared("my_node"); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); @@ -266,7 +272,8 @@ TEST(CriticTests, GoalCritic) auto node = std::make_shared("my_node"); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); @@ -318,7 +325,8 @@ TEST(CriticTests, PathAngleCritic) auto node = std::make_shared("my_node"); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); @@ -436,7 +444,8 @@ TEST(CriticTests, PreferForwardCritic) auto node = std::make_shared("my_node"); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); @@ -492,7 +501,8 @@ TEST(CriticTests, TwirlingCritic) auto node = std::make_shared("my_node"); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); @@ -556,7 +566,8 @@ TEST(CriticTests, PathFollowCritic) auto node = std::make_shared("my_node"); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); @@ -607,7 +618,8 @@ TEST(CriticTests, PathAlignCritic) auto node = std::make_shared("my_node"); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); @@ -717,7 +729,8 @@ TEST(CriticTests, VelocityDeadbandCritic) auto node = std::make_shared("my_node"); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); auto getParam = param_handler.getParamGetter("critic"); std::vector deadband_velocities_; getParam(deadband_velocities_, "deadband_velocities", std::vector{0.08, 0.08, 0.08}); diff --git a/nav2_mppi_controller/test/motion_model_tests.cpp b/nav2_mppi_controller/test/motion_model_tests.cpp index 7babbd9f5db..28653ad50c2 100644 --- a/nav2_mppi_controller/test/motion_model_tests.cpp +++ b/nav2_mppi_controller/test/motion_model_tests.cpp @@ -127,7 +127,8 @@ TEST(MotionModelTests, AckermannTest) control_sequence.reset(timesteps); // populates with zeros state.reset(batches, timesteps); // populates with zeros auto node = std::make_shared("my_node"); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); std::unique_ptr model = std::make_unique(¶m_handler, node->get_name()); @@ -185,7 +186,8 @@ TEST(MotionModelTests, AckermannReversingTest) control_sequence2.reset(timesteps); // populates with zeros state.reset(batches, timesteps); // populates with zeros auto node = std::make_shared("my_node"); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); std::unique_ptr model = std::make_unique(¶m_handler, node->get_name()); diff --git a/nav2_mppi_controller/test/noise_generator_test.cpp b/nav2_mppi_controller/test/noise_generator_test.cpp index b9daf8af5f3..7533913d828 100644 --- a/nav2_mppi_controller/test/noise_generator_test.cpp +++ b/nav2_mppi_controller/test/noise_generator_test.cpp @@ -38,7 +38,8 @@ TEST(NoiseGeneratorTest, NoiseGeneratorLifecycle) auto node = std::make_shared("node"); node->declare_parameter("test_name.regenerate_noises", rclcpp::ParameterValue(false)); - ParametersHandler handler(node); + std::string name = "test"; +ParametersHandler handler(node, name); generator.initialize(settings, false, "test_name", &handler); generator.reset(settings, false); @@ -50,7 +51,8 @@ TEST(NoiseGeneratorTest, NoiseGeneratorMain) // Tests shuts down internal thread cleanly auto node = std::make_shared("node"); node->declare_parameter("test_name.regenerate_noises", rclcpp::ParameterValue(true)); - ParametersHandler handler(node); + std::string name = "test"; +ParametersHandler handler(node, name); NoiseGenerator generator; mppi::models::OptimizerSettings settings; settings.batch_size = 100; @@ -138,7 +140,8 @@ TEST(NoiseGeneratorTest, NoiseGeneratorMainNoRegenerate) // This time with no regeneration of noises auto node = std::make_shared("node"); node->declare_parameter("test_name.regenerate_noises", rclcpp::ParameterValue(false)); - ParametersHandler handler(node); + std::string name = "test"; +ParametersHandler handler(node, name); NoiseGenerator generator; mppi::models::OptimizerSettings settings; settings.batch_size = 100; diff --git a/nav2_mppi_controller/test/optimizer_smoke_test.cpp b/nav2_mppi_controller/test/optimizer_smoke_test.cpp index 09551c387ae..e4339944ad3 100644 --- a/nav2_mppi_controller/test/optimizer_smoke_test.cpp +++ b/nav2_mppi_controller/test/optimizer_smoke_test.cpp @@ -66,7 +66,8 @@ TEST_P(OptimizerSuite, OptimizerTest) { printInfo(optimizer_settings, path_settings, critics); auto node = getDummyNode(optimizer_settings, critics); - auto parameters_handler = std::make_unique(node); + std::string name = "test"; + auto parameters_handler = std::make_unique(node, name); auto optimizer = getDummyOptimizer(node, costmap_ros, parameters_handler.get()); // evalControl args diff --git a/nav2_mppi_controller/test/optimizer_unit_tests.cpp b/nav2_mppi_controller/test/optimizer_unit_tests.cpp index 26eb2fff83a..1c3732399e6 100644 --- a/nav2_mppi_controller/test/optimizer_unit_tests.cpp +++ b/nav2_mppi_controller/test/optimizer_unit_tests.cpp @@ -229,7 +229,8 @@ TEST(OptimizerTests, BasicInitializedFunctions) node->declare_parameter("mppic.ay_min", rclcpp::ParameterValue(3.0)); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); optimizer_tester.initialize(node, "mppic", costmap_ros, ¶m_handler); @@ -266,7 +267,8 @@ TEST(OptimizerTests, TestOptimizerMotionModels) node->declare_parameter("controller_frequency", rclcpp::ParameterValue(30.0)); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); optimizer_tester.initialize(node, "mppic", costmap_ros, ¶m_handler); @@ -298,7 +300,8 @@ TEST(OptimizerTests, setOffsetTests) node->declare_parameter("mppic.time_steps", rclcpp::ParameterValue(50)); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); optimizer_tester.initialize(node, "mppic", costmap_ros, ¶m_handler); @@ -321,7 +324,8 @@ TEST(OptimizerTests, resetTests) node->declare_parameter("mppic.time_steps", rclcpp::ParameterValue(50)); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); optimizer_tester.initialize(node, "mppic", costmap_ros, ¶m_handler); @@ -341,7 +345,8 @@ TEST(OptimizerTests, FallbackTests) node->declare_parameter("mppic.retry_attempt_limit", rclcpp::ParameterValue(2)); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); optimizer_tester.initialize(node, "mppic", costmap_ros, ¶m_handler); @@ -365,7 +370,8 @@ TEST(OptimizerTests, PrepareTests) node->declare_parameter("mppic.retry_attempt_limit", rclcpp::ParameterValue(2)); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); optimizer_tester.initialize(node, "mppic", costmap_ros, ¶m_handler); @@ -393,7 +399,8 @@ TEST(OptimizerTests, shiftControlSequenceTests) node->declare_parameter("mppic.retry_attempt_limit", rclcpp::ParameterValue(2)); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); optimizer_tester.initialize(node, "mppic", costmap_ros, ¶m_handler); @@ -436,7 +443,8 @@ TEST(OptimizerTests, SpeedLimitTests) node->declare_parameter("mppic.retry_attempt_limit", rclcpp::ParameterValue(2)); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); optimizer_tester.initialize(node, "mppic", costmap_ros, ¶m_handler); @@ -476,7 +484,8 @@ TEST(OptimizerTests, applyControlSequenceConstraintsTests) node->declare_parameter("mppic.wz_max", rclcpp::ParameterValue(2.0)); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); optimizer_tester.initialize(node, "mppic", costmap_ros, ¶m_handler); @@ -537,7 +546,8 @@ TEST(OptimizerTests, updateStateVelocitiesTests) node->declare_parameter("mppic.az_max", rclcpp::ParameterValue(3.5)); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); optimizer_tester.initialize(node, "mppic", costmap_ros, ¶m_handler); @@ -562,7 +572,8 @@ TEST(OptimizerTests, getControlFromSequenceAsTwistTests) node->declare_parameter("mppic.wz_max", rclcpp::ParameterValue(2.0)); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); optimizer_tester.initialize(node, "mppic", costmap_ros, ¶m_handler); @@ -597,7 +608,8 @@ TEST(OptimizerTests, integrateStateVelocitiesTests) node->declare_parameter("mppic.time_steps", rclcpp::ParameterValue(50)); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); optimizer_tester.initialize(node, "mppic", costmap_ros, ¶m_handler); @@ -663,7 +675,8 @@ TEST(OptimizerTests, TestGetters) node->declare_parameter("controller_frequency", rclcpp::ParameterValue(30.0)); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - ParametersHandler param_handler(node); + std::string test = "test"; + ParametersHandler param_handler(node, test); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); optimizer_tester.initialize(node, "mppic", costmap_ros, ¶m_handler); diff --git a/nav2_mppi_controller/test/parameter_handler_test.cpp b/nav2_mppi_controller/test/parameter_handler_test.cpp index 56917bbb3c0..93392970869 100644 --- a/nav2_mppi_controller/test/parameter_handler_test.cpp +++ b/nav2_mppi_controller/test/parameter_handler_test.cpp @@ -29,8 +29,8 @@ class ParametersHandlerWrapper : public ParametersHandler ParametersHandlerWrapper() = default; explicit ParametersHandlerWrapper( - const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent) - : ParametersHandler(parent) {} + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, std::string name) + : ParametersHandler(parent, name) {} template auto asWrapped(rclcpp::Parameter parameter) @@ -44,17 +44,19 @@ using namespace mppi; // NOLINT TEST(ParameterHandlerTest, asTypeConversionTest) { ParametersHandlerWrapper a; - rclcpp::Parameter int_p("int_parameter", rclcpp::ParameterValue(1)); - rclcpp::Parameter double_p("double_parameter", rclcpp::ParameterValue(10.0)); - rclcpp::Parameter bool_p("bool_parameter", rclcpp::ParameterValue(false)); - rclcpp::Parameter string_p("string_parameter", rclcpp::ParameterValue(std::string("hello"))); + rclcpp::Parameter int_p("test.int_parameter", rclcpp::ParameterValue(1)); + rclcpp::Parameter double_p("test.double_parameter", rclcpp::ParameterValue(10.0)); + rclcpp::Parameter bool_p("test.bool_parameter", rclcpp::ParameterValue(false)); + rclcpp::Parameter string_p("test.string_parameter", rclcpp::ParameterValue(std::string("hello"))); - rclcpp::Parameter intv_p("intv_parameter", rclcpp::ParameterValue(std::vector{1})); + rclcpp::Parameter intv_p("test.intv_parameter", rclcpp::ParameterValue(std::vector{1})); rclcpp::Parameter doublev_p( - "doublev_parameter", rclcpp::ParameterValue(std::vector{10.0})); - rclcpp::Parameter boolv_p("boolv_parameter", rclcpp::ParameterValue(std::vector{false})); + "test.doublev_parameter", rclcpp::ParameterValue(std::vector{10.0})); + rclcpp::Parameter boolv_p("test.boolv_parameter", + rclcpp::ParameterValue(std::vector{false})); rclcpp::Parameter stringv_p( - "stringv_parameter", rclcpp::ParameterValue(std::vector{std::string("hello")})); + "test.stringv_parameter", + rclcpp::ParameterValue(std::vector{std::string("hello")})); EXPECT_EQ(a.asWrapped(int_p), 1); EXPECT_EQ(a.asWrapped(double_p), 10.0); @@ -89,15 +91,15 @@ TEST(ParameterHandlerTest, PrePostDynamicCallbackTest) dynamic_triggered = true; }; - rclcpp::Parameter random_param("blah_blah", rclcpp::ParameterValue(true)); - rclcpp::Parameter random_param2("use_sim_time", rclcpp::ParameterValue(true)); + rclcpp::Parameter random_param(".blah_blah", rclcpp::ParameterValue(true)); + rclcpp::Parameter random_param2(".use_sim_time", rclcpp::ParameterValue(true)); bool val = false; ParametersHandlerWrapper a; a.addPreCallback(preCb); a.addPostCallback(postCb); - a.addParamCallback("use_sim_time", dynamicCb); - a.setParamCallback(val, "blah_blah"); + a.addParamCallback(".use_sim_time", dynamicCb); + a.setParamCallback(val, ".blah_blah"); // Dynamic callback should not trigger, wrong parameter, but val should be updated a.dynamicParamsCallback(std::vector{random_param}); @@ -117,23 +119,24 @@ TEST(ParameterHandlerTest, PrePostDynamicCallbackTest) TEST(ParameterHandlerTest, GetSystemParamsTest) { auto node = std::make_shared("my_node"); - node->declare_parameter("param1", rclcpp::ParameterValue(true)); - node->declare_parameter("ns.param2", rclcpp::ParameterValue(7)); + node->declare_parameter("test.param1", rclcpp::ParameterValue(true)); + node->declare_parameter("ns.test.param2", rclcpp::ParameterValue(7)); // Get parameters in global namespace and in subnamespaces - ParametersHandler handler(node); + std::string name = "test"; + ParametersHandler handler(node, name); auto getParameter = handler.getParamGetter(""); bool p1 = false; int p2 = 0; - getParameter(p1, "param1", false); - getParameter(p2, "ns.param2", 0); + getParameter(p1, "test.param1", false); + getParameter(p2, "ns.test.param2", 0); EXPECT_EQ(p1, true); EXPECT_EQ(p2, 7); // Get parameters in subnamespaces using name semantics of getter auto getParameter2 = handler.getParamGetter("ns"); p2 = 0; - getParameter2(p2, "param2", 0); + getParameter2(p2, "test.param2", 0); EXPECT_EQ(p2, 7); } @@ -141,16 +144,17 @@ TEST(ParameterHandlerTest, DynamicAndStaticParametersTest) { auto node = std::make_shared("my_node"); - node->declare_parameter("dynamic_int", rclcpp::ParameterValue(7)); - node->declare_parameter("static_int", rclcpp::ParameterValue(7)); - ParametersHandlerWrapper handler(node); + node->declare_parameter("test.dynamic_int", rclcpp::ParameterValue(7)); + node->declare_parameter("test.static_int", rclcpp::ParameterValue(7)); + std::string name = "test"; + ParametersHandlerWrapper handler(node, name); handler.start(); // Get parameters and check they have initial values auto getParameter = handler.getParamGetter(""); int p1 = 0, p2 = 0; - getParameter(p1, "dynamic_int", 0, ParameterType::Dynamic); - getParameter(p2, "static_int", 0, ParameterType::Static); + getParameter(p1, "test.dynamic_int", 0, ParameterType::Dynamic); + getParameter(p2, "test.static_int", 0, ParameterType::Static); EXPECT_EQ(p1, 7); EXPECT_EQ(p2, 7); @@ -164,8 +168,8 @@ TEST(ParameterHandlerTest, DynamicAndStaticParametersTest) rec_param->set_parameters_atomically( { rclcpp::Parameter("my_node.verbose", true), - rclcpp::Parameter("dynamic_int", 10), - rclcpp::Parameter("static_int", 10) + rclcpp::Parameter("test.dynamic_int", 10), + rclcpp::Parameter("test.static_int", 10) }); auto rc = rclcpp::spin_until_future_complete( @@ -178,7 +182,7 @@ TEST(ParameterHandlerTest, DynamicAndStaticParametersTest) EXPECT_FALSE(result.reason.empty()); EXPECT_EQ( result.reason, std::string("Rejected change to static parameter: ") + - "{\"name\": \"static_int\", \"type\": \"integer\", \"value\": \"10\"}"); + "{\"name\": \"test.static_int\", \"type\": \"integer\", \"value\": \"10\"}"); // Now, only param1 should change, param 2 should be the same EXPECT_EQ(p1, 10); @@ -188,16 +192,17 @@ TEST(ParameterHandlerTest, DynamicAndStaticParametersTest) TEST(ParameterHandlerTest, DynamicAndStaticParametersNotVerboseTest) { auto node = std::make_shared("my_node"); - node->declare_parameter("dynamic_int", rclcpp::ParameterValue(7)); - node->declare_parameter("static_int", rclcpp::ParameterValue(7)); - ParametersHandlerWrapper handler(node); + node->declare_parameter("test.dynamic_int", rclcpp::ParameterValue(7)); + node->declare_parameter("test.static_int", rclcpp::ParameterValue(7)); + std::string name = "test"; + ParametersHandlerWrapper handler(node, name); handler.start(); // Get parameters and check they have initial values auto getParameter = handler.getParamGetter(""); int p1 = 0, p2 = 0; - getParameter(p1, "dynamic_int", 0, ParameterType::Dynamic); - getParameter(p2, "static_int", 0, ParameterType::Static); + getParameter(p1, "test.dynamic_int", 0, ParameterType::Dynamic); + getParameter(p2, "test.static_int", 0, ParameterType::Static); EXPECT_EQ(p1, 7); EXPECT_EQ(p2, 7); @@ -211,8 +216,8 @@ TEST(ParameterHandlerTest, DynamicAndStaticParametersNotVerboseTest) rec_param->set_parameters_atomically( { // Don't set default param rclcpp::Parameter("my_node.verbose", false), - rclcpp::Parameter("dynamic_int", 10), - rclcpp::Parameter("static_int", 10) + rclcpp::Parameter("test.dynamic_int", 10), + rclcpp::Parameter("test.static_int", 10) }); auto rc = rclcpp::spin_until_future_complete( @@ -225,7 +230,7 @@ TEST(ParameterHandlerTest, DynamicAndStaticParametersNotVerboseTest) EXPECT_FALSE(result.reason.empty()); EXPECT_EQ( result.reason, std::string("Rejected change to static parameter: ") + - "{\"name\": \"static_int\", \"type\": \"integer\", \"value\": \"10\"}"); + "{\"name\": \"test.static_int\", \"type\": \"integer\", \"value\": \"10\"}"); // Now, only param1 should change, param 2 should be the same EXPECT_EQ(p1, 10); @@ -236,9 +241,10 @@ TEST(ParameterHandlerTest, DynamicAndStaticParametersNotDeclaredTest) { auto node = std::make_shared("my_node"); - node->declare_parameter("dynamic_int", rclcpp::ParameterValue(7)); - node->declare_parameter("static_int", rclcpp::ParameterValue(7)); - ParametersHandlerWrapper handler(node); + node->declare_parameter("test.dynamic_int", rclcpp::ParameterValue(7)); + node->declare_parameter("test.static_int", rclcpp::ParameterValue(7)); + std::string name = "test"; + ParametersHandlerWrapper handler(node, name); handler.start(); // Set verbose true to get more information about bad parameter usage @@ -266,9 +272,9 @@ TEST(ParameterHandlerTest, DynamicAndStaticParametersNotDeclaredTest) // Try to set some parameters that have not been declared via the service client result_future = rec_param->set_parameters_atomically( { - rclcpp::Parameter("static_int", 10), - rclcpp::Parameter("not_declared", true), - rclcpp::Parameter("not_declared2", true), + rclcpp::Parameter("test.static_int", 10), + rclcpp::Parameter("test.not_declared", true), + rclcpp::Parameter("test.not_declared2", true), }); rc = rclcpp::spin_until_future_complete( diff --git a/nav2_mppi_controller/test/path_handler_test.cpp b/nav2_mppi_controller/test/path_handler_test.cpp index 96bf9f7a627..87e6e9ea58d 100644 --- a/nav2_mppi_controller/test/path_handler_test.cpp +++ b/nav2_mppi_controller/test/path_handler_test.cpp @@ -104,7 +104,8 @@ TEST(PathHandlerTests, TestBounds) auto results = costmap_ros->set_parameters_atomically( {rclcpp::Parameter("global_frame", "odom"), rclcpp::Parameter("robot_base_frame", "base_link")}); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); rclcpp_lifecycle::State state; costmap_ros->on_configure(state); @@ -152,7 +153,8 @@ TEST(PathHandlerTests, TestTransforms) node->declare_parameter("dummy.max_robot_pose_search_dist", rclcpp::ParameterValue(99999.9)); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); rclcpp_lifecycle::State state; costmap_ros->on_configure(state); diff --git a/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp b/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp index c867ec678b9..fea66927707 100644 --- a/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp +++ b/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp @@ -26,7 +26,8 @@ using namespace mppi; // NOLINT TEST(TrajectoryVisualizerTests, StateTransition) { auto node = std::make_shared("my_node"); - auto parameters_handler = std::make_unique(node); + std::string name = "test"; + auto parameters_handler = std::make_unique(node, name); TrajectoryVisualizer vis; vis.on_configure(node, "my_name", "map", parameters_handler.get()); @@ -38,7 +39,8 @@ TEST(TrajectoryVisualizerTests, StateTransition) TEST(TrajectoryVisualizerTests, VisPathRepub) { auto node = std::make_shared("my_node"); - auto parameters_handler = std::make_unique(node); + std::string name = "test"; + auto parameters_handler = std::make_unique(node, name); nav_msgs::msg::Path received_path; nav_msgs::msg::Path pub_path; pub_path.header.frame_id = "fake_frame"; @@ -61,7 +63,8 @@ TEST(TrajectoryVisualizerTests, VisPathRepub) TEST(TrajectoryVisualizerTests, VisOptimalTrajectory) { auto node = std::make_shared("my_node"); - auto parameters_handler = std::make_unique(node); + std::string name = "test"; + auto parameters_handler = std::make_unique(node, name); visualization_msgs::msg::MarkerArray received_msg; auto my_sub = node->create_subscription( @@ -123,7 +126,8 @@ TEST(TrajectoryVisualizerTests, VisOptimalTrajectory) TEST(TrajectoryVisualizerTests, VisCandidateTrajectories) { auto node = std::make_shared("my_node"); - auto parameters_handler = std::make_unique(node); + std::string name = "test"; + auto parameters_handler = std::make_unique(node, name); visualization_msgs::msg::MarkerArray received_msg; auto my_sub = node->create_subscription( @@ -150,7 +154,8 @@ TEST(TrajectoryVisualizerTests, VisCandidateTrajectories) TEST(TrajectoryVisualizerTests, VisOptimalPath) { auto node = std::make_shared("my_node"); - auto parameters_handler = std::make_unique(node); + std::string name = "test"; + auto parameters_handler = std::make_unique(node, name); builtin_interfaces::msg::Time cmd_stamp; cmd_stamp.sec = 5; cmd_stamp.nanosec = 10; diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp index e01261434d1..e42c26202a5 100644 --- a/nav2_navfn_planner/src/navfn_planner.cpp +++ b/nav2_navfn_planner/src/navfn_planner.cpp @@ -525,19 +525,21 @@ NavfnPlanner::dynamicParametersCallback(std::vector parameter { rcl_interfaces::msg::SetParametersResult result; for (auto parameter : parameters) { - const auto & type = parameter.get_type(); - const auto & name = parameter.get_name(); - - if (type == ParameterType::PARAMETER_DOUBLE) { - if (name == name_ + ".tolerance") { + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + if(param_name.find(name_ + ".") != 0) { + continue; + } + if (param_type == ParameterType::PARAMETER_DOUBLE) { + if (param_name == name_ + ".tolerance") { tolerance_ = parameter.as_double(); } - } else if (type == ParameterType::PARAMETER_BOOL) { - if (name == name_ + ".use_astar") { + } else if (param_type == ParameterType::PARAMETER_BOOL) { + if (param_name == name_ + ".use_astar") { use_astar_ = parameter.as_bool(); - } else if (name == name_ + ".allow_unknown") { + } else if (param_name == name_ + ".allow_unknown") { allow_unknown_ = parameter.as_bool(); - } else if (name == name_ + ".use_final_approach_orientation") { + } else if (param_name == name_ + ".use_final_approach_orientation") { use_final_approach_orientation_ = parameter.as_bool(); } } diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 09154e75df4..ac1d1efd89d 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -726,11 +726,14 @@ PlannerServer::dynamicParametersCallback(std::vector paramete std::lock_guard lock(dynamic_params_lock_); rcl_interfaces::msg::SetParametersResult result; for (auto parameter : parameters) { - const auto & type = parameter.get_type(); - const auto & name = parameter.get_name(); + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + if (param_name.find('.') != std::string::npos) { + continue; + } - if (type == ParameterType::PARAMETER_DOUBLE) { - if (name == "expected_planner_frequency") { + if (param_type == ParameterType::PARAMETER_DOUBLE) { + if (param_name == "expected_planner_frequency") { if (parameter.as_double() > 0) { max_planner_duration_ = 1 / parameter.as_double(); } else { diff --git a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp index 91e151d1534..293a6dace2e 100644 --- a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp @@ -212,11 +212,15 @@ rcl_interfaces::msg::SetParametersResult ParameterHandler::validateParameterUpda rcl_interfaces::msg::SetParametersResult result; result.successful = true; for (auto parameter : parameters) { - const auto & type = parameter.get_type(); - const auto & name = parameter.get_name(); - - if (type == ParameterType::PARAMETER_DOUBLE) { - if (name == plugin_name_ + ".inflation_cost_scaling_factor" && parameter.as_double() <= 0.0) { + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + if (param_name.find(plugin_name_ + ".") != 0) { + continue; + } + if (param_type == ParameterType::PARAMETER_DOUBLE) { + if (param_name == plugin_name_ + ".inflation_cost_scaling_factor" && + parameter.as_double() <= 0.0) + { RCLCPP_WARN( logger_, "The value inflation_cost_scaling_factor is incorrectly set, " "it should be >0. Ignoring parameter update."); @@ -225,11 +229,11 @@ rcl_interfaces::msg::SetParametersResult ParameterHandler::validateParameterUpda RCLCPP_WARN( logger_, "The value of parameter '%s' is incorrectly set to %f, " "it should be >=0. Ignoring parameter update.", - name.c_str(), parameter.as_double()); + param_name.c_str(), parameter.as_double()); result.successful = false; } - } else if (type == ParameterType::PARAMETER_BOOL) { - if (name == plugin_name_ + ".allow_reversing") { + } else if (param_type == ParameterType::PARAMETER_BOOL) { + if (param_name == plugin_name_ + ".allow_reversing") { if (params_.use_rotate_to_heading && parameter.as_bool()) { RCLCPP_WARN( logger_, "Both use_rotate_to_heading and allow_reversing " @@ -248,75 +252,69 @@ ParameterHandler::updateParametersCallback( rcl_interfaces::msg::SetParametersResult result; std::lock_guard lock_reinit(mutex_); - for (auto parameter : parameters) { - const auto & type = parameter.get_type(); - const auto & name = parameter.get_name(); + for (const auto & parameter : parameters) { + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); - if (type == ParameterType::PARAMETER_DOUBLE) { - if (name == plugin_name_ + ".inflation_cost_scaling_factor") { - if (parameter.as_double() <= 0.0) { - RCLCPP_WARN( - logger_, "The value inflation_cost_scaling_factor is incorrectly set, " - "it should be >0. Ignoring parameter update."); - continue; - } + if (param_type == ParameterType::PARAMETER_DOUBLE) { + if (param_name == plugin_name_ + ".inflation_cost_scaling_factor") { params_.inflation_cost_scaling_factor = parameter.as_double(); - } else if (name == plugin_name_ + ".desired_linear_vel") { + } else if (param_name == plugin_name_ + ".desired_linear_vel") { params_.desired_linear_vel = parameter.as_double(); params_.base_desired_linear_vel = parameter.as_double(); - } else if (name == plugin_name_ + ".lookahead_dist") { + } else if (param_name == plugin_name_ + ".lookahead_dist") { params_.lookahead_dist = parameter.as_double(); - } else if (name == plugin_name_ + ".max_lookahead_dist") { + } else if (param_name == plugin_name_ + ".max_lookahead_dist") { params_.max_lookahead_dist = parameter.as_double(); - } else if (name == plugin_name_ + ".min_lookahead_dist") { + } else if (param_name == plugin_name_ + ".min_lookahead_dist") { params_.min_lookahead_dist = parameter.as_double(); - } else if (name == plugin_name_ + ".lookahead_time") { + } else if (param_name == plugin_name_ + ".lookahead_time") { params_.lookahead_time = parameter.as_double(); - } else if (name == plugin_name_ + ".rotate_to_heading_angular_vel") { + } else if (param_name == plugin_name_ + ".rotate_to_heading_angular_vel") { params_.rotate_to_heading_angular_vel = parameter.as_double(); - } else if (name == plugin_name_ + ".min_approach_linear_velocity") { + } else if (param_name == plugin_name_ + ".min_approach_linear_velocity") { params_.min_approach_linear_velocity = parameter.as_double(); - } else if (name == plugin_name_ + ".curvature_lookahead_dist") { + } else if (param_name == plugin_name_ + ".curvature_lookahead_dist") { params_.curvature_lookahead_dist = parameter.as_double(); - } else if (name == plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot") { + } else if (param_name == plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot") { params_.max_allowed_time_to_collision_up_to_carrot = parameter.as_double(); - } else if (name == plugin_name_ + ".cost_scaling_dist") { + } else if (param_name == plugin_name_ + ".cost_scaling_dist") { params_.cost_scaling_dist = parameter.as_double(); - } else if (name == plugin_name_ + ".cost_scaling_gain") { + } else if (param_name == plugin_name_ + ".cost_scaling_gain") { params_.cost_scaling_gain = parameter.as_double(); - } else if (name == plugin_name_ + ".regulated_linear_scaling_min_radius") { + } else if (param_name == plugin_name_ + ".regulated_linear_scaling_min_radius") { params_.regulated_linear_scaling_min_radius = parameter.as_double(); - } else if (name == plugin_name_ + ".regulated_linear_scaling_min_speed") { + } else if (param_name == plugin_name_ + ".regulated_linear_scaling_min_speed") { params_.regulated_linear_scaling_min_speed = parameter.as_double(); - } else if (name == plugin_name_ + ".max_angular_accel") { + } else if (param_name == plugin_name_ + ".max_angular_accel") { params_.max_angular_accel = parameter.as_double(); - } else if (name == plugin_name_ + ".cancel_deceleration") { + } else if (param_name == plugin_name_ + ".cancel_deceleration") { params_.cancel_deceleration = parameter.as_double(); - } else if (name == plugin_name_ + ".rotate_to_heading_min_angle") { + } else if (param_name == plugin_name_ + ".rotate_to_heading_min_angle") { params_.rotate_to_heading_min_angle = parameter.as_double(); - } else if (name == plugin_name_ + ".transform_tolerance") { + } else if (param_name == plugin_name_ + ".transform_tolerance") { params_.transform_tolerance = parameter.as_double(); - } else if (name == plugin_name_ + ".max_robot_pose_search_dist") { + } else if (param_name == plugin_name_ + ".max_robot_pose_search_dist") { params_.max_robot_pose_search_dist = parameter.as_double(); } - } else if (type == ParameterType::PARAMETER_BOOL) { - if (name == plugin_name_ + ".use_velocity_scaled_lookahead_dist") { + } else if (param_type == ParameterType::PARAMETER_BOOL) { + if (param_name == plugin_name_ + ".use_velocity_scaled_lookahead_dist") { params_.use_velocity_scaled_lookahead_dist = parameter.as_bool(); - } else if (name == plugin_name_ + ".use_regulated_linear_velocity_scaling") { + } else if (param_name == plugin_name_ + ".use_regulated_linear_velocity_scaling") { params_.use_regulated_linear_velocity_scaling = parameter.as_bool(); - } else if (name == plugin_name_ + ".use_fixed_curvature_lookahead") { + } else if (param_name == plugin_name_ + ".use_fixed_curvature_lookahead") { params_.use_fixed_curvature_lookahead = parameter.as_bool(); - } else if (name == plugin_name_ + ".use_cost_regulated_linear_velocity_scaling") { + } else if (param_name == plugin_name_ + ".use_cost_regulated_linear_velocity_scaling") { params_.use_cost_regulated_linear_velocity_scaling = parameter.as_bool(); - } else if (name == plugin_name_ + ".use_collision_detection") { + } else if (param_name == plugin_name_ + ".use_collision_detection") { params_.use_collision_detection = parameter.as_bool(); - } else if (name == plugin_name_ + ".stateful") { + } else if (param_name == plugin_name_ + ".stateful") { params_.stateful = parameter.as_bool(); - } else if (name == plugin_name_ + ".use_rotate_to_heading") { + } else if (param_name == plugin_name_ + ".use_rotate_to_heading") { params_.use_rotate_to_heading = parameter.as_bool(); - } else if (name == plugin_name_ + ".use_cancel_deceleration") { + } else if (param_name == plugin_name_ + ".use_cancel_deceleration") { params_.use_cancel_deceleration = parameter.as_bool(); - } else if (name == plugin_name_ + ".allow_reversing") { + } else if (param_name == plugin_name_ + ".allow_reversing") { if (params_.use_rotate_to_heading && parameter.as_bool()) { RCLCPP_WARN( logger_, "Both use_rotate_to_heading and allow_reversing " @@ -324,7 +322,7 @@ ParameterHandler::updateParametersCallback( continue; } params_.allow_reversing = parameter.as_bool(); - } else if (name == plugin_name_ + ".interpolate_curvature_after_goal") { + } else if (param_name == plugin_name_ + ".interpolate_curvature_after_goal") { params_.interpolate_curvature_after_goal = parameter.as_bool(); } } diff --git a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp index e6e435adbf5..b964cf0166b 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -411,29 +411,32 @@ RotationShimController::dynamicParametersCallback(std::vector std::lock_guard lock_reinit(mutex_); for (auto parameter : parameters) { - const auto & type = parameter.get_type(); - const auto & name = parameter.get_name(); + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + if (param_name.find(plugin_name_ + ".") != 0) { + continue; + } - if (type == ParameterType::PARAMETER_DOUBLE) { - if (name == plugin_name_ + ".angular_dist_threshold") { + if (param_type == ParameterType::PARAMETER_DOUBLE) { + if (param_name == plugin_name_ + ".angular_dist_threshold") { angular_dist_threshold_ = parameter.as_double(); - } else if (name == plugin_name_ + ".forward_sampling_distance") { + } else if (param_name == plugin_name_ + ".forward_sampling_distance") { forward_sampling_distance_ = parameter.as_double(); - } else if (name == plugin_name_ + ".rotate_to_heading_angular_vel") { + } else if (param_name == plugin_name_ + ".rotate_to_heading_angular_vel") { rotate_to_heading_angular_vel_ = parameter.as_double(); - } else if (name == plugin_name_ + ".max_angular_accel") { + } else if (param_name == plugin_name_ + ".max_angular_accel") { max_angular_accel_ = parameter.as_double(); - } else if (name == plugin_name_ + ".simulate_ahead_time") { + } else if (param_name == plugin_name_ + ".simulate_ahead_time") { simulate_ahead_time_ = parameter.as_double(); } - } else if (type == ParameterType::PARAMETER_BOOL) { - if (name == plugin_name_ + ".rotate_to_goal_heading") { + } else if (param_type == ParameterType::PARAMETER_BOOL) { + if (param_name == plugin_name_ + ".rotate_to_goal_heading") { rotate_to_goal_heading_ = parameter.as_bool(); - } else if (name == plugin_name_ + ".rotate_to_heading_once") { + } else if (param_name == plugin_name_ + ".rotate_to_heading_once") { rotate_to_heading_once_ = parameter.as_bool(); - } else if (name == plugin_name_ + ".closed_loop") { + } else if (param_name == plugin_name_ + ".closed_loop") { closed_loop_ = parameter.as_bool(); - } else if (name == plugin_name_ + ".use_path_orientations") { + } else if (param_name == plugin_name_ + ".use_path_orientations") { use_path_orientations_ = parameter.as_bool(); } } diff --git a/nav2_smac_planner/src/smac_planner_2d.cpp b/nav2_smac_planner/src/smac_planner_2d.cpp index 3936fc7157d..1210bc1619c 100644 --- a/nav2_smac_planner/src/smac_planner_2d.cpp +++ b/nav2_smac_planner/src/smac_planner_2d.cpp @@ -362,34 +362,36 @@ SmacPlanner2D::dynamicParametersCallback(std::vector paramete bool reinit_downsampler = false; for (auto parameter : parameters) { - const auto & type = parameter.get_type(); - const auto & name = parameter.get_name(); - - if (type == ParameterType::PARAMETER_DOUBLE) { - if (name == _name + ".tolerance") { + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + if(param_name.find(_name + ".") != 0) { + continue; + } + if (param_type == ParameterType::PARAMETER_DOUBLE) { + if (param_name == _name + ".tolerance") { _tolerance = static_cast(parameter.as_double()); - } else if (name == _name + ".cost_travel_multiplier") { + } else if (param_name == _name + ".cost_travel_multiplier") { reinit_a_star = true; _search_info.cost_penalty = parameter.as_double(); - } else if (name == _name + ".max_planning_time") { + } else if (param_name == _name + ".max_planning_time") { reinit_a_star = true; _max_planning_time = parameter.as_double(); } - } else if (type == ParameterType::PARAMETER_BOOL) { - if (name == _name + ".downsample_costmap") { + } else if (param_type == ParameterType::PARAMETER_BOOL) { + if (param_name == _name + ".downsample_costmap") { reinit_downsampler = true; _downsample_costmap = parameter.as_bool(); - } else if (name == _name + ".allow_unknown") { + } else if (param_name == _name + ".allow_unknown") { reinit_a_star = true; _allow_unknown = parameter.as_bool(); - } else if (name == _name + ".use_final_approach_orientation") { + } else if (param_name == _name + ".use_final_approach_orientation") { _use_final_approach_orientation = parameter.as_bool(); } - } else if (type == ParameterType::PARAMETER_INTEGER) { - if (name == _name + ".downsampling_factor") { + } else if (param_type == ParameterType::PARAMETER_INTEGER) { + if (param_name == _name + ".downsampling_factor") { reinit_downsampler = true; _downsampling_factor = parameter.as_int(); - } else if (name == _name + ".max_iterations") { + } else if (param_name == _name + ".max_iterations") { reinit_a_star = true; _max_iterations = parameter.as_int(); if (_max_iterations <= 0) { @@ -398,7 +400,7 @@ SmacPlanner2D::dynamicParametersCallback(std::vector paramete "disabling maximum iterations."); _max_iterations = std::numeric_limits::max(); } - } else if (name == _name + ".max_on_approach_iterations") { + } else if (param_name == _name + ".max_on_approach_iterations") { reinit_a_star = true; _max_on_approach_iterations = parameter.as_int(); if (_max_on_approach_iterations <= 0) { @@ -407,7 +409,7 @@ SmacPlanner2D::dynamicParametersCallback(std::vector paramete "disabling tolerance and on approach iterations."); _max_on_approach_iterations = std::numeric_limits::max(); } - } else if (name == _name + ".terminal_checking_interval") { + } else if (param_name == _name + ".terminal_checking_interval") { reinit_a_star = true; _terminal_checking_interval = parameter.as_int(); } diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index d0e5441ddfd..d6f4ec26fb8 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -620,19 +620,21 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para bool reinit_smoother = false; for (auto parameter : parameters) { - const auto & type = parameter.get_type(); - const auto & name = parameter.get_name(); - - if (type == ParameterType::PARAMETER_DOUBLE) { - if (name == _name + ".max_planning_time") { + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + if(param_name.find(_name + ".") != 0 && param_name != "resolution") { + continue; + } + if (param_type == ParameterType::PARAMETER_DOUBLE) { + if (param_name == _name + ".max_planning_time") { reinit_a_star = true; _max_planning_time = parameter.as_double(); - } else if (name == _name + ".tolerance") { + } else if (param_name == _name + ".tolerance") { _tolerance = static_cast(parameter.as_double()); - } else if (name == _name + ".lookup_table_size") { + } else if (param_name == _name + ".lookup_table_size") { reinit_a_star = true; _lookup_table_size = parameter.as_double(); - } else if (name == _name + ".minimum_turning_radius") { + } else if (param_name == _name + ".minimum_turning_radius") { reinit_a_star = true; if (_smoother) { reinit_smoother = true; @@ -645,29 +647,29 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para } _minimum_turning_radius_global_coords = static_cast(parameter.as_double()); - } else if (name == _name + ".reverse_penalty") { + } else if (param_name == _name + ".reverse_penalty") { reinit_a_star = true; _search_info.reverse_penalty = static_cast(parameter.as_double()); - } else if (name == _name + ".change_penalty") { + } else if (param_name == _name + ".change_penalty") { reinit_a_star = true; _search_info.change_penalty = static_cast(parameter.as_double()); - } else if (name == _name + ".non_straight_penalty") { + } else if (param_name == _name + ".non_straight_penalty") { reinit_a_star = true; _search_info.non_straight_penalty = static_cast(parameter.as_double()); - } else if (name == _name + ".cost_penalty") { + } else if (param_name == _name + ".cost_penalty") { reinit_a_star = true; _search_info.cost_penalty = static_cast(parameter.as_double()); - } else if (name == _name + ".analytic_expansion_ratio") { + } else if (param_name == _name + ".analytic_expansion_ratio") { reinit_a_star = true; _search_info.analytic_expansion_ratio = static_cast(parameter.as_double()); - } else if (name == _name + ".analytic_expansion_max_length") { + } else if (param_name == _name + ".analytic_expansion_max_length") { reinit_a_star = true; _search_info.analytic_expansion_max_length = static_cast(parameter.as_double()) / _costmap->getResolution(); - } else if (name == _name + ".analytic_expansion_max_cost") { + } else if (param_name == _name + ".analytic_expansion_max_cost") { reinit_a_star = true; _search_info.analytic_expansion_max_cost = static_cast(parameter.as_double()); - } else if (name == "resolution") { + } else if (param_name == "resolution") { // Special case: When the costmap's resolution changes, need to reinitialize // the controller to have new resolution information RCLCPP_INFO(_logger, "Costmap resolution changed. Reinitializing SmacPlannerHybrid."); @@ -676,35 +678,35 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para reinit_downsampler = true; reinit_smoother = true; } - } else if (type == ParameterType::PARAMETER_BOOL) { - if (name == _name + ".downsample_costmap") { + } else if (param_type == ParameterType::PARAMETER_BOOL) { + if (param_name == _name + ".downsample_costmap") { reinit_downsampler = true; _downsample_costmap = parameter.as_bool(); - } else if (name == _name + ".allow_unknown") { + } else if (param_name == _name + ".allow_unknown") { reinit_a_star = true; _allow_unknown = parameter.as_bool(); - } else if (name == _name + ".cache_obstacle_heuristic") { + } else if (param_name == _name + ".cache_obstacle_heuristic") { reinit_a_star = true; _search_info.cache_obstacle_heuristic = parameter.as_bool(); - } else if (name == _name + ".allow_primitive_interpolation") { + } else if (param_name == _name + ".allow_primitive_interpolation") { _search_info.allow_primitive_interpolation = parameter.as_bool(); reinit_a_star = true; - } else if (name == _name + ".smooth_path") { + } else if (param_name == _name + ".smooth_path") { if (parameter.as_bool()) { reinit_smoother = true; } else { _smoother.reset(); } - } else if (name == _name + ".analytic_expansion_max_cost_override") { + } else if (param_name == _name + ".analytic_expansion_max_cost_override") { _search_info.analytic_expansion_max_cost_override = parameter.as_bool(); reinit_a_star = true; } - } else if (type == ParameterType::PARAMETER_INTEGER) { - if (name == _name + ".downsampling_factor") { + } else if (param_type == ParameterType::PARAMETER_INTEGER) { + if (param_name == _name + ".downsampling_factor") { reinit_a_star = true; reinit_downsampler = true; _downsampling_factor = parameter.as_int(); - } else if (name == _name + ".max_iterations") { + } else if (param_name == _name + ".max_iterations") { reinit_a_star = true; _max_iterations = parameter.as_int(); if (_max_iterations <= 0) { @@ -713,7 +715,7 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para "disabling maximum iterations."); _max_iterations = std::numeric_limits::max(); } - } else if (name == _name + ".max_on_approach_iterations") { + } else if (param_name == _name + ".max_on_approach_iterations") { reinit_a_star = true; _max_on_approach_iterations = parameter.as_int(); if (_max_on_approach_iterations <= 0) { @@ -722,10 +724,10 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para "disabling tolerance and on approach iterations."); _max_on_approach_iterations = std::numeric_limits::max(); } - } else if (name == _name + ".terminal_checking_interval") { + } else if (param_name == _name + ".terminal_checking_interval") { reinit_a_star = true; _terminal_checking_interval = parameter.as_int(); - } else if (name == _name + ".angle_quantization_bins") { + } else if (param_name == _name + ".angle_quantization_bins") { reinit_collision_checker = true; reinit_a_star = true; int angle_quantizations = parameter.as_int(); @@ -739,7 +741,7 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para ); _coarse_search_resolution = 1; } - } else if (name == _name + ".coarse_search_resolution") { + } else if (param_name == _name + ".coarse_search_resolution") { _coarse_search_resolution = parameter.as_int(); if (_coarse_search_resolution <= 0) { RCLCPP_WARN( @@ -757,8 +759,8 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para _coarse_search_resolution = 1; } } - } else if (type == ParameterType::PARAMETER_STRING) { - if (name == _name + ".motion_model_for_search") { + } else if (param_type == ParameterType::PARAMETER_STRING) { + if (param_name == _name + ".motion_model_for_search") { reinit_a_star = true; _motion_model = fromString(parameter.as_string()); if (_motion_model == MotionModel::UNKNOWN) { @@ -768,7 +770,7 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para "valid options are MOORE, VON_NEUMANN, DUBIN, REEDS_SHEPP.", _motion_model_for_search.c_str()); } - } else if (name == _name + ".goal_heading_mode") { + } else if (param_name == _name + ".goal_heading_mode") { std::string goal_heading_type = parameter.as_string(); GoalHeadingMode goal_heading_mode = fromStringToGH(goal_heading_type); RCLCPP_INFO( diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index d86d245d8e0..4968068e534 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -542,66 +542,68 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector par bool reinit_smoother = false; for (auto parameter : parameters) { - const auto & type = parameter.get_type(); - const auto & name = parameter.get_name(); - - if (type == ParameterType::PARAMETER_DOUBLE) { - if (name == _name + ".max_planning_time") { + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + if(param_name.find(_name + ".") != 0) { + continue; + } + if (param_type == ParameterType::PARAMETER_DOUBLE) { + if (param_name == _name + ".max_planning_time") { reinit_a_star = true; _max_planning_time = parameter.as_double(); - } else if (name == _name + ".tolerance") { + } else if (param_name == _name + ".tolerance") { _tolerance = static_cast(parameter.as_double()); - } else if (name == _name + ".lookup_table_size") { + } else if (param_name == _name + ".lookup_table_size") { reinit_a_star = true; _lookup_table_size = parameter.as_double(); - } else if (name == _name + ".reverse_penalty") { + } else if (param_name == _name + ".reverse_penalty") { reinit_a_star = true; _search_info.reverse_penalty = static_cast(parameter.as_double()); - } else if (name == _name + ".change_penalty") { + } else if (param_name == _name + ".change_penalty") { reinit_a_star = true; _search_info.change_penalty = static_cast(parameter.as_double()); - } else if (name == _name + ".non_straight_penalty") { + } else if (param_name == _name + ".non_straight_penalty") { reinit_a_star = true; _search_info.non_straight_penalty = static_cast(parameter.as_double()); - } else if (name == _name + ".cost_penalty") { + } else if (param_name == _name + ".cost_penalty") { reinit_a_star = true; _search_info.cost_penalty = static_cast(parameter.as_double()); - } else if (name == _name + ".rotation_penalty") { + } else if (param_name == _name + ".rotation_penalty") { reinit_a_star = true; _search_info.rotation_penalty = static_cast(parameter.as_double()); - } else if (name == _name + ".analytic_expansion_ratio") { + } else if (param_name == _name + ".analytic_expansion_ratio") { reinit_a_star = true; _search_info.analytic_expansion_ratio = static_cast(parameter.as_double()); - } else if (name == _name + ".analytic_expansion_max_length") { + } else if (param_name == _name + ".analytic_expansion_max_length") { reinit_a_star = true; _search_info.analytic_expansion_max_length = static_cast(parameter.as_double()) / _costmap->getResolution(); - } else if (name == _name + ".analytic_expansion_max_cost") { + } else if (param_name == _name + ".analytic_expansion_max_cost") { reinit_a_star = true; _search_info.analytic_expansion_max_cost = static_cast(parameter.as_double()); } - } else if (type == ParameterType::PARAMETER_BOOL) { - if (name == _name + ".allow_unknown") { + } else if (param_type == ParameterType::PARAMETER_BOOL) { + if (param_name == _name + ".allow_unknown") { reinit_a_star = true; _allow_unknown = parameter.as_bool(); - } else if (name == _name + ".cache_obstacle_heuristic") { + } else if (param_name == _name + ".cache_obstacle_heuristic") { reinit_a_star = true; _search_info.cache_obstacle_heuristic = parameter.as_bool(); - } else if (name == _name + ".allow_reverse_expansion") { + } else if (param_name == _name + ".allow_reverse_expansion") { reinit_a_star = true; _search_info.allow_reverse_expansion = parameter.as_bool(); - } else if (name == _name + ".smooth_path") { + } else if (param_name == _name + ".smooth_path") { if (parameter.as_bool()) { reinit_smoother = true; } else { _smoother.reset(); } - } else if (name == _name + ".analytic_expansion_max_cost_override") { + } else if (param_name == _name + ".analytic_expansion_max_cost_override") { _search_info.analytic_expansion_max_cost_override = parameter.as_bool(); reinit_a_star = true; } - } else if (type == ParameterType::PARAMETER_INTEGER) { - if (name == _name + ".max_iterations") { + } else if (param_type == ParameterType::PARAMETER_INTEGER) { + if (param_name == _name + ".max_iterations") { reinit_a_star = true; _max_iterations = parameter.as_int(); if (_max_iterations <= 0) { @@ -610,7 +612,7 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector par "disabling maximum iterations."); _max_iterations = std::numeric_limits::max(); } - } else if (name == _name + ".max_on_approach_iterations") { + } else if (param_name == _name + ".max_on_approach_iterations") { reinit_a_star = true; _max_on_approach_iterations = parameter.as_int(); if (_max_on_approach_iterations <= 0) { @@ -619,10 +621,10 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector par "disabling tolerance and on approach iterations."); _max_on_approach_iterations = std::numeric_limits::max(); } - } else if (name == _name + ".terminal_checking_interval") { + } else if (param_name == _name + ".terminal_checking_interval") { reinit_a_star = true; _terminal_checking_interval = parameter.as_int(); - } else if (name == _name + ".coarse_search_resolution") { + } else if (param_name == _name + ".coarse_search_resolution") { _coarse_search_resolution = parameter.as_int(); if (_coarse_search_resolution <= 0) { RCLCPP_WARN( @@ -640,8 +642,8 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector par _coarse_search_resolution = 1; } } - } else if (type == ParameterType::PARAMETER_STRING) { - if (name == _name + ".lattice_filepath") { + } else if (param_type == ParameterType::PARAMETER_STRING) { + if (param_name == _name + ".lattice_filepath") { reinit_a_star = true; if (_smoother) { reinit_smoother = true; @@ -657,7 +659,7 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector par ); _coarse_search_resolution = 1; } - } else if (name == _name + ".goal_heading_mode") { + } else if (param_name == _name + ".goal_heading_mode") { std::string goal_heading_type = parameter.as_string(); GoalHeadingMode goal_heading_mode = fromStringToGH(goal_heading_type); RCLCPP_INFO( diff --git a/nav2_theta_star_planner/src/theta_star_planner.cpp b/nav2_theta_star_planner/src/theta_star_planner.cpp index df90b2b4aff..202516daa9d 100644 --- a/nav2_theta_star_planner/src/theta_star_planner.cpp +++ b/nav2_theta_star_planner/src/theta_star_planner.cpp @@ -238,26 +238,28 @@ ThetaStarPlanner::dynamicParametersCallback(std::vector param { rcl_interfaces::msg::SetParametersResult result; for (auto parameter : parameters) { - const auto & type = parameter.get_type(); - const auto & name = parameter.get_name(); - - if (type == ParameterType::PARAMETER_INTEGER) { - if (name == name_ + ".how_many_corners") { + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + if(param_name.find(name_ + ".") != 0) { + continue; + } + if (param_type == ParameterType::PARAMETER_INTEGER) { + if (param_name == name_ + ".how_many_corners") { planner_->how_many_corners_ = parameter.as_int(); } - if (name == name_ + ".terminal_checking_interval") { + if (param_name == name_ + ".terminal_checking_interval") { planner_->terminal_checking_interval_ = parameter.as_int(); } - } else if (type == ParameterType::PARAMETER_DOUBLE) { - if (name == name_ + ".w_euc_cost") { + } else if (param_type == ParameterType::PARAMETER_DOUBLE) { + if (param_name == name_ + ".w_euc_cost") { planner_->w_euc_cost_ = parameter.as_double(); - } else if (name == name_ + ".w_traversal_cost") { + } else if (param_name == name_ + ".w_traversal_cost") { planner_->w_traversal_cost_ = parameter.as_double(); } - } else if (type == ParameterType::PARAMETER_BOOL) { - if (name == name_ + ".use_final_approach_orientation") { + } else if (param_type == ParameterType::PARAMETER_BOOL) { + if (param_name == name_ + ".use_final_approach_orientation") { use_final_approach_orientation_ = parameter.as_bool(); - } else if (name == name_ + ".allow_unknown") { + } else if (param_name == name_ + ".allow_unknown") { planner_->allow_unknown_ = parameter.as_bool(); } } diff --git a/nav2_velocity_smoother/src/velocity_smoother.cpp b/nav2_velocity_smoother/src/velocity_smoother.cpp index 3e2fb782c81..cfce20042ff 100644 --- a/nav2_velocity_smoother/src/velocity_smoother.cpp +++ b/nav2_velocity_smoother/src/velocity_smoother.cpp @@ -397,11 +397,14 @@ VelocitySmoother::dynamicParametersCallback(std::vector param result.successful = true; for (auto parameter : parameters) { - const auto & type = parameter.get_type(); - const auto & name = parameter.get_name(); + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + if (param_name.find('.') != std::string::npos) { + continue; + } - if (type == ParameterType::PARAMETER_DOUBLE) { - if (name == "smoothing_frequency") { + if (param_type == ParameterType::PARAMETER_DOUBLE) { + if (param_name == "smoothing_frequency") { smoothing_frequency_ = parameter.as_double(); if (timer_) { timer_->cancel(); @@ -412,22 +415,23 @@ VelocitySmoother::dynamicParametersCallback(std::vector param timer_ = this->create_wall_timer( std::chrono::milliseconds(static_cast(timer_duration_ms)), std::bind(&VelocitySmoother::smootherTimer, this)); - } else if (name == "velocity_timeout") { + } else if (param_name == "velocity_timeout") { velocity_timeout_ = rclcpp::Duration::from_seconds(parameter.as_double()); - } else if (name == "odom_duration") { + } else if (param_name == "odom_duration") { odom_duration_ = parameter.as_double(); odom_smoother_ = std::make_unique( shared_from_this(), odom_duration_, odom_topic_); } - } else if (type == ParameterType::PARAMETER_DOUBLE_ARRAY) { + } else if (param_type == ParameterType::PARAMETER_DOUBLE_ARRAY) { if (parameter.as_double_array().size() != 3) { - RCLCPP_WARN(get_logger(), "Invalid size of parameter %s. Must be size 3", name.c_str()); + RCLCPP_WARN(get_logger(), "Invalid size of parameter %s. Must be size 3", + param_name.c_str()); result.successful = false; break; } - if (name == "max_velocity") { + if (param_name == "max_velocity") { for (unsigned int i = 0; i != 3; i++) { if (parameter.as_double_array()[i] < 0.0) { RCLCPP_WARN( @@ -439,7 +443,7 @@ VelocitySmoother::dynamicParametersCallback(std::vector param if (result.successful) { max_velocities_ = parameter.as_double_array(); } - } else if (name == "min_velocity") { + } else if (param_name == "min_velocity") { for (unsigned int i = 0; i != 3; i++) { if (parameter.as_double_array()[i] > 0.0) { RCLCPP_WARN( @@ -451,7 +455,7 @@ VelocitySmoother::dynamicParametersCallback(std::vector param if (result.successful) { min_velocities_ = parameter.as_double_array(); } - } else if (name == "max_accel") { + } else if (param_name == "max_accel") { for (unsigned int i = 0; i != 3; i++) { if (parameter.as_double_array()[i] < 0.0) { RCLCPP_WARN( @@ -463,7 +467,7 @@ VelocitySmoother::dynamicParametersCallback(std::vector param if (result.successful) { max_accels_ = parameter.as_double_array(); } - } else if (name == "max_decel") { + } else if (param_name == "max_decel") { for (unsigned int i = 0; i != 3; i++) { if (parameter.as_double_array()[i] > 0.0) { RCLCPP_WARN( @@ -475,11 +479,11 @@ VelocitySmoother::dynamicParametersCallback(std::vector param if (result.successful) { max_decels_ = parameter.as_double_array(); } - } else if (name == "deadband_velocity") { + } else if (param_name == "deadband_velocity") { deadband_velocities_ = parameter.as_double_array(); } - } else if (type == ParameterType::PARAMETER_STRING) { - if (name == "feedback") { + } else if (param_type == ParameterType::PARAMETER_STRING) { + if (param_name == "feedback") { if (parameter.as_string() == "OPEN_LOOP") { open_loop_ = true; odom_smoother_.reset(); @@ -494,7 +498,7 @@ VelocitySmoother::dynamicParametersCallback(std::vector param result.successful = false; break; } - } else if (name == "odom_topic") { + } else if (param_name == "odom_topic") { odom_topic_ = parameter.as_string(); odom_smoother_ = std::make_unique( diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 099d0f98c9c..cd3993dc4e0 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -468,15 +468,18 @@ WaypointFollower::dynamicParametersCallback(std::vector param rcl_interfaces::msg::SetParametersResult result; for (auto parameter : parameters) { - const auto & type = parameter.get_type(); - const auto & name = parameter.get_name(); + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + if (param_name.find('.') != std::string::npos) { + continue; + } - if (type == ParameterType::PARAMETER_INTEGER) { - if (name == "loop_rate") { + if (param_type == ParameterType::PARAMETER_INTEGER) { + if (param_name == "loop_rate") { loop_rate_ = parameter.as_int(); } - } else if (type == ParameterType::PARAMETER_BOOL) { - if (name == "stop_on_failure") { + } else if (param_type == ParameterType::PARAMETER_BOOL) { + if (param_name == "stop_on_failure") { stop_on_failure_ = parameter.as_bool(); } } From 2eb91b4c4e5cb746133139c49fa5cec1350a6ae0 Mon Sep 17 00:00:00 2001 From: Nils-Christian Iseke <48475933+Nils-ChristianIseke@users.noreply.github.com> Date: Fri, 23 May 2025 21:46:50 +0200 Subject: [PATCH 19/46] Increase Readability of testing. (#5190) Signed-off-by: Nils-Christian Iseke --- .../test/parameter_handler_test.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/nav2_mppi_controller/test/parameter_handler_test.cpp b/nav2_mppi_controller/test/parameter_handler_test.cpp index 93392970869..74c26b63c49 100644 --- a/nav2_mppi_controller/test/parameter_handler_test.cpp +++ b/nav2_mppi_controller/test/parameter_handler_test.cpp @@ -119,24 +119,24 @@ TEST(ParameterHandlerTest, PrePostDynamicCallbackTest) TEST(ParameterHandlerTest, GetSystemParamsTest) { auto node = std::make_shared("my_node"); - node->declare_parameter("test.param1", rclcpp::ParameterValue(true)); - node->declare_parameter("ns.test.param2", rclcpp::ParameterValue(7)); + std::string name = "test"; + node->declare_parameter("param1", rclcpp::ParameterValue(true)); + node->declare_parameter(name + ".param2", rclcpp::ParameterValue(7)); // Get parameters in global namespace and in subnamespaces - std::string name = "test"; ParametersHandler handler(node, name); auto getParameter = handler.getParamGetter(""); bool p1 = false; int p2 = 0; - getParameter(p1, "test.param1", false); - getParameter(p2, "ns.test.param2", 0); + getParameter(p1, "param1", false); + getParameter(p2, name + ".param2", 0); EXPECT_EQ(p1, true); EXPECT_EQ(p2, 7); // Get parameters in subnamespaces using name semantics of getter - auto getParameter2 = handler.getParamGetter("ns"); + auto getParameter2 = handler.getParamGetter(name); p2 = 0; - getParameter2(p2, "test.param2", 0); + getParameter2(p2, "param2", 0); EXPECT_EQ(p2, 7); } From c2d1c519a5665357496b1a0a57280f2532d0b3e2 Mon Sep 17 00:00:00 2001 From: Leander Stephen D'Souza Date: Tue, 27 May 2025 19:12:19 +0100 Subject: [PATCH 20/46] Add value rewrites to RewrittenYaml (#5191) * Added context manager for safe file closing and prevent warnings. Signed-off-by: Leander Stephen D'Souza * Implement value_rewrites to ReWrittenYaml. Signed-off-by: Leander Stephen D'Souza * Use placeholders for costmap filters using the new value rewrite feature. Signed-off-by: Leander Stephen D'Souza * Enable system tests to use value rewrites. Signed-off-by: Leander Stephen D'Souza * Define remappings for costmap filters instead of inline substitution. Signed-off-by: Leander Stephen D'Souza --------- Signed-off-by: Leander Stephen D'Souza --- .gitignore | 1 + nav2_bringup/launch/bringup_launch.py | 6 + nav2_bringup/launch/keepout_zone_launch.py | 5 + nav2_bringup/launch/speed_zone_launch.py | 5 + nav2_bringup/params/nav2_params.yaml | 6 +- nav2_common/CMakeLists.txt | 13 ++ .../nav2_common/launch/rewritten_yaml.py | 43 +++++- nav2_common/package.xml | 4 + nav2_common/test/test_rewritten_yaml.py | 133 ++++++++++++++++++ .../test_assisted_teleop_behavior_launch.py | 7 +- .../backup/test_backup_behavior.launch.py | 4 + .../test_drive_on_heading_behavior.launch.py | 4 + .../spin/test_spin_behavior.launch.py | 4 + .../wait/test_wait_behavior_launch.py | 7 +- .../src/route/test_route_launch.py | 4 + .../test_system_failure_launch.py | 4 + .../src/waypoint_follower/test_case_launch.py | 4 + 17 files changed, 245 insertions(+), 9 deletions(-) create mode 100644 nav2_common/test/test_rewritten_yaml.py diff --git a/.gitignore b/.gitignore index 203501ea402..bb346f49ba9 100644 --- a/.gitignore +++ b/.gitignore @@ -45,6 +45,7 @@ install __pycache__/ *.py[cod] .ipynb_checkpoints +.pytest_cache/ sphinx_doc/_build diff --git a/nav2_bringup/launch/bringup_launch.py b/nav2_bringup/launch/bringup_launch.py index 2725a0244dc..ba6fa035c11 100644 --- a/nav2_bringup/launch/bringup_launch.py +++ b/nav2_bringup/launch/bringup_launch.py @@ -51,11 +51,17 @@ def generate_launch_description() -> LaunchDescription: # Map fully qualified names to relative ones so the node's namespace can be prepended. remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] + yaml_substitutions = { + 'KEEPOUT_ZONE_ENABLED': use_keepout_zones, + 'SPEED_ZONE_ENABLED': use_speed_zones, + } + configured_params = ParameterFile( RewrittenYaml( source_file=params_file, root_key=namespace, param_rewrites={}, + value_rewrites=yaml_substitutions, convert_types=True, ), allow_substs=True, diff --git a/nav2_bringup/launch/keepout_zone_launch.py b/nav2_bringup/launch/keepout_zone_launch.py index 502b6de4548..2a8a1925bd6 100644 --- a/nav2_bringup/launch/keepout_zone_launch.py +++ b/nav2_bringup/launch/keepout_zone_launch.py @@ -45,11 +45,16 @@ def generate_launch_description() -> LaunchDescription: # Map fully qualified names to relative ones so the node's namespace can be prepended. remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] + yaml_substitutions = { + 'KEEPOUT_ZONE_ENABLED': use_keepout_zones, + } + configured_params = ParameterFile( RewrittenYaml( source_file=params_file, root_key=namespace, param_rewrites={}, + value_rewrites=yaml_substitutions, convert_types=True, ), allow_substs=True, diff --git a/nav2_bringup/launch/speed_zone_launch.py b/nav2_bringup/launch/speed_zone_launch.py index 240cc55a3b1..02f8254231b 100644 --- a/nav2_bringup/launch/speed_zone_launch.py +++ b/nav2_bringup/launch/speed_zone_launch.py @@ -45,11 +45,16 @@ def generate_launch_description() -> LaunchDescription: # Map fully qualified names to relative ones so the node's namespace can be prepended. remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] + yaml_substitutions = { + 'SPEED_ZONE_ENABLED': use_speed_zones, + } + configured_params = ParameterFile( RewrittenYaml( source_file=params_file, root_key=namespace, param_rewrites={}, + value_rewrites=yaml_substitutions, convert_types=True, ), allow_substs=True, diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 19c1edbf591..7e9261e1564 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -227,7 +227,7 @@ local_costmap: filters: ["keepout_filter"] keepout_filter: plugin: "nav2_costmap_2d::KeepoutFilter" - enabled: True + enabled: KEEPOUT_ZONE_ENABLED filter_info_topic: "keepout_costmap_filter_info" inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" @@ -279,11 +279,11 @@ global_costmap: filters: ["keepout_filter", "speed_filter"] keepout_filter: plugin: "nav2_costmap_2d::KeepoutFilter" - enabled: True + enabled: KEEPOUT_ZONE_ENABLED filter_info_topic: "keepout_costmap_filter_info" speed_filter: plugin: "nav2_costmap_2d::SpeedFilter" - enabled: True + enabled: SPEED_ZONE_ENABLED filter_info_topic: "speed_costmap_filter_info" speed_limit_topic: "speed_limit" obstacle_layer: diff --git a/nav2_common/CMakeLists.txt b/nav2_common/CMakeLists.txt index 139835a896b..1f79afd2c96 100644 --- a/nav2_common/CMakeLists.txt +++ b/nav2_common/CMakeLists.txt @@ -15,3 +15,16 @@ install( DIRECTORY cmake DESTINATION share/${PROJECT_NAME} ) + +install( + DIRECTORY test + DESTINATION share/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_cmake_test REQUIRED) + find_package(ament_cmake_pytest REQUIRED) + + ament_add_pytest_test(test_rewritten_yaml test/test_rewritten_yaml.py + ) +endif() diff --git a/nav2_common/nav2_common/launch/rewritten_yaml.py b/nav2_common/nav2_common/launch/rewritten_yaml.py index 49ec39b5377..e475dc410fc 100644 --- a/nav2_common/nav2_common/launch/rewritten_yaml.py +++ b/nav2_common/nav2_common/launch/rewritten_yaml.py @@ -48,6 +48,7 @@ def __init__( param_rewrites: dict[str, launch.SomeSubstitutionsType], root_key: Optional[launch.SomeSubstitutionsType] = None, key_rewrites: Optional[dict[str, launch.SomeSubstitutionsType]] = None, + value_rewrites: Optional[dict[str, launch.SomeSubstitutionsType]] = None, convert_types: bool = False, ) -> None: super().__init__() @@ -58,6 +59,7 @@ def __init__( :param: param_rewrites mappings to replace :param: root_key if provided, the contents are placed under this key :param: key_rewrites keys of mappings to replace + :param: value_rewrites values to replace :param: convert_types whether to attempt converting the string to a number or boolean """ @@ -68,6 +70,7 @@ def __init__( normalize_to_list_of_substitutions(source_file) self.__param_rewrites = {} self.__key_rewrites = {} + self.__value_rewrites = {} self.__convert_types = convert_types self.__root_key = None for key in param_rewrites: @@ -79,6 +82,11 @@ def __init__( self.__key_rewrites[key] = normalize_to_list_of_substitutions( key_rewrites[key] ) + if value_rewrites is not None: + for value in value_rewrites: + self.__value_rewrites[value] = normalize_to_list_of_substitutions( + value_rewrites[value] + ) if root_key is not None: self.__root_key = normalize_to_list_of_substitutions(root_key) @@ -94,11 +102,15 @@ def describe(self) -> str: def perform(self, context: launch.LaunchContext) -> str: yaml_filename = launch.utilities.perform_substitutions(context, self.name) rewritten_yaml = tempfile.NamedTemporaryFile(mode='w', delete=False) - param_rewrites, keys_rewrites = self.resolve_rewrites(context) - data = yaml.safe_load(open(yaml_filename)) + param_rewrites, keys_rewrites, value_rewrites = self.resolve_rewrites(context) + + with open(yaml_filename, 'r') as yaml_file: + data = yaml.safe_load(yaml_file) + self.substitute_params(data, param_rewrites) self.add_params(data, param_rewrites) self.substitute_keys(data, keys_rewrites) + self.substitute_values(data, value_rewrites) if self.__root_key is not None: root_key = launch.utilities.perform_substitutions(context, self.__root_key) if root_key: @@ -108,7 +120,7 @@ def perform(self, context: launch.LaunchContext) -> str: return rewritten_yaml.name def resolve_rewrites(self, context: launch.LaunchContext) -> \ - tuple[dict[str, str], dict[str, str]]: + tuple[dict[str, str], dict[str, str], dict[str, str]]: resolved_params = {} for key in self.__param_rewrites: resolved_params[key] = launch.utilities.perform_substitutions( @@ -119,7 +131,12 @@ def resolve_rewrites(self, context: launch.LaunchContext) -> \ resolved_keys[key] = launch.utilities.perform_substitutions( context, self.__key_rewrites[key] ) - return resolved_params, resolved_keys + resolved_values = {} + for value in self.__value_rewrites: + resolved_values[value] = launch.utilities.perform_substitutions( + context, self.__value_rewrites[value] + ) + return resolved_params, resolved_keys, resolved_values def substitute_params(self, yaml: dict[str, YamlValue], param_rewrites: dict[str, str]) -> None: @@ -149,6 +166,24 @@ def add_params(self, yaml: dict[str, YamlValue], if 'ros__parameters' in yaml_keys: yaml = self.updateYamlPathVals(yaml, yaml_keys, new_val) + def substitute_values( + self, yaml: dict[str, YamlValue], + value_rewrites: dict[str, str]) -> None: + + def process_value(value: YamlValue) -> YamlValue: + if isinstance(value, dict): + for k, v in list(value.items()): + value[k] = process_value(v) + return value + elif isinstance(value, list): + return [process_value(v) for v in value] + elif str(value) in value_rewrites: + return self.convert(value_rewrites[str(value)]) + return value + + for key in list(yaml.keys()): + yaml[key] = process_value(yaml[key]) + def updateYamlPathVals( self, yaml: dict[str, YamlValue], yaml_key_list: list[str], rewrite_val: YamlValue) -> dict[str, YamlValue]: diff --git a/nav2_common/package.xml b/nav2_common/package.xml index f2cebe1c8f7..675de1fcc2f 100644 --- a/nav2_common/package.xml +++ b/nav2_common/package.xml @@ -20,6 +20,10 @@ ament_cmake_core + ament_cmake_test + ament_cmake_pytest + python3-pytest + ament_cmake diff --git a/nav2_common/test/test_rewritten_yaml.py b/nav2_common/test/test_rewritten_yaml.py new file mode 100644 index 00000000000..2bf69b95f35 --- /dev/null +++ b/nav2_common/test/test_rewritten_yaml.py @@ -0,0 +1,133 @@ +# Copyright (c) 2025 Leander Stephen Desouza +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +import tempfile +from typing import Generator + +import launch +from launch.substitutions import LaunchConfiguration +from nav2_common.launch import RewrittenYaml +import pytest +import yaml + + +class TestRewrittenYamlValueRewrites: + """Test that value rewrites work correctly in RewrittenYaml.""" + + @pytest.fixture(autouse=True) + def setup_teardown(self) -> Generator[None, None, None]: + # Create a temporary YAML file for testing + self.test_yaml = tempfile.NamedTemporaryFile(mode='w', delete=False) + self.test_yaml.write("""\ + param0: placeholder_bool + param1: placeholder_string + param2: placeholder_float + param3: placeholder_int + param4: placeholder_list + param5: placeholder_dict + nested: + param6: placeholder_bool + param7: placeholder_string + param8: placeholder_float + param9: placeholder_int + param10: placeholder_list + param11: placeholder_dict + other_value: 42 + list_values: + - placeholder_bool + - placeholder_string + - placeholder_float + - placeholder_int + - placeholder_list + - placeholder_dict + - normal_value + """) + self.test_yaml.close() + yield + os.unlink(self.test_yaml.name) + + def test_value_rewrites(self) -> None: + """Test that value rewrites work for various types.""" + # Set up launch configurations for our test values + launch_configurations = { + 'test_bool': 'true', + 'test_string': 'replaced_string', + 'test_float': '3.14', + 'test_int': '100', + 'test_list': '["string", 42, 2.718, ["sublist_item1", "sublist_item2"]]', + 'test_dict': ('{"key1": "value1", "key2": 2, "key3": 3.14, ' + '"key4": ["list_item1", "list_item2"]}') + } + context = launch.LaunchContext() + for key, value in launch_configurations.items(): + context.launch_configurations[key] = value + + value_rewrites = { + 'placeholder_bool': LaunchConfiguration('test_bool'), + 'placeholder_string': LaunchConfiguration('test_string'), + 'placeholder_float': LaunchConfiguration('test_float'), + 'placeholder_int': LaunchConfiguration('test_int'), + 'placeholder_list': LaunchConfiguration('test_list'), + 'placeholder_dict': LaunchConfiguration('test_dict'), + } + + rewriter = RewrittenYaml( + source_file=self.test_yaml.name, + param_rewrites={}, + value_rewrites=value_rewrites, + convert_types=True, + ) + output_file = rewriter.perform(context) + + try: + with open(output_file) as f: + result = yaml.safe_load(f) + + assert result['param0'] is True + assert result['param1'] == 'replaced_string' + assert result['param2'] == 3.14 + assert result['param3'] == 100 + assert result['param4'] == \ + '["string", 42, 2.718, ["sublist_item1", "sublist_item2"]]' + assert result['param5'] == \ + '{"key1": "value1", "key2": 2, "key3": 3.14, "key4": ["list_item1", "list_item2"]}' + + # Check nested values + assert result['nested']['param6'] is True + assert result['nested']['param7'] == 'replaced_string' + assert result['nested']['param8'] == 3.14 + assert result['nested']['param9'] == 100 + assert result['nested']['param10'] == \ + '["string", 42, 2.718, ["sublist_item1", "sublist_item2"]]' + assert result['nested']['param11'] == \ + '{"key1": "value1", "key2": 2, "key3": 3.14, "key4": ["list_item1", "list_item2"]}' + + # Check list values + assert result['list_values'][0] is True + assert result['list_values'][1] == 'replaced_string' + assert result['list_values'][2] == 3.14 + assert result['list_values'][3] == 100 + assert result['list_values'][4] == \ + '["string", 42, 2.718, ["sublist_item1", "sublist_item2"]]' + assert result['list_values'][5] == \ + '{"key1": "value1", "key2": 2, "key3": 3.14, "key4": ["list_item1", "list_item2"]}' + + # Check other values remain unchanged + assert result['nested']['other_value'] == 42 + assert result['list_values'][6] == 'normal_value' + + finally: + if os.path.exists(output_file): + os.unlink(output_file) diff --git a/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_launch.py b/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_launch.py index 14e3737c710..50f8450b08b 100755 --- a/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_launch.py +++ b/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_launch.py @@ -50,7 +50,12 @@ def generate_launch_description() -> LaunchDescription: # Replace the `use_astar` setting on the params file configured_params = RewrittenYaml( - source_file=params_file, root_key='', param_rewrites={}, convert_types=True + source_file=params_file, root_key='', param_rewrites={}, + value_rewrites={ + 'KEEPOUT_ZONE_ENABLED': 'False', + 'SPEED_ZONE_ENABLED': 'False', + }, + convert_types=True ) return LaunchDescription( diff --git a/nav2_system_tests/src/behaviors/backup/test_backup_behavior.launch.py b/nav2_system_tests/src/behaviors/backup/test_backup_behavior.launch.py index eb5b5a07e76..60fa938e673 100755 --- a/nav2_system_tests/src/behaviors/backup/test_backup_behavior.launch.py +++ b/nav2_system_tests/src/behaviors/backup/test_backup_behavior.launch.py @@ -44,6 +44,10 @@ def generate_launch_description() -> LaunchDescription: source_file=params_file, root_key='', param_rewrites=param_substitutions, + value_rewrites={ + 'KEEPOUT_ZONE_ENABLED': 'False', + 'SPEED_ZONE_ENABLED': 'False', + }, convert_types=True, ) diff --git a/nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior.launch.py b/nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior.launch.py index d635adf9539..07daadc6ce1 100755 --- a/nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior.launch.py +++ b/nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior.launch.py @@ -44,6 +44,10 @@ def generate_launch_description() -> LaunchDescription: source_file=params_file, root_key='', param_rewrites=param_substitutions, + value_rewrites={ + 'KEEPOUT_ZONE_ENABLED': 'False', + 'SPEED_ZONE_ENABLED': 'False', + }, convert_types=True, ) diff --git a/nav2_system_tests/src/behaviors/spin/test_spin_behavior.launch.py b/nav2_system_tests/src/behaviors/spin/test_spin_behavior.launch.py index 5c48acad34f..0d7c9acfd34 100755 --- a/nav2_system_tests/src/behaviors/spin/test_spin_behavior.launch.py +++ b/nav2_system_tests/src/behaviors/spin/test_spin_behavior.launch.py @@ -44,6 +44,10 @@ def generate_launch_description() -> LaunchDescription: source_file=params_file, root_key='', param_rewrites=param_substitutions, + value_rewrites={ + 'KEEPOUT_ZONE_ENABLED': 'False', + 'SPEED_ZONE_ENABLED': 'False', + }, convert_types=True, ) diff --git a/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py b/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py index c2956735ae3..b7f5009b9a5 100755 --- a/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py +++ b/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py @@ -53,7 +53,12 @@ def generate_launch_description() -> LaunchDescription: # Replace the `use_astar` setting on the params file configured_params = RewrittenYaml( - source_file=params_file, root_key='', param_rewrites={}, convert_types=True + source_file=params_file, root_key='', param_rewrites={}, + value_rewrites={ + 'KEEPOUT_ZONE_ENABLED': 'False', + 'SPEED_ZONE_ENABLED': 'False', + }, + convert_types=True ) return LaunchDescription( diff --git a/nav2_system_tests/src/route/test_route_launch.py b/nav2_system_tests/src/route/test_route_launch.py index 752470fb815..e475e84437a 100755 --- a/nav2_system_tests/src/route/test_route_launch.py +++ b/nav2_system_tests/src/route/test_route_launch.py @@ -73,6 +73,10 @@ def generate_launch_description() -> LaunchDescription: source_file=params_file, root_key='', param_rewrites=param_substitutions, + value_rewrites={ + 'KEEPOUT_ZONE_ENABLED': 'False', + 'SPEED_ZONE_ENABLED': 'False', + }, convert_types=True, ) diff --git a/nav2_system_tests/src/system_failure/test_system_failure_launch.py b/nav2_system_tests/src/system_failure/test_system_failure_launch.py index 46654ffac52..861308108a0 100755 --- a/nav2_system_tests/src/system_failure/test_system_failure_launch.py +++ b/nav2_system_tests/src/system_failure/test_system_failure_launch.py @@ -59,6 +59,10 @@ def generate_launch_description() -> LaunchDescription: source_file=params_file, root_key='', param_rewrites=param_substitutions, + value_rewrites={ + 'KEEPOUT_ZONE_ENABLED': 'False', + 'SPEED_ZONE_ENABLED': 'False', + }, convert_types=True, ) diff --git a/nav2_system_tests/src/waypoint_follower/test_case_launch.py b/nav2_system_tests/src/waypoint_follower/test_case_launch.py index 9e5c8ccd3e6..cbb793118f9 100755 --- a/nav2_system_tests/src/waypoint_follower/test_case_launch.py +++ b/nav2_system_tests/src/waypoint_follower/test_case_launch.py @@ -57,6 +57,10 @@ def generate_launch_description() -> LaunchDescription: source_file=params_file, root_key='', param_rewrites=param_substitutions, + value_rewrites={ + 'KEEPOUT_ZONE_ENABLED': 'False', + 'SPEED_ZONE_ENABLED': 'False', + }, convert_types=True) context = LaunchContext() From 2bbfa8588448a6d91df6d78e98f3d8142c07a7cf Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 27 May 2025 17:33:46 -0700 Subject: [PATCH 21/46] Updates for Kilted Branch off (#5199) * Adding kilted build to main branch workflow Signed-off-by: Steve Macenski * Updating kilted for ci image build Signed-off-by: Steve Macenski * Bumping to 1.4.0 for kilted release Signed-off-by: Steve Macenski --------- Signed-off-by: Steve Macenski --- .github/workflows/build_main_against_distros.yml | 2 +- .github/workflows/update_ci_image.yaml | 1 + nav2_amcl/package.xml | 2 +- nav2_behavior_tree/package.xml | 2 +- nav2_behaviors/package.xml | 2 +- nav2_bringup/package.xml | 2 +- nav2_bt_navigator/package.xml | 2 +- nav2_collision_monitor/package.xml | 2 +- nav2_common/package.xml | 2 +- nav2_constrained_smoother/package.xml | 2 +- nav2_controller/package.xml | 2 +- nav2_core/package.xml | 2 +- nav2_costmap_2d/package.xml | 2 +- nav2_docking/opennav_docking/package.xml | 2 +- nav2_docking/opennav_docking_bt/package.xml | 2 +- nav2_docking/opennav_docking_core/package.xml | 2 +- nav2_dwb_controller/costmap_queue/package.xml | 2 +- nav2_dwb_controller/dwb_core/package.xml | 2 +- nav2_dwb_controller/dwb_critics/package.xml | 2 +- nav2_dwb_controller/dwb_msgs/package.xml | 2 +- nav2_dwb_controller/dwb_plugins/package.xml | 2 +- nav2_dwb_controller/nav2_dwb_controller/package.xml | 2 +- nav2_dwb_controller/nav_2d_msgs/package.xml | 2 +- nav2_dwb_controller/nav_2d_utils/package.xml | 2 +- nav2_graceful_controller/package.xml | 2 +- nav2_lifecycle_manager/package.xml | 2 +- nav2_loopback_sim/package.xml | 2 +- nav2_map_server/package.xml | 2 +- nav2_mppi_controller/package.xml | 2 +- nav2_msgs/package.xml | 2 +- nav2_navfn_planner/package.xml | 2 +- nav2_planner/package.xml | 2 +- nav2_regulated_pure_pursuit_controller/package.xml | 2 +- nav2_rotation_shim_controller/package.xml | 2 +- nav2_rviz_plugins/package.xml | 2 +- nav2_simple_commander/package.xml | 2 +- nav2_smac_planner/package.xml | 2 +- nav2_smoother/package.xml | 2 +- nav2_system_tests/package.xml | 2 +- nav2_theta_star_planner/package.xml | 2 +- nav2_util/package.xml | 2 +- nav2_velocity_smoother/package.xml | 2 +- nav2_voxel_grid/package.xml | 2 +- nav2_waypoint_follower/package.xml | 2 +- navigation2/package.xml | 2 +- 45 files changed, 45 insertions(+), 44 deletions(-) diff --git a/.github/workflows/build_main_against_distros.yml b/.github/workflows/build_main_against_distros.yml index 6cc0ba96e24..5c5f58d9103 100644 --- a/.github/workflows/build_main_against_distros.yml +++ b/.github/workflows/build_main_against_distros.yml @@ -13,7 +13,7 @@ jobs: strategy: fail-fast: false matrix: - ros_distro: [jazzy] + ros_distro: [jazzy, kilted] steps: - name: Checkout repository diff --git a/.github/workflows/update_ci_image.yaml b/.github/workflows/update_ci_image.yaml index 8e914771645..5692ce8cf26 100644 --- a/.github/workflows/update_ci_image.yaml +++ b/.github/workflows/update_ci_image.yaml @@ -8,6 +8,7 @@ on: push: branches: - main + - kilted - jazzy - humble - humble_main diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml index 38da836d086..d7af79e860b 100644 --- a/nav2_amcl/package.xml +++ b/nav2_amcl/package.xml @@ -2,7 +2,7 @@ nav2_amcl - 1.3.1 + 1.4.0

amcl is a probabilistic localization system for a robot moving in diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml index 851efab9c24..31da48c1868 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -2,7 +2,7 @@ nav2_behavior_tree - 1.3.1 + 1.4.0 Nav2 behavior tree wrappers, nodes, and utilities Michael Jeronimo Carlos Orduno diff --git a/nav2_behaviors/package.xml b/nav2_behaviors/package.xml index 2ef8566947b..5c983cb3554 100644 --- a/nav2_behaviors/package.xml +++ b/nav2_behaviors/package.xml @@ -2,7 +2,7 @@ nav2_behaviors - 1.3.1 + 1.4.0 Nav2 behavior server Carlos Orduno Steve Macenski diff --git a/nav2_bringup/package.xml b/nav2_bringup/package.xml index da293f0997f..dc5ebc7bb19 100644 --- a/nav2_bringup/package.xml +++ b/nav2_bringup/package.xml @@ -2,7 +2,7 @@ nav2_bringup - 1.3.1 + 1.4.0 Bringup scripts and configurations for the Nav2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml index fdc5a87bbed..d7a8e903bc3 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -2,7 +2,7 @@ nav2_bt_navigator - 1.3.1 + 1.4.0 Nav2 BT Navigator Server Michael Jeronimo Apache-2.0 diff --git a/nav2_collision_monitor/package.xml b/nav2_collision_monitor/package.xml index 22871152b78..bb166d9bbf4 100644 --- a/nav2_collision_monitor/package.xml +++ b/nav2_collision_monitor/package.xml @@ -2,7 +2,7 @@ nav2_collision_monitor - 1.3.1 + 1.4.0 Collision Monitor Alexey Merzlyakov Steve Macenski diff --git a/nav2_common/package.xml b/nav2_common/package.xml index 675de1fcc2f..7aa6a3925eb 100644 --- a/nav2_common/package.xml +++ b/nav2_common/package.xml @@ -2,7 +2,7 @@ nav2_common - 1.3.1 + 1.4.0 Common support functionality used throughout the navigation 2 stack Carl Delsey Apache-2.0 diff --git a/nav2_constrained_smoother/package.xml b/nav2_constrained_smoother/package.xml index 0628a745261..09f4d27015c 100644 --- a/nav2_constrained_smoother/package.xml +++ b/nav2_constrained_smoother/package.xml @@ -2,7 +2,7 @@ nav2_constrained_smoother - 1.3.1 + 1.4.0 Ceres constrained smoother Matej Vargovcik Steve Macenski diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml index 7eb51ca459c..1532aecbf07 100644 --- a/nav2_controller/package.xml +++ b/nav2_controller/package.xml @@ -2,7 +2,7 @@ nav2_controller - 1.3.1 + 1.4.0 Controller action interface Carl Delsey Apache-2.0 diff --git a/nav2_core/package.xml b/nav2_core/package.xml index 55e94f1b9fc..044eb215676 100644 --- a/nav2_core/package.xml +++ b/nav2_core/package.xml @@ -2,7 +2,7 @@ nav2_core - 1.3.1 + 1.4.0 A set of headers for plugins core to the Nav2 stack Steve Macenski Carl Delsey diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml index 9340bd404a8..9a7e1146cb8 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -2,7 +2,7 @@ nav2_costmap_2d - 1.3.1 + 1.4.0 This package provides an implementation of a 2D costmap that takes in sensor data from the world, builds a 2D or 3D occupancy grid of the data (depending diff --git a/nav2_docking/opennav_docking/package.xml b/nav2_docking/opennav_docking/package.xml index bb802980419..5e8b0070525 100644 --- a/nav2_docking/opennav_docking/package.xml +++ b/nav2_docking/opennav_docking/package.xml @@ -2,7 +2,7 @@ opennav_docking - 1.3.1 + 1.4.0 A Task Server for robot charger docking Steve Macenski Apache-2.0 diff --git a/nav2_docking/opennav_docking_bt/package.xml b/nav2_docking/opennav_docking_bt/package.xml index 37785551165..d2071203bfe 100644 --- a/nav2_docking/opennav_docking_bt/package.xml +++ b/nav2_docking/opennav_docking_bt/package.xml @@ -2,7 +2,7 @@ opennav_docking_bt - 1.3.1 + 1.4.0 A set of BT nodes and XMLs for docking Steve Macenski Apache-2.0 diff --git a/nav2_docking/opennav_docking_core/package.xml b/nav2_docking/opennav_docking_core/package.xml index 2df639d5161..57a3d15b386 100644 --- a/nav2_docking/opennav_docking_core/package.xml +++ b/nav2_docking/opennav_docking_core/package.xml @@ -2,7 +2,7 @@ opennav_docking_core - 1.3.1 + 1.4.0 A set of headers for plugins core to the opennav docking framework Steve Macenski Apache-2.0 diff --git a/nav2_dwb_controller/costmap_queue/package.xml b/nav2_dwb_controller/costmap_queue/package.xml index 3a9a7bf9ccc..695cebd19b7 100644 --- a/nav2_dwb_controller/costmap_queue/package.xml +++ b/nav2_dwb_controller/costmap_queue/package.xml @@ -1,7 +1,7 @@ costmap_queue - 1.3.1 + 1.4.0 The costmap_queue package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_core/package.xml b/nav2_dwb_controller/dwb_core/package.xml index ea3e167a2bb..8b55d813004 100644 --- a/nav2_dwb_controller/dwb_core/package.xml +++ b/nav2_dwb_controller/dwb_core/package.xml @@ -2,7 +2,7 @@ dwb_core - 1.3.1 + 1.4.0 DWB core interfaces package Carl Delsey BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_critics/package.xml b/nav2_dwb_controller/dwb_critics/package.xml index 92d1f2df16f..a697e486021 100644 --- a/nav2_dwb_controller/dwb_critics/package.xml +++ b/nav2_dwb_controller/dwb_critics/package.xml @@ -1,7 +1,7 @@ dwb_critics - 1.3.1 + 1.4.0 The dwb_critics package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_msgs/package.xml b/nav2_dwb_controller/dwb_msgs/package.xml index e6ce9307a62..594c7619a41 100644 --- a/nav2_dwb_controller/dwb_msgs/package.xml +++ b/nav2_dwb_controller/dwb_msgs/package.xml @@ -2,7 +2,7 @@ dwb_msgs - 1.3.1 + 1.4.0 Message/Service definitions specifically for the dwb_core David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_plugins/package.xml b/nav2_dwb_controller/dwb_plugins/package.xml index 3e0b0c941ec..ba51d4d06e1 100644 --- a/nav2_dwb_controller/dwb_plugins/package.xml +++ b/nav2_dwb_controller/dwb_plugins/package.xml @@ -1,7 +1,7 @@ dwb_plugins - 1.3.1 + 1.4.0 Standard implementations of the GoalChecker and TrajectoryGenerators for dwb_core diff --git a/nav2_dwb_controller/nav2_dwb_controller/package.xml b/nav2_dwb_controller/nav2_dwb_controller/package.xml index f98c8473a88..7d8363bcfc2 100644 --- a/nav2_dwb_controller/nav2_dwb_controller/package.xml +++ b/nav2_dwb_controller/nav2_dwb_controller/package.xml @@ -2,7 +2,7 @@ nav2_dwb_controller - 1.3.1 + 1.4.0 ROS2 controller (DWB) metapackage diff --git a/nav2_dwb_controller/nav_2d_msgs/package.xml b/nav2_dwb_controller/nav_2d_msgs/package.xml index 9dbb53a1ccd..5f5ed5f97bc 100644 --- a/nav2_dwb_controller/nav_2d_msgs/package.xml +++ b/nav2_dwb_controller/nav_2d_msgs/package.xml @@ -2,7 +2,7 @@ nav_2d_msgs - 1.3.1 + 1.4.0 Basic message types for two dimensional navigation, extending from geometry_msgs::Pose2D. David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/nav_2d_utils/package.xml b/nav2_dwb_controller/nav_2d_utils/package.xml index f3dae14813b..060a6c2f701 100644 --- a/nav2_dwb_controller/nav_2d_utils/package.xml +++ b/nav2_dwb_controller/nav_2d_utils/package.xml @@ -2,7 +2,7 @@ nav_2d_utils - 1.3.1 + 1.4.0 A handful of useful utility functions for nav_2d packages. David V. Lu!! BSD-3-Clause diff --git a/nav2_graceful_controller/package.xml b/nav2_graceful_controller/package.xml index b45196dcf5b..be7ee9c4806 100644 --- a/nav2_graceful_controller/package.xml +++ b/nav2_graceful_controller/package.xml @@ -2,7 +2,7 @@ nav2_graceful_controller - 1.3.1 + 1.4.0 Graceful motion controller Alberto Tudela Apache-2.0 diff --git a/nav2_lifecycle_manager/package.xml b/nav2_lifecycle_manager/package.xml index f5c010075f9..f70a336d141 100644 --- a/nav2_lifecycle_manager/package.xml +++ b/nav2_lifecycle_manager/package.xml @@ -2,7 +2,7 @@ nav2_lifecycle_manager - 1.3.1 + 1.4.0 A controller/manager for the lifecycle nodes of the Navigation 2 system Michael Jeronimo Apache-2.0 diff --git a/nav2_loopback_sim/package.xml b/nav2_loopback_sim/package.xml index 9376cfb50c7..5c5599706ae 100644 --- a/nav2_loopback_sim/package.xml +++ b/nav2_loopback_sim/package.xml @@ -2,7 +2,7 @@ nav2_loopback_sim - 1.3.1 + 1.4.0 A loopback simulator to replace physics simulation steve macenski Apache-2.0 diff --git a/nav2_map_server/package.xml b/nav2_map_server/package.xml index 1525e3f5ca8..9b4d47790b7 100644 --- a/nav2_map_server/package.xml +++ b/nav2_map_server/package.xml @@ -2,7 +2,7 @@ nav2_map_server - 1.3.1 + 1.4.0 Refactored map server for ROS2 Navigation diff --git a/nav2_mppi_controller/package.xml b/nav2_mppi_controller/package.xml index 3ed96f7abdb..ed8332288bf 100644 --- a/nav2_mppi_controller/package.xml +++ b/nav2_mppi_controller/package.xml @@ -2,7 +2,7 @@ nav2_mppi_controller - 1.3.1 + 1.4.0 nav2_mppi_controller Steve Macenski Aleksei Budyakov diff --git a/nav2_msgs/package.xml b/nav2_msgs/package.xml index c9e806f194e..12a085fbf88 100644 --- a/nav2_msgs/package.xml +++ b/nav2_msgs/package.xml @@ -2,7 +2,7 @@ nav2_msgs - 1.3.1 + 1.4.0 Messages and service files for the Nav2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_navfn_planner/package.xml b/nav2_navfn_planner/package.xml index 9ad576e5ab2..94e70c2f61c 100644 --- a/nav2_navfn_planner/package.xml +++ b/nav2_navfn_planner/package.xml @@ -2,7 +2,7 @@ nav2_navfn_planner - 1.3.1 + 1.4.0 Nav2 NavFn planner Steve Macenski Carlos Orduno diff --git a/nav2_planner/package.xml b/nav2_planner/package.xml index 26de6cfa13a..68d97a5c0d0 100644 --- a/nav2_planner/package.xml +++ b/nav2_planner/package.xml @@ -2,7 +2,7 @@ nav2_planner - 1.3.1 + 1.4.0 Nav2 planner server package Steve Macenski Apache-2.0 diff --git a/nav2_regulated_pure_pursuit_controller/package.xml b/nav2_regulated_pure_pursuit_controller/package.xml index ff4105b62e7..41b2bade1ee 100644 --- a/nav2_regulated_pure_pursuit_controller/package.xml +++ b/nav2_regulated_pure_pursuit_controller/package.xml @@ -2,7 +2,7 @@ nav2_regulated_pure_pursuit_controller - 1.3.1 + 1.4.0 Regulated Pure Pursuit Controller Steve Macenski Shrijit Singh diff --git a/nav2_rotation_shim_controller/package.xml b/nav2_rotation_shim_controller/package.xml index 9e450f75736..686c6915731 100644 --- a/nav2_rotation_shim_controller/package.xml +++ b/nav2_rotation_shim_controller/package.xml @@ -2,7 +2,7 @@ nav2_rotation_shim_controller - 1.3.1 + 1.4.0 Rotation Shim Controller Steve Macenski Apache-2.0 diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml index 0c509957230..5360d4750ce 100644 --- a/nav2_rviz_plugins/package.xml +++ b/nav2_rviz_plugins/package.xml @@ -2,7 +2,7 @@ nav2_rviz_plugins - 1.3.1 + 1.4.0 Navigation 2 plugins for rviz Michael Jeronimo Apache-2.0 diff --git a/nav2_simple_commander/package.xml b/nav2_simple_commander/package.xml index 080f5d93e73..e2293165f1b 100644 --- a/nav2_simple_commander/package.xml +++ b/nav2_simple_commander/package.xml @@ -2,7 +2,7 @@ nav2_simple_commander - 1.3.1 + 1.4.0 An importable library for writing mobile robot applications in python3 steve Apache-2.0 diff --git a/nav2_smac_planner/package.xml b/nav2_smac_planner/package.xml index d73d553496b..8303ffc165f 100644 --- a/nav2_smac_planner/package.xml +++ b/nav2_smac_planner/package.xml @@ -2,7 +2,7 @@ nav2_smac_planner - 1.3.1 + 1.4.0 Smac global planning plugin: A*, Hybrid-A*, State Lattice Steve Macenski Apache-2.0 diff --git a/nav2_smoother/package.xml b/nav2_smoother/package.xml index 7523ffc0aa1..72daaa67bd2 100644 --- a/nav2_smoother/package.xml +++ b/nav2_smoother/package.xml @@ -2,7 +2,7 @@ nav2_smoother - 1.3.1 + 1.4.0 Smoother action interface Matej Vargovcik Steve Macenski diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index 684dfd6800f..7a8261b63d7 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -2,7 +2,7 @@ nav2_system_tests - 1.3.1 + 1.4.0 A sets of system-level tests for Nav2 usually involving full robot simulation Carlos Orduno Apache-2.0 diff --git a/nav2_theta_star_planner/package.xml b/nav2_theta_star_planner/package.xml index 0da357b5815..a8860994253 100644 --- a/nav2_theta_star_planner/package.xml +++ b/nav2_theta_star_planner/package.xml @@ -2,7 +2,7 @@ nav2_theta_star_planner - 1.3.1 + 1.4.0 Theta* Global Planning Plugin Steve Macenski Anshumaan Singh diff --git a/nav2_util/package.xml b/nav2_util/package.xml index bdc150c4120..97583144f86 100644 --- a/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -2,7 +2,7 @@ nav2_util - 1.3.1 + 1.4.0 Nav2 utilities Michael Jeronimo Mohammad Haghighipanah diff --git a/nav2_velocity_smoother/package.xml b/nav2_velocity_smoother/package.xml index 5163de4f846..f2a0c2cd61b 100644 --- a/nav2_velocity_smoother/package.xml +++ b/nav2_velocity_smoother/package.xml @@ -2,7 +2,7 @@ nav2_velocity_smoother - 1.3.1 + 1.4.0 Nav2's Output velocity smoother Steve Macenski Apache-2.0 diff --git a/nav2_voxel_grid/package.xml b/nav2_voxel_grid/package.xml index 748944d74e2..ef9eedd9b42 100644 --- a/nav2_voxel_grid/package.xml +++ b/nav2_voxel_grid/package.xml @@ -2,7 +2,7 @@ nav2_voxel_grid - 1.3.1 + 1.4.0 voxel_grid provides an implementation of an efficient 3D voxel grid. The occupancy grid can support 3 different representations for the state of a cell: marked, free, or unknown. Due to the underlying implementation relying on bitwise and and or integer operations, the voxel grid only supports 16 different levels per voxel column. However, this limitation yields raytracing and cell marking performance in the grid comparable to standard 2D structures making it quite fast compared to most 3D structures. diff --git a/nav2_waypoint_follower/package.xml b/nav2_waypoint_follower/package.xml index aeb3d44009d..409e652d8ff 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -2,7 +2,7 @@ nav2_waypoint_follower - 1.3.1 + 1.4.0 A waypoint follower navigation server Steve Macenski Apache-2.0 diff --git a/navigation2/package.xml b/navigation2/package.xml index 325943f50d3..6aa4209831c 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -2,7 +2,7 @@ navigation2 - 1.3.1 + 1.4.0 ROS2 Navigation Stack From c0df07e223e3eb74515206cf973e6e10cb609bf8 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 27 May 2025 22:07:36 -0700 Subject: [PATCH 22/46] Removing underlay workspace from Main (#5200) Signed-off-by: Steve Macenski --- tools/underlay.repos | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/tools/underlay.repos b/tools/underlay.repos index 0918320f01f..3025a6dde48 100644 --- a/tools/underlay.repos +++ b/tools/underlay.repos @@ -1,8 +1,8 @@ repositories: - BehaviorTree/BehaviorTree.CPP: - type: git - url: https://github.com/BehaviorTree/BehaviorTree.CPP.git - version: 4.7.0 + # BehaviorTree/BehaviorTree.CPP: + # type: git + # url: https://github.com/BehaviorTree/BehaviorTree.CPP.git + # version: 4.6.2 # ros/angles: # type: git # url: https://github.com/ros/angles.git @@ -31,10 +31,6 @@ repositories: # type: git # url: https://github.com/cra-ros-pkg/robot_localization.git # version: ros2 - ros-navigation/nav2_minimal_turtlebot_simulation: - type: git - url: https://github.com/ros-navigation/nav2_minimal_turtlebot_simulation.git - version: main ros/common_interfaces: type: git url: https://github.com/ros2/common_interfaces.git @@ -43,3 +39,7 @@ repositories: type: git url: https://github.com/ros2/launch.git version: humble + # ros-navigation/nav2_minimal_turtlebot_simulation: + # type: git + # url: https://github.com/ros-navigation/nav2_minimal_turtlebot_simulation.git + # version: main From f50c6987bce1d96ca9a7f9d204d4fdc638c0717d Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 28 May 2025 11:25:04 -0700 Subject: [PATCH 23/46] Adding missing dep to loopback sim (#5204) * Adding missing dep Signed-off-by: Steve Macenski * typo Signed-off-by: Steve Macenski * updating fix Signed-off-by: Steve Macenski --------- Signed-off-by: Steve Macenski --- nav2_loopback_sim/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_loopback_sim/package.xml b/nav2_loopback_sim/package.xml index 5c5599706ae..9d55a6d4837 100644 --- a/nav2_loopback_sim/package.xml +++ b/nav2_loopback_sim/package.xml @@ -12,6 +12,7 @@ nav_msgs tf_transformations tf2_ros + python3-transforms3d ament_copyright ament_flake8 From c94ffa77e0bb67ce8580ff3b5d2d53c75d4d61dc Mon Sep 17 00:00:00 2001 From: Marco Bassa <101661130+MarcoMatteoBassa@users.noreply.github.com> Date: Wed, 28 May 2025 20:54:12 +0200 Subject: [PATCH 24/46] Adding parameter warn_when_defaulting_parameters to control default parameter warnings (#5189) * Adding a parameter warn_when_defaulting_parameters to control default parameter warnings instead of using a flag Signed-off-by: Marco Bassa * Adding parameter strict_param_loading for optionally throwing an exception if parameter overrides are missing Signed-off-by: Marco Bassa * Using default false declaration instead of declare_or_get in param util Signed-off-by: Marco Bassa --------- Signed-off-by: Marco Bassa --- nav2_util/test/test_node_utils.cpp | 44 ++++++++++++++++++++++++++++++ 1 file changed, 44 insertions(+) diff --git a/nav2_util/test/test_node_utils.cpp b/nav2_util/test/test_node_utils.cpp index d7ffb3cf5d5..17b650c10a6 100644 --- a/nav2_util/test/test_node_utils.cpp +++ b/nav2_util/test/test_node_utils.cpp @@ -78,6 +78,50 @@ TEST(DeclareParameterIfNotDeclared, DeclareParameterIfNotDeclared) ASSERT_EQ(param, "fred"); } +TEST(DeclareOrGetParam, DeclareOrGetParam) +{ + auto node = std::make_shared("test_node"); + + // test declared parameter + node->declare_parameter("foobar", "foo"); + std::string param = declare_or_get_parameter(node, "foobar", std::string{"bar"}); + EXPECT_EQ(param, "foo"); + node->get_parameter("foobar", param); + EXPECT_EQ(param, "foo"); + + // test undeclared parameter + node->set_parameter(rclcpp::Parameter("warn_on_missing_params", true)); + int int_param = declare_or_get_parameter(node, "waldo", 3); + EXPECT_EQ(int_param, 3); + + // test unknown parameter with strict_param_loading enabled + bool got_exception{false}; + node->set_parameter(rclcpp::Parameter("strict_param_loading", true)); + try { + declare_or_get_parameter(node, "burpy", true); + } catch (const rclcpp::exceptions::InvalidParameterValueException & exc) { + got_exception = true; + } + EXPECT_TRUE(got_exception); + // The parameter is anyway declared with the default val and subsequent calls won't fail + EXPECT_TRUE(declare_or_get_parameter(node, "burpy", true)); + + // test declaration by type of existing param + int_param = declare_or_get_parameter(node, "waldo", + rclcpp::ParameterType::PARAMETER_INTEGER); + EXPECT_EQ(int_param, 3); + + // test declaration by type of non existing param + got_exception = false; + try { + int_param = declare_or_get_parameter(node, "wololo", + rclcpp::ParameterType::PARAMETER_INTEGER); + } catch (const rclcpp::exceptions::InvalidParameterValueException & exc) { + got_exception = true; + } + EXPECT_TRUE(got_exception); +} + TEST(GetPluginTypeParam, GetPluginTypeParam) { ::testing::FLAGS_gtest_death_test_style = "threadsafe"; From 1f2d1e8ab31b52c66e59dd93c7d302aa88e70b32 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 28 May 2025 12:18:07 -0700 Subject: [PATCH 25/46] Update mergify.yml Signed-off-by: Steve Macenski --- .github/mergify.yml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/mergify.yml b/.github/mergify.yml index bf71c93c9f0..754975652a2 100644 --- a/.github/mergify.yml +++ b/.github/mergify.yml @@ -26,14 +26,14 @@ pull_request_rules: branches: - humble - - name: backport to humble_main at reviewers discretion + - name: backport to kilted at reviewers discretion conditions: - base=main - - "label=backport-humble-main" + - "label=backport-kilted" actions: backport: branches: - - humble_main + - kilted - name: delete head branch after merge conditions: From 701454f0ab53ac823ffe69e3cf45a26776b2cd43 Mon Sep 17 00:00:00 2001 From: Stevedan Ogochukwu Omodolor <61468301+stevedanomodolor@users.noreply.github.com> Date: Thu, 29 May 2025 00:55:45 +0200 Subject: [PATCH 26/46] include bug fix for nav2_smac_planner (#5198) Signed-off-by: Stevedan Omodolor --- .../nav2_smac_planner/analytic_expansion.hpp | 5 +++-- nav2_smac_planner/src/analytic_expansion.cpp | 20 +++++++++++-------- nav2_smac_planner/test/test_a_star.cpp | 4 +++- 3 files changed, 18 insertions(+), 11 deletions(-) diff --git a/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp index 0eecb78185e..6c6f33490f4 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp @@ -112,14 +112,15 @@ class AnalyticExpansion /** * @brief Refined analytic path from the current node to the goal - * @param current_node The node to start the analytic path from + * @param node The node to start the analytic path from. Node head may + * change as a result of refinement * @param goal_node The goal node to plan to * @param getter The function object that gets valid nodes from the graph * @param analytic_nodes The set of analytic nodes to refine * @return The score of the refined path */ float refineAnalyticPath( - const NodePtr & current_node, + NodePtr & node, const NodePtr & goal_node, const NodeGetter & getter, AnalyticExpansionNodes & analytic_nodes); diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp index da46f781975..774454c216a 100644 --- a/nav2_smac_planner/src/analytic_expansion.cpp +++ b/nav2_smac_planner/src/analytic_expansion.cpp @@ -66,6 +66,7 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic AnalyticExpansionNodes current_best_analytic_nodes = {}; NodePtr current_best_goal = nullptr; + NodePtr current_best_node = nullptr; float current_best_score = std::numeric_limits::max(); closest_distance = std::min( @@ -97,13 +98,15 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic current_node->motion_table.state_space); if (!analytic_nodes.empty()) { found_valid_expansion = true; - bool score = refineAnalyticPath( - current_node, current_goal_node, getter, analytic_nodes); + NodePtr node = current_node; + float score = refineAnalyticPath( + node, current_goal_node, getter, analytic_nodes); // Update the best score if we found a better path if (score < current_best_score) { current_best_analytic_nodes = analytic_nodes; current_best_goal = current_goal_node; current_best_score = score; + current_best_node = node; } } } @@ -116,13 +119,15 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic current_node, current_goal_node, getter, current_node->motion_table.state_space); if (!analytic_nodes.empty()) { - bool score = refineAnalyticPath( - current_node, current_goal_node, getter, analytic_nodes); + NodePtr node = current_node; + float score = refineAnalyticPath( + node, current_goal_node, getter, analytic_nodes); // Update the best score if we found a better path if (score < current_best_score) { current_best_analytic_nodes = analytic_nodes; current_best_goal = current_goal_node; current_best_score = score; + current_best_node = node; } } } @@ -131,7 +136,7 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic if (!current_best_analytic_nodes.empty()) { return setAnalyticPath( - current_node, current_best_goal, + current_best_node, current_best_goal, current_best_analytic_nodes); } analytic_iterations--; @@ -273,12 +278,11 @@ typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansion float AnalyticExpansion::refineAnalyticPath( - const NodePtr & current_node, + NodePtr & node, const NodePtr & goal_node, const NodeGetter & getter, AnalyticExpansionNodes & analytic_nodes) { - NodePtr node = current_node; NodePtr test_node = node; AnalyticExpansionNodes refined_analytic_nodes; for (int i = 0; i < 8; i++) { @@ -407,7 +411,7 @@ getAnalyticPath( template<> float AnalyticExpansion::refineAnalyticPath( - const NodePtr &, + NodePtr &, const NodePtr &, const NodeGetter &, AnalyticExpansionNodes &) diff --git a/nav2_smac_planner/test/test_a_star.cpp b/nav2_smac_planner/test/test_a_star.cpp index 3d64ffbc2d2..6e3a34daeb8 100644 --- a/nav2_smac_planner/test/test_a_star.cpp +++ b/nav2_smac_planner/test/test_a_star.cpp @@ -141,7 +141,9 @@ TEST(AStarTest, test_a_star_2d) int dummy_int2 = 0; EXPECT_EQ(expander.tryAnalyticExpansion(nullptr, {}, {}, {}, nullptr, dummy_int1, dummy_int2), nullptr); - EXPECT_EQ(expander.refineAnalyticPath(nullptr, nullptr, nullptr, + + nav2_smac_planner::Node2D * start = nullptr; + EXPECT_EQ(expander.refineAnalyticPath(start, nullptr, nullptr, analytic_expansion_nodes), std::numeric_limits::max()); nav2_smac_planner::AnalyticExpansion::AnalyticExpansionNodes expected_nodes = expander.getAnalyticPath(nullptr, nullptr, nullptr, nullptr); From c5be3731514e9c2cef40670b86171280c62dd867 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 28 May 2025 15:57:17 -0700 Subject: [PATCH 27/46] Revert "Removing underlay workspace from Main" (#5206) * Revert "Removing underlay workspace from Main (#5200)" This reverts commit 1278df713a4fa4e80cc6d5903046180ec3b3686b. * Update underlay.repos Signed-off-by: Steve Macenski --------- Signed-off-by: Steve Macenski --- tools/underlay.repos | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/tools/underlay.repos b/tools/underlay.repos index 3025a6dde48..24a86813ac5 100644 --- a/tools/underlay.repos +++ b/tools/underlay.repos @@ -1,8 +1,8 @@ repositories: - # BehaviorTree/BehaviorTree.CPP: - # type: git - # url: https://github.com/BehaviorTree/BehaviorTree.CPP.git - # version: 4.6.2 + BehaviorTree/BehaviorTree.CPP: + type: git + url: https://github.com/BehaviorTree/BehaviorTree.CPP.git + version: 4.6.2 # ros/angles: # type: git # url: https://github.com/ros/angles.git From 0e2e50e2e5fa4290631e90a69a09f88519be5db8 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Sun, 1 Jun 2025 13:16:37 -0700 Subject: [PATCH 28/46] Option to Reduce Lethal to High-Cost Navigable To Get Out of Keepout Zones if Wandered In (#5187) * Adding toggle option of keepout zone Signed-off-by: Steve Macenski * Default off Signed-off-by: Steve Macenski * Join conditions Signed-off-by: Steve Macenski * spell check Signed-off-by: Steve Macenski * copilot suggestions Signed-off-by: Steve Macenski * Update nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp Co-authored-by: Leander Stephen D'Souza Signed-off-by: Steve Macenski * Update nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp Co-authored-by: Leander Stephen D'Souza Signed-off-by: Steve Macenski * Update keepout_filter.cpp Signed-off-by: Steve Macenski --------- Signed-off-by: Steve Macenski Co-authored-by: Leander Stephen D'Souza --- nav2_bringup/params/nav2_params.yaml | 4 ++ .../costmap_filters/keepout_filter.hpp | 6 ++ .../costmap_filters/keepout_filter.cpp | 65 +++++++++++++++++-- 3 files changed, 69 insertions(+), 6 deletions(-) diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 7e9261e1564..f1d7fb2169b 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -229,6 +229,8 @@ local_costmap: plugin: "nav2_costmap_2d::KeepoutFilter" enabled: KEEPOUT_ZONE_ENABLED filter_info_topic: "keepout_costmap_filter_info" + override_lethal_cost: True + lethal_override_cost: 200 inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 @@ -281,6 +283,8 @@ global_costmap: plugin: "nav2_costmap_2d::KeepoutFilter" enabled: KEEPOUT_ZONE_ENABLED filter_info_topic: "keepout_costmap_filter_info" + override_lethal_cost: True + lethal_override_cost: 200 speed_filter: plugin: "nav2_costmap_2d::SpeedFilter" enabled: SPEED_ZONE_ENABLED diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp index e2225196d72..a705c74f99a 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp @@ -102,6 +102,12 @@ class KeepoutFilter : public CostmapFilter nav_msgs::msg::OccupancyGrid::SharedPtr filter_mask_; std::string global_frame_; // Frame of current layer (master_grid) + + bool override_lethal_cost_{false}; // If true, lethal cost will be overridden + unsigned char lethal_override_cost_{252}; // Value to override lethal cost with + bool last_pose_lethal_{false}; // If true, last pose was lethal + unsigned int lethal_state_update_min_x_{999999u}, lethal_state_update_min_y_{999999u}; + unsigned int lethal_state_update_max_x_{0u}, lethal_state_update_max_y_{0u}; }; } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp index ac01861ef67..0f24178e36d 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp @@ -74,6 +74,15 @@ void KeepoutFilter::initializeFilter( std::bind(&KeepoutFilter::filterInfoCallback, this, std::placeholders::_1)); global_frame_ = layered_costmap_->getGlobalFrameID(); + + declareParameter("override_lethal_cost", rclcpp::ParameterValue(false)); + node->get_parameter(name_ + "." + "override_lethal_cost", override_lethal_cost_); + declareParameter("lethal_override_cost", rclcpp::ParameterValue(MAX_NON_OBSTACLE)); + node->get_parameter(name_ + "." + "lethal_override_cost", lethal_override_cost_); + + // clamp lethal_override_cost_ in case if higher than MAX_NON_OBSTACLE is given + lethal_override_cost_ = \ + std::clamp(lethal_override_cost_, FREE_SPACE, MAX_NON_OBSTACLE); } void KeepoutFilter::filterInfoCallback( @@ -149,7 +158,7 @@ void KeepoutFilter::maskCallback( void KeepoutFilter::process( nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i, int max_j, - const geometry_msgs::msg::Pose2D & /*pose*/) + const geometry_msgs::msg::Pose2D & pose) { std::lock_guard guard(*getMutex()); @@ -244,10 +253,47 @@ void KeepoutFilter::process( } // unsigned<-signed conversions. - unsigned const int mg_min_x_u = static_cast(mg_min_x); - unsigned const int mg_min_y_u = static_cast(mg_min_y); - unsigned const int mg_max_x_u = static_cast(mg_max_x); - unsigned const int mg_max_y_u = static_cast(mg_max_y); + unsigned int mg_min_x_u = static_cast(mg_min_x); + unsigned int mg_min_y_u = static_cast(mg_min_y); + unsigned int mg_max_x_u = static_cast(mg_max_x); + unsigned int mg_max_y_u = static_cast(mg_max_y); + + // Let's find the pose's cost if we are allowed to override the lethal cost + bool is_pose_lethal = false; + if (override_lethal_cost_) { + geometry_msgs::msg::Pose2D mask_pose; + if (transformPose(global_frame_, pose, filter_mask_->header.frame_id, mask_pose)) { + unsigned int mask_robot_i, mask_robot_j; + if (worldToMask(filter_mask_, mask_pose.x, mask_pose.y, mask_robot_i, mask_robot_j)) { + auto data = getMaskCost(filter_mask_, mask_robot_i, mask_robot_j); + is_pose_lethal = (data == INSCRIBED_INFLATED_OBSTACLE || data == LETHAL_OBSTACLE); + if (is_pose_lethal) { + RCLCPP_WARN_THROTTLE( + logger_, *(clock_), 2000, + "KeepoutFilter: Pose is in keepout zone, reducing cost override to navigate out."); + } + } + } + + // If in lethal space or just exited lethal space, + // we need to update all possible spaces touched during this state + if (is_pose_lethal || (last_pose_lethal_ && !is_pose_lethal)) { + lethal_state_update_min_x_ = std::min(mg_min_x_u, lethal_state_update_min_x_); + mg_min_x_u = lethal_state_update_min_x_; + lethal_state_update_min_y_ = std::min(mg_min_y_u, lethal_state_update_min_y_); + mg_min_y_u = lethal_state_update_min_y_; + lethal_state_update_max_x_ = std::max(mg_max_x_u, lethal_state_update_max_x_); + mg_max_x_u = lethal_state_update_max_x_; + lethal_state_update_max_y_ = std::max(mg_max_y_u, lethal_state_update_max_y_); + mg_max_y_u = lethal_state_update_max_y_; + } else { + // If out of lethal space, reset managed lethal state sizes + lethal_state_update_min_x_ = master_grid.getSizeInCellsX(); + lethal_state_update_min_y_ = master_grid.getSizeInCellsY(); + lethal_state_update_max_x_ = 0u; + lethal_state_update_max_y_ = 0u; + } + } unsigned int i, j; // master_grid iterators unsigned int index; // corresponding index of master_grid @@ -284,12 +330,19 @@ void KeepoutFilter::process( if (data == NO_INFORMATION) { continue; } + if (data > old_data || old_data == NO_INFORMATION) { - master_array[index] = data; + if (override_lethal_cost_ && is_pose_lethal) { + master_array[index] = lethal_override_cost_; + } else { + master_array[index] = data; + } } } } } + + last_pose_lethal_ = is_pose_lethal; } void KeepoutFilter::resetFilter() From db45206098118e70bb599374060a9505d47f7c5e Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Sun, 1 Jun 2025 13:31:34 -0700 Subject: [PATCH 29/46] Prototype solving #5192 Issue 2: Reeds-Shepp reduce small reverse expansions (#5207) * prototype solving 5192 issue 2 Signed-off-by: Steve Macenski * Removing unnecessary variable Signed-off-by: Steve Macenski --------- Signed-off-by: Steve Macenski --- .../launch/tb3_loopback_simulation_launch.py | 2 + nav2_bringup/launch/tb3_simulation_launch.py | 2 + .../nav2_smac_planner/analytic_expansion.hpp | 39 ++++++++- nav2_smac_planner/src/analytic_expansion.cpp | 86 ++++++++++++++----- nav2_smac_planner/test/test_a_star.cpp | 2 +- 5 files changed, 108 insertions(+), 23 deletions(-) diff --git a/nav2_bringup/launch/tb3_loopback_simulation_launch.py b/nav2_bringup/launch/tb3_loopback_simulation_launch.py index 26c22a31d50..9f51e66c04b 100644 --- a/nav2_bringup/launch/tb3_loopback_simulation_launch.py +++ b/nav2_bringup/launch/tb3_loopback_simulation_launch.py @@ -144,6 +144,8 @@ def generate_launch_description() -> LaunchDescription: 'use_composition': use_composition, 'use_respawn': use_respawn, 'use_localization': 'False', # Don't use SLAM, AMCL + 'use_keepout_zones': 'False', + 'use_speed_zones': 'False', }.items(), ) diff --git a/nav2_bringup/launch/tb3_simulation_launch.py b/nav2_bringup/launch/tb3_simulation_launch.py index cc40ed90c56..e0a5f217646 100644 --- a/nav2_bringup/launch/tb3_simulation_launch.py +++ b/nav2_bringup/launch/tb3_simulation_launch.py @@ -195,6 +195,8 @@ def generate_launch_description() -> LaunchDescription: 'autostart': autostart, 'use_composition': use_composition, 'use_respawn': use_respawn, + 'use_keepout_zones': 'False', + 'use_speed_zones': 'False', }.items(), ) # The SDF file for the world is a xacro file because we wanted to diff --git a/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp index 6c6f33490f4..0c5cdede0bd 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp @@ -15,6 +15,10 @@ #ifndef NAV2_SMAC_PLANNER__ANALYTIC_EXPANSION_HPP_ #define NAV2_SMAC_PLANNER__ANALYTIC_EXPANSION_HPP_ +#include +#include +#include + #include #include #include @@ -61,7 +65,33 @@ class AnalyticExpansion Coordinates proposed_coords; }; - typedef std::vector AnalyticExpansionNodes; + /** + * @struct AnalyticExpansionNodes + * @brief Analytic expansion nodes and associated metadata + * + * This structure holds a collection of analytic expansion nodes and the number of direction + * changes encountered during the expansion. + */ + struct AnalyticExpansionNodes + { + AnalyticExpansionNodes() = default; + + void add( + NodePtr & node, + Coordinates & initial_coords, + Coordinates & proposed_coords) + { + nodes.emplace_back(node, initial_coords, proposed_coords); + } + + void setDirectionChanges(int changes) + { + direction_changes = changes; + } + + std::vector nodes; + int direction_changes{0}; + }; /** * @brief Constructor for analytic expansion object @@ -136,6 +166,13 @@ class AnalyticExpansion const NodePtr & node, const NodePtr & goal, const AnalyticExpansionNodes & expanded_nodes); + /** + * @brief Counts the number of direction changes in a Reeds-Shepp path + * @param path The Reeds-Shepp path to count direction changes in + * @return The number of direction changes in the path + */ + int countDirectionChanges(const ompl::base::ReedsSheppStateSpace::ReedsSheppPath & path); + /** * @brief Takes an expanded nodes to clean up, if necessary, of any state * information that may be polluting it from a prior search iteration diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp index 774454c216a..13df5feb6af 100644 --- a/nav2_smac_planner/src/analytic_expansion.cpp +++ b/nav2_smac_planner/src/analytic_expansion.cpp @@ -12,10 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. Reserved. -#include -#include -#include - #include #include #include @@ -64,7 +60,7 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic NodeT::getCoords( current_node->getIndex(), _collision_checker->getCostmap()->getSizeInCellsX(), _dim_3_size); - AnalyticExpansionNodes current_best_analytic_nodes = {}; + AnalyticExpansionNodes current_best_analytic_nodes; NodePtr current_best_goal = nullptr; NodePtr current_best_node = nullptr; float current_best_score = std::numeric_limits::max(); @@ -96,7 +92,7 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic getAnalyticPath( current_node, current_goal_node, getter, current_node->motion_table.state_space); - if (!analytic_nodes.empty()) { + if (!analytic_nodes.nodes.empty()) { found_valid_expansion = true; NodePtr node = current_node; float score = refineAnalyticPath( @@ -118,7 +114,7 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic getAnalyticPath( current_node, current_goal_node, getter, current_node->motion_table.state_space); - if (!analytic_nodes.empty()) { + if (!analytic_nodes.nodes.empty()) { NodePtr node = current_node; float score = refineAnalyticPath( node, current_goal_node, getter, analytic_nodes); @@ -134,7 +130,7 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic } } - if (!current_best_analytic_nodes.empty()) { + if (!current_best_analytic_nodes.nodes.empty()) { return setAnalyticPath( current_best_node, current_best_goal, current_best_analytic_nodes); @@ -146,6 +142,28 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic return NodePtr(nullptr); } +template +int AnalyticExpansion::countDirectionChanges( + const ompl::base::ReedsSheppStateSpace::ReedsSheppPath & path) +{ + const double * lengths = path.length_; + int changes = 0; + int last_dir = 0; + for (int i = 0; i < 5; ++i) { + if (lengths[i] == 0.0) { + continue; + } + + int currentDirection = (lengths[i] > 0.0) ? 1 : -1; + if (last_dir != 0 && currentDirection != last_dir) { + ++changes; + } + last_dir = currentDirection; + } + + return changes; +} + template typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansion::getAnalyticPath( const NodePtr & node, @@ -163,6 +181,12 @@ typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansiondistance(from(), to()); + auto rs_state_space = dynamic_cast(state_space.get()); + int direction_changes = 0; + if (rs_state_space) { + direction_changes = countDirectionChanges(rs_state_space->reedsShepp(from.get(), to.get())); + } + // A move of sqrt(2) is guaranteed to be in a new cell static const float sqrt_2 = sqrtf(2.0f); @@ -179,7 +203,7 @@ typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansion reals; double theta; @@ -214,7 +238,7 @@ typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansionsetPose(proposed_coordinates); if (next->isNodeValid(_traverse_unknown, _collision_checker) && next != prev) { // Save the node, and its previous coordinates in case we need to abort - possible_nodes.emplace_back(next, initial_node_coords, proposed_coordinates); + possible_nodes.add(next, initial_node_coords, proposed_coordinates); node_costs.emplace_back(next->getCost()); prev = next; } else { @@ -264,7 +288,7 @@ typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansionsetPose(node_pose.initial_coords); } @@ -273,6 +297,7 @@ typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansion::refineAnalyticPath( getAnalyticPath( test_node, goal_node, getter, test_node->motion_table.state_space); - if (refined_analytic_nodes.empty()) { + if (refined_analytic_nodes.nodes.empty()) { break; } + if (refined_analytic_nodes.direction_changes > analytic_nodes.direction_changes) { + // If the direction changes are worse, we don't want to use this path + continue; + } analytic_nodes = refined_analytic_nodes; node = test_node; } else { @@ -314,7 +343,7 @@ float AnalyticExpansion::refineAnalyticPath( // higher than the minimum turning radius and use the best solution based on // a scoring function similar to that used in traversal cost estimation. auto scoringFn = [&](const AnalyticExpansionNodes & expansion) { - if (expansion.size() < 2) { + if (expansion.nodes.size() < 2) { return std::numeric_limits::max(); } @@ -322,18 +351,19 @@ float AnalyticExpansion::refineAnalyticPath( float normalized_cost = 0.0; // Analytic expansions are consistently spaced const float distance = hypotf( - expansion[1].proposed_coords.x - expansion[0].proposed_coords.x, - expansion[1].proposed_coords.y - expansion[0].proposed_coords.y); - const float & weight = expansion[0].node->motion_table.cost_penalty; - for (auto iter = expansion.begin(); iter != expansion.end(); ++iter) { + expansion.nodes[1].proposed_coords.x - expansion.nodes[0].proposed_coords.x, + expansion.nodes[1].proposed_coords.y - expansion.nodes[0].proposed_coords.y); + const float & weight = expansion.nodes[0].node->motion_table.cost_penalty; + for (auto iter = expansion.nodes.begin(); iter != expansion.nodes.end(); ++iter) { normalized_cost = iter->node->getCost() / 252.0f; - // Search's Traversal Cost Function + // Search's Traversal Cost Function score += distance * (1.0 + weight * normalized_cost); } return score; }; - float best_score = scoringFn(analytic_nodes); + float original_score = scoringFn(analytic_nodes); + float best_score = original_score; float score = std::numeric_limits::max(); float min_turn_rad = node->motion_table.min_turning_radius; const float max_min_turn_rad = 4.0 * min_turn_rad; // Up to 4x the turning radius @@ -347,7 +377,21 @@ float AnalyticExpansion::refineAnalyticPath( } refined_analytic_nodes = getAnalyticPath(node, goal_node, getter, state_space); score = scoringFn(refined_analytic_nodes); - if (score <= best_score) { + + // Normal scoring: prioritize lower cost as long as not more directional changes + if (score <= best_score && + refined_analytic_nodes.direction_changes <= analytic_nodes.direction_changes) + { + analytic_nodes = refined_analytic_nodes; + best_score = score; + continue; + } + + // Special case: If we have a better score than original (only) and less directional changes + // the path quality is still better than the original and is less operationally complex + if (score <= original_score && + refined_analytic_nodes.direction_changes < analytic_nodes.direction_changes) + { analytic_nodes = refined_analytic_nodes; best_score = score; } @@ -365,7 +409,7 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::setAnalytic _detached_nodes.clear(); // Legitimate final path - set the parent relationships, states, and poses NodePtr prev = node; - for (const auto & node_pose : expanded_nodes) { + for (const auto & node_pose : expanded_nodes.nodes) { auto n = node_pose.node; cleanNode(n); if (n->getIndex() != goal_node->getIndex()) { diff --git a/nav2_smac_planner/test/test_a_star.cpp b/nav2_smac_planner/test/test_a_star.cpp index 6e3a34daeb8..701e7ad01ac 100644 --- a/nav2_smac_planner/test/test_a_star.cpp +++ b/nav2_smac_planner/test/test_a_star.cpp @@ -147,7 +147,7 @@ TEST(AStarTest, test_a_star_2d) analytic_expansion_nodes), std::numeric_limits::max()); nav2_smac_planner::AnalyticExpansion::AnalyticExpansionNodes expected_nodes = expander.getAnalyticPath(nullptr, nullptr, nullptr, nullptr); - EXPECT_EQ(expected_nodes.size(), 0); + EXPECT_EQ(expected_nodes.nodes.size(), 0); delete costmapA; } From ca2a75866715eca368015ce08af0572c3b242d19 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 2 Jun 2025 11:18:47 -0700 Subject: [PATCH 30/46] Revert recent smac changes causing regressions (#5221) * Revert "Prototype solving #5192 Issue 2: Reeds-Shepp reduce small reverse expansions (#5207)" This reverts commit c32873decf651b203d7da978bc307b7fbdc196ca. * Revert "include bug fix for nav2_smac_planner (#5198)" This reverts commit 6a74ba61ca0ea117b2995dd0c8e266a5ec9741ba. * Revert "Feat/smac planner include orientation flexibility (#4127)" This reverts commit f5543c39aba5d3302208bc6e9696e7cbd41de8c9. --- .../launch/tb3_loopback_simulation_launch.py | 2 - nav2_bringup/launch/tb3_simulation_launch.py | 2 - .../include/nav2_smac_planner/a_star.hpp | 47 +- .../nav2_smac_planner/analytic_expansion.hpp | 73 +- .../include/nav2_smac_planner/constants.hpp | 35 - .../nav2_smac_planner/goal_manager.hpp | 190 - .../include/nav2_smac_planner/node_2d.hpp | 22 +- .../include/nav2_smac_planner/node_hybrid.hpp | 6 +- .../nav2_smac_planner/node_lattice.hpp | 6 +- .../nav2_smac_planner/smac_planner_hybrid.hpp | 2 - .../smac_planner_lattice.hpp | 2 - .../include/nav2_smac_planner/types.hpp | 12 - .../sample_primitives/test/output.json | 3931 ----------------- nav2_smac_planner/src/a_star.cpp | 142 +- nav2_smac_planner/src/analytic_expansion.cpp | 277 +- nav2_smac_planner/src/node_2d.cpp | 6 +- nav2_smac_planner/src/node_hybrid.cpp | 14 +- nav2_smac_planner/src/node_lattice.cpp | 15 +- nav2_smac_planner/src/smac_planner_hybrid.cpp | 76 +- .../src/smac_planner_lattice.cpp | 73 +- nav2_smac_planner/test/CMakeLists.txt | 10 - nav2_smac_planner/test/test_a_star.cpp | 147 +- nav2_smac_planner/test/test_goal_manager.cpp | 177 - nav2_smac_planner/test/test_node2d.cpp | 4 +- nav2_smac_planner/test/test_smac_hybrid.cpp | 108 +- nav2_smac_planner/test/test_smac_lattice.cpp | 84 +- 26 files changed, 169 insertions(+), 5294 deletions(-) delete mode 100644 nav2_smac_planner/include/nav2_smac_planner/goal_manager.hpp delete mode 100644 nav2_smac_planner/lattice_primitives/sample_primitives/test/output.json delete mode 100644 nav2_smac_planner/test/test_goal_manager.cpp diff --git a/nav2_bringup/launch/tb3_loopback_simulation_launch.py b/nav2_bringup/launch/tb3_loopback_simulation_launch.py index 9f51e66c04b..26c22a31d50 100644 --- a/nav2_bringup/launch/tb3_loopback_simulation_launch.py +++ b/nav2_bringup/launch/tb3_loopback_simulation_launch.py @@ -144,8 +144,6 @@ def generate_launch_description() -> LaunchDescription: 'use_composition': use_composition, 'use_respawn': use_respawn, 'use_localization': 'False', # Don't use SLAM, AMCL - 'use_keepout_zones': 'False', - 'use_speed_zones': 'False', }.items(), ) diff --git a/nav2_bringup/launch/tb3_simulation_launch.py b/nav2_bringup/launch/tb3_simulation_launch.py index e0a5f217646..cc40ed90c56 100644 --- a/nav2_bringup/launch/tb3_simulation_launch.py +++ b/nav2_bringup/launch/tb3_simulation_launch.py @@ -195,8 +195,6 @@ def generate_launch_description() -> LaunchDescription: 'autostart': autostart, 'use_composition': use_composition, 'use_respawn': use_respawn, - 'use_keepout_zones': 'False', - 'use_speed_zones': 'False', }.items(), ) # The SDF file for the world is a xacro file because we wanted to diff --git a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp index b0e3bab0188..8b127960cc3 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp @@ -32,7 +32,6 @@ #include "nav2_smac_planner/node_hybrid.hpp" #include "nav2_smac_planner/node_lattice.hpp" #include "nav2_smac_planner/node_basic.hpp" -#include "nav2_smac_planner/goal_manager.hpp" #include "nav2_smac_planner/types.hpp" #include "nav2_smac_planner/constants.hpp" @@ -55,9 +54,6 @@ class AStarAlgorithm typedef typename NodeT::CoordinateVector CoordinateVector; typedef typename NodeVector::iterator NeighborIterator; typedef std::function NodeGetter; - typedef GoalManager GoalManagerT; - typedef std::vector> GoalStateVector; - /** * @struct nav2_smac_planner::NodeComparator @@ -94,8 +90,6 @@ class AStarAlgorithm * or planning time exceeded * @param max_planning_time Maximum time (in seconds) to wait for a plan, createPath returns * false after this timeout - * @param lookup_table_size Size of the lookup table to store heuristic values - * @param dim_3_size Number of quantization bins */ void initialize( const bool & allow_unknown, @@ -131,15 +125,11 @@ class AStarAlgorithm * @param mx The node X index of the goal * @param my The node Y index of the goal * @param dim_3 The node dim_3 index of the goal - * @param goal_heading_mode The goal heading mode to use - * @param coarse_search_resolution The resolution to search for goal heading */ void setGoal( const float & mx, const float & my, - const unsigned int & dim_3, - const GoalHeadingMode & goal_heading_mode = GoalHeadingMode::DEFAULT, - const int & coarse_search_resolution = 1); + const unsigned int & dim_3); /** * @brief Set the starting pose for planning, as a node index @@ -164,6 +154,12 @@ class AStarAlgorithm */ NodePtr & getStart(); + /** + * @brief Get pointer reference to goal node + * @return Node pointer reference to goal node + */ + NodePtr & getGoal(); + /** * @brief Get maximum number of on-approach iterations after within threshold * @return Reference to Maximum on-approach iterations parameter @@ -194,18 +190,6 @@ class AStarAlgorithm */ unsigned int & getSizeDim3(); - /** - * @brief Get the resolution of the coarse search - * @return Size of the goals to expand - */ - unsigned int getCoarseSearchResolution(); - - /** - * @brief Get the goals manager class - * @return Goal manager class - */ - GoalManagerT getGoalManager(); - protected: /** * @brief Get pointer to next goal in open set @@ -226,6 +210,13 @@ class AStarAlgorithm */ inline NodePtr addToGraph(const uint64_t & index); + /** + * @brief Check if this node is the goal node + * @param node Node pointer to check if its the goal node + * @return if node is goal + */ + inline bool isGoal(NodePtr & node); + /** * @brief Get cost of heuristic of node * @param node Node pointer to get heuristic for @@ -249,11 +240,6 @@ class AStarAlgorithm */ inline void clearGraph(); - /** - * @brief Check if node has been visited - * @param current_node Node to check if visited - * @return if node has been visited - */ inline bool onVisitationCheckNode(const NodePtr & node); /** @@ -274,11 +260,12 @@ class AStarAlgorithm unsigned int _x_size; unsigned int _y_size; unsigned int _dim3_size; - unsigned int _coarse_search_resolution; SearchInfo _search_info; + Coordinates _goal_coordinates; NodePtr _start; - GoalManagerT _goal_manager; + NodePtr _goal; + Graph _graph; NodeQueue _queue; diff --git a/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp index 0c5cdede0bd..6be84d1c109 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp @@ -15,10 +15,6 @@ #ifndef NAV2_SMAC_PLANNER__ANALYTIC_EXPANSION_HPP_ #define NAV2_SMAC_PLANNER__ANALYTIC_EXPANSION_HPP_ -#include -#include -#include - #include #include #include @@ -39,10 +35,8 @@ class AnalyticExpansion { public: typedef NodeT * NodePtr; - typedef std::vector NodeVector; typedef typename NodeT::Coordinates Coordinates; typedef std::function NodeGetter; - typedef typename NodeT::CoordinateVector CoordinateVector; /** * @struct nav2_smac_planner::AnalyticExpansion::AnalyticExpansionNodes @@ -65,33 +59,7 @@ class AnalyticExpansion Coordinates proposed_coords; }; - /** - * @struct AnalyticExpansionNodes - * @brief Analytic expansion nodes and associated metadata - * - * This structure holds a collection of analytic expansion nodes and the number of direction - * changes encountered during the expansion. - */ - struct AnalyticExpansionNodes - { - AnalyticExpansionNodes() = default; - - void add( - NodePtr & node, - Coordinates & initial_coords, - Coordinates & proposed_coords) - { - nodes.emplace_back(node, initial_coords, proposed_coords); - } - - void setDirectionChanges(int changes) - { - direction_changes = changes; - } - - std::vector nodes; - int direction_changes{0}; - }; + typedef std::vector AnalyticExpansionNodes; /** * @brief Constructor for analytic expansion object @@ -111,22 +79,17 @@ class AnalyticExpansion /** * @brief Attempt an analytic path completion * @param node The node to start the analytic path from - * @param coarse_check_goals Coarse list of goals nodes to plan to - * @param fine_check_goals Fine list of goals nodes to plan to - * @param goals_coords vector of goal coordinates to plan to + * @param goal The goal node to plan to * @param getter Gets a node at a set of coordinates * @param iterations Iterations to run over - * @param closest_distance Closest distance to goal - * @return Node pointer reference to goal node with the best score out of the goals node if - * successful, else return nullptr + * @param best_cost Best heuristic cost to propertionally expand more closer to the goal + * @return Node pointer reference to goal node if successful, else + * return nullptr */ NodePtr tryAnalyticExpansion( const NodePtr & current_node, - const NodeVector & coarse_check_goals, - const NodeVector & fine_check_goals, - const CoordinateVector & goals_coords, - const NodeGetter & getter, int & iterations, - int & closest_distance); + const NodePtr & goal_node, + const NodeGetter & getter, int & iterations, int & best_cost); /** * @brief Perform an analytic path expansion to the goal @@ -140,21 +103,6 @@ class AnalyticExpansion const NodePtr & node, const NodePtr & goal, const NodeGetter & getter, const ompl::base::StateSpacePtr & state_space); - /** - * @brief Refined analytic path from the current node to the goal - * @param node The node to start the analytic path from. Node head may - * change as a result of refinement - * @param goal_node The goal node to plan to - * @param getter The function object that gets valid nodes from the graph - * @param analytic_nodes The set of analytic nodes to refine - * @return The score of the refined path - */ - float refineAnalyticPath( - NodePtr & node, - const NodePtr & goal_node, - const NodeGetter & getter, - AnalyticExpansionNodes & analytic_nodes); - /** * @brief Takes final analytic expansion and appends to current expanded node * @param node The node to start the analytic path from @@ -166,13 +114,6 @@ class AnalyticExpansion const NodePtr & node, const NodePtr & goal, const AnalyticExpansionNodes & expanded_nodes); - /** - * @brief Counts the number of direction changes in a Reeds-Shepp path - * @param path The Reeds-Shepp path to count direction changes in - * @return The number of direction changes in the path - */ - int countDirectionChanges(const ompl::base::ReedsSheppStateSpace::ReedsSheppPath & path); - /** * @brief Takes an expanded nodes to clean up, if necessary, of any state * information that may be polluting it from a prior search iteration diff --git a/nav2_smac_planner/include/nav2_smac_planner/constants.hpp b/nav2_smac_planner/include/nav2_smac_planner/constants.hpp index 6344c86fb89..af44ce53659 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/constants.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/constants.hpp @@ -28,14 +28,6 @@ enum class MotionModel STATE_LATTICE = 4, }; -enum class GoalHeadingMode -{ - UNKNOWN = 0, - DEFAULT = 1, - BIDIRECTIONAL = 2, - ALL_DIRECTION = 3, -}; - inline std::string toString(const MotionModel & n) { switch (n) { @@ -67,33 +59,6 @@ inline MotionModel fromString(const std::string & n) } } -inline std::string toString(const GoalHeadingMode & n) -{ - switch (n) { - case GoalHeadingMode::DEFAULT: - return "DEFAULT"; - case GoalHeadingMode::BIDIRECTIONAL: - return "BIDIRECTIONAL"; - case GoalHeadingMode::ALL_DIRECTION: - return "ALL_DIRECTION"; - default: - return "Unknown"; - } -} - -inline GoalHeadingMode fromStringToGH(const std::string & n) -{ - if (n == "DEFAULT") { - return GoalHeadingMode::DEFAULT; - } else if (n == "BIDIRECTIONAL") { - return GoalHeadingMode::BIDIRECTIONAL; - } else if (n == "ALL_DIRECTION") { - return GoalHeadingMode::ALL_DIRECTION; - } else { - return GoalHeadingMode::UNKNOWN; - } -} - const float UNKNOWN_COST = 255.0; const float OCCUPIED_COST = 254.0; const float INSCRIBED_COST = 253.0; diff --git a/nav2_smac_planner/include/nav2_smac_planner/goal_manager.hpp b/nav2_smac_planner/include/nav2_smac_planner/goal_manager.hpp deleted file mode 100644 index 826fe9d47e5..00000000000 --- a/nav2_smac_planner/include/nav2_smac_planner/goal_manager.hpp +++ /dev/null @@ -1,190 +0,0 @@ -// Copyright (c) 2020, Samsung Research America -// Copyright (c) 2020, Applied Electric Vehicles Pty Ltd -// Copyright (c) 2024, Stevedan Ogochukwu Omodolor Omodia -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. Reserved. - -#ifndef NAV2_SMAC_PLANNER__GOAL_MANAGER_HPP_ -#define NAV2_SMAC_PLANNER__GOAL_MANAGER_HPP_ - -#include -#include -#include - -#include "nav2_smac_planner/types.hpp" -#include "nav2_smac_planner/node_2d.hpp" -#include "nav2_smac_planner/node_hybrid.hpp" -#include "nav2_smac_planner/node_lattice.hpp" -#include "nav2_smac_planner/node_basic.hpp" -#include "nav2_smac_planner/collision_checker.hpp" - - -namespace nav2_smac_planner -{ - -/** -* @class nav2_smac_planner::GoalManager -* @brief Responsible for managing multiple variables storing information on the goal -*/ -template -class GoalManager -{ -public: - typedef NodeT * NodePtr; - typedef std::vector NodeVector; - typedef std::unordered_set NodeSet; - typedef std::vector> GoalStateVector; - typedef typename NodeT::Coordinates Coordinates; - typedef typename NodeT::CoordinateVector CoordinateVector; - - /** - * @brief Constructor: Initializes empty goal state. sets and coordinate lists. - */ - GoalManager() - : _goals_set(NodeSet()), - _goals_state(GoalStateVector()), - _goals_coordinate(CoordinateVector()) - { - } - - /** - * @brief Destructor for the GoalManager - */ - ~GoalManager() = default; - - /** - * @brief Checks if the goals set is empty - * @return true if the goals set is empty - */ - bool goalsIsEmpty() - { - return _goals_state.empty(); - } - - /** - * @brief Sets the internal goals' state list to the provided goals. - * @param goals Vector of goals state to set, - */ - - void setGoalStates( - GoalStateVector & goals_state) - { - clear(); - _goals_state = goals_state; - } - - /** - * @brief Clears all internal goal data, including goals, states, and coordinates. - */ - void clear() - { - _goals_set.clear(); - _goals_state.clear(); - _goals_coordinate.clear(); - } - - /** - * @brief Populates coarse and fine goal lists for analytic expansion. - * @param coarse_check_goals Output list of goals for coarse search expansion. - * @param fine_check_goals Output list of goals for fine search refinement. - * @param coarse_search_resolution Number of fine goals per coarse goal. - */ - void prepareGoalsForAnalyticExpansion( - NodeVector & coarse_check_goals, NodeVector & fine_check_goals, - int coarse_search_resolution) - { - for (unsigned int i = 0; i < _goals_state.size(); i++) { - if (_goals_state[i].is_valid) { - if (i % coarse_search_resolution == 0) { - coarse_check_goals.push_back(_goals_state[i].goal); - } else { - fine_check_goals.push_back(_goals_state[i].goal); - } - } - } - } - - /** - * @brief Filters and marks invalid goals based on collision checking and tolerance thresholds. - * - * Stores only valid (or tolerably infeasible) goals into internal goal sets and coordinates. - * - * @param tolerance Heuristic tolerance allowed for infeasible goals. - * @param collision_checker Collision checker to validate goal positions. - * @param traverse_unknown Flag whether traversal through unknown space is allowed. - */ - void removeInvalidGoals( - const float & tolerance, - GridCollisionChecker * collision_checker, - const bool & traverse_unknown) - { - for (unsigned int i = 0; i < _goals_state.size(); i++) { - if (_goals_state[i].goal->isNodeValid(traverse_unknown, collision_checker) || - tolerance > 0.001) - { - _goals_state[i].is_valid = true; - _goals_set.insert(_goals_state[i].goal); - _goals_coordinate.push_back(_goals_state[i].goal->pose); - } else { - _goals_state[i].is_valid = false; - } - } - } - - /** - * @brief Check if a given node is part of the goal set. - * @param node Node pointer to check. - * @return if node matches any goal in the goal set. - */ - inline bool isGoal(NodePtr & node) - { - return _goals_set.find(node) != _goals_set.end(); - } - - /** - * @brief Get pointer reference to goals set vector - * @return unordered_set of node pointers reference to the goals nodes - */ - inline NodeSet & getGoalsSet() - { - return _goals_set; - } - - /** - * @brief Get pointer reference to goals state - * @return vector of node pointers reference to the goals state - */ - inline GoalStateVector & getGoalsState() - { - return _goals_state; - } - - /** - * @brief Get pointer reference to goals coordinates - * @return vector of goals coordinates reference to the goals coordinates - */ - inline CoordinateVector & getGoalsCoordinates() - { - return _goals_coordinate; - } - -protected: - NodeSet _goals_set; - GoalStateVector _goals_state; - CoordinateVector _goals_coordinate; - NodePtr primary_goal; -}; - -} // namespace nav2_smac_planner - -#endif // NAV2_SMAC_PLANNER__GOAL_MANAGER_HPP_ diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp index 6d54e1696ab..1bd0993a8eb 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp @@ -50,16 +50,6 @@ class Node2D : x(x_in), y(y_in) {} - inline bool operator==(const Coordinates & rhs) const - { - return this->x == rhs.x && this->y == rhs.y; - } - - inline bool operator!=(const Coordinates & rhs) const - { - return !(*this == rhs); - } - float x, y; }; typedef std::vector CoordinateVector; @@ -85,15 +75,6 @@ class Node2D return this->_index == rhs._index; } - /** - * @brief setting continuous coordinate search poses (in partial-cells) - * @param Pose pose - */ - inline void setPose(const Coordinates & pose_in) - { - pose = pose_in; - } - /** * @brief Reset method for new search */ @@ -243,7 +224,7 @@ class Node2D */ static float getHeuristicCost( const Coordinates & node_coords, - const CoordinateVector & goals_coords); + const Coordinates & goal_coordinates); /** * @brief Initialize the neighborhood to be used in A* @@ -283,7 +264,6 @@ class Node2D bool backtracePath(CoordinateVector & path); Node2D * parent; - Coordinates pose; static float cost_travel_multiplier; static std::vector _neighbors_grid_offsets; diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp index cc3650563a8..ee3a4bf231c 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp @@ -167,12 +167,12 @@ class NodeHybrid : x(x_in), y(y_in), theta(theta_in) {} - inline bool operator==(const Coordinates & rhs) const + inline bool operator==(const Coordinates & rhs) { return this->x == rhs.x && this->y == rhs.y && this->theta == rhs.theta; } - inline bool operator!=(const Coordinates & rhs) const + inline bool operator!=(const Coordinates & rhs) { return !(*this == rhs); } @@ -374,7 +374,7 @@ class NodeHybrid */ static float getHeuristicCost( const Coordinates & node_coords, - const CoordinateVector & goals_coords); + const Coordinates & goal_coordinates); /** * @brief Initialize motion models diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp index 5b07e5453bf..824b435447d 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp @@ -314,7 +314,7 @@ class NodeLattice */ static float getHeuristicCost( const Coordinates & node_coords, - const CoordinateVector & goals_coords); + const Coordinates & goal_coordinates); /** * @brief Initialize motion models @@ -408,8 +408,8 @@ class NodeLattice bool backtracePath(CoordinateVector & path); /** - * @brief add node to the path - * @param current_node + * \brief add node to the path + * \param current_node */ void addNodeToPath(NodePtr current_node, CoordinateVector & path); diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp index 0b50c0e15fd..ca88725cc23 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp @@ -123,8 +123,6 @@ class SmacPlannerHybrid : public nav2_core::GlobalPlanner bool _debug_visualizations; std::string _motion_model_for_search; MotionModel _motion_model; - GoalHeadingMode _goal_heading_mode; - int _coarse_search_resolution; rclcpp_lifecycle::LifecyclePublisher::SharedPtr _raw_plan_publisher; rclcpp_lifecycle::LifecyclePublisher::SharedPtr _planned_footprints_publisher; diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp index 8cead0a0145..42b30066b8a 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp @@ -122,8 +122,6 @@ class SmacPlannerLattice : public nav2_core::GlobalPlanner _smoothed_footprints_publisher; rclcpp_lifecycle::LifecyclePublisher::SharedPtr _expansions_publisher; - GoalHeadingMode _goal_heading_mode; - int _coarse_search_resolution; std::mutex _mutex; rclcpp_lifecycle::LifecycleNode::WeakPtr _node; diff --git a/nav2_smac_planner/include/nav2_smac_planner/types.hpp b/nav2_smac_planner/include/nav2_smac_planner/types.hpp index b0ccaa8bb29..cc6c9975c5f 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/types.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/types.hpp @@ -188,18 +188,6 @@ struct MotionPrimitive MotionPoses poses; }; - /** - * @struct nav2_smac_planner::GoalState - * @brief A struct to store the goal state - */ - -template -struct GoalState -{ - NodeT * goal = nullptr; - bool is_valid = true; -}; - typedef std::vector MotionPrimitives; typedef std::vector MotionPrimitivePtrs; diff --git a/nav2_smac_planner/lattice_primitives/sample_primitives/test/output.json b/nav2_smac_planner/lattice_primitives/sample_primitives/test/output.json deleted file mode 100644 index feae5b38326..00000000000 --- a/nav2_smac_planner/lattice_primitives/sample_primitives/test/output.json +++ /dev/null @@ -1,3931 +0,0 @@ -{ - "version": 1.0, - "date_generated": "2022-03-17", - "lattice_metadata": { - "motion_model": "ackermann", - "turning_radius": 0.5, - "grid_resolution": 0.05, - "stopping_threshold": 5, - "num_of_headings": 15, - "heading_angles": [ - 0.0, - 0.4636476090008061, - 0.7853981633974483, - 1.1071487177940904, - 1.5707963267948966, - 2.0344439357957027, - 2.356194490192345, - 2.677945044588987, - 3.141592653589793, - 3.6052402625905993, - 3.9269908169872414, - 4.2487413713838835, - 4.71238898038469, - 5.176036589385496, - 5.497787143782138 - ], - "number_of_trajectories": 80 - }, - "primitives": [ - { - "trajectory_id": 0, - "start_angle_index": 0, - "end_angle_index": 13, - "left_turn": false, - "trajectory_radius": 0.52586, - "trajectory_length": 0.64852, - "arc_length": 0.58221, - "straight_length": 0.06631, - "poses": [ - [ - 0.04981, - -0.00236, - 6.18831988221979 - ], - [ - 0.09917, - -0.00944, - 6.093454457259993 - ], - [ - 0.14765, - -0.02115, - 5.998589032300196 - ], - [ - 0.19479, - -0.03741, - 5.903723607340399 - ], - [ - 0.24018, - -0.05805, - 5.808858182380602 - ], - [ - 0.28341, - -0.08291, - 5.713992757420805 - ], - [ - 0.3241, - -0.11175, - 5.619127332461009 - ], - [ - 0.36187, - -0.14431, - 5.524261907501212 - ], - [ - 0.39638, - -0.1803, - 5.429396482541415 - ], - [ - 0.42733, - -0.2194, - 5.334531057581619 - ], - [ - 0.45444, - -0.26126, - 5.239665632621822 - ], - [ - 0.47769, - -0.30538, - 5.176036589385496 - ], - [ - 0.5, - -0.35, - 5.176036589385496 - ] - ] - }, - { - "trajectory_id": 1, - "start_angle_index": 0, - "end_angle_index": 15, - "left_turn": false, - "trajectory_radius": 0.63541, - "trajectory_length": 0.36821, - "arc_length": 0.29461, - "straight_length": 0.07361, - "poses": [ - [ - 0.05254, - -0.00218, - 6.200401159939406 - ], - [ - 0.10472, - -0.00869, - 6.117617012699226 - ], - [ - 0.15619, - -0.0195, - 6.034832865459045 - ], - [ - 0.20658, - -0.03452, - 5.952048718218864 - ], - [ - 0.25556, - -0.05366, - 5.869264570978684 - ], - [ - 0.30295, - -0.07648, - 5.81953769817878 - ], - [ - 0.35, - -0.1, - 5.81953769817878 - ] - ] - }, - { - "trajectory_id": 2, - "start_angle_index": 0, - "end_angle_index": 0, - "left_turn": true, - "trajectory_radius": 0.0, - "trajectory_length": 0.15, - "arc_length": 0.0, - "straight_length": 0.15, - "poses": [ - [ - 0.05, - 0.0, - 0.0 - ], - [ - 0.1, - 0.0, - 0.0 - ], - [ - 0.15, - 0.0, - 0.0 - ] - ] - }, - { - "trajectory_id": 3, - "start_angle_index": 0, - "end_angle_index": 1, - "left_turn": true, - "trajectory_radius": 0.63541, - "trajectory_length": 0.36821, - "arc_length": 0.29461, - "straight_length": 0.07361, - "poses": [ - [ - 0.05254, - 0.00218, - 0.0827841472401804 - ], - [ - 0.10472, - 0.00869, - 0.1655682944803608 - ], - [ - 0.15619, - 0.0195, - 0.2483524417205412 - ], - [ - 0.20658, - 0.03452, - 0.3311365889607216 - ], - [ - 0.25556, - 0.05366, - 0.413920736200902 - ], - [ - 0.30295, - 0.07648, - 0.4636476090008061 - ], - [ - 0.35, - 0.1, - 0.4636476090008061 - ] - ] - }, - { - "trajectory_id": 4, - "start_angle_index": 0, - "end_angle_index": 3, - "left_turn": true, - "trajectory_radius": 0.52586, - "trajectory_length": 0.64852, - "arc_length": 0.58221, - "straight_length": 0.06631, - "poses": [ - [ - 0.04981, - 0.00236, - 0.0948654249597968 - ], - [ - 0.09917, - 0.00944, - 0.1897308499195936 - ], - [ - 0.14765, - 0.02115, - 0.2845962748793904 - ], - [ - 0.19479, - 0.03741, - 0.3794616998391872 - ], - [ - 0.24018, - 0.05805, - 0.4743271247989839 - ], - [ - 0.28341, - 0.08291, - 0.5691925497587808 - ], - [ - 0.3241, - 0.11175, - 0.6640579747185776 - ], - [ - 0.36187, - 0.14431, - 0.7589233996783744 - ], - [ - 0.39638, - 0.1803, - 0.8537888246381711 - ], - [ - 0.42733, - 0.2194, - 0.9486542495979677 - ], - [ - 0.45444, - 0.26126, - 1.0435196745577644 - ], - [ - 0.47769, - 0.30538, - 1.1071487177940904 - ], - [ - 0.5, - 0.35, - 1.1071487177940904 - ] - ] - }, - { - "trajectory_id": 5, - "start_angle_index": 1, - "end_angle_index": 15, - "left_turn": false, - "trajectory_radius": 0.50312, - "trajectory_length": 0.46654, - "arc_length": 0.46654, - "straight_length": 0.0, - "poses": [ - [ - 0.04747, - 0.02076, - 0.360614807000627 - ], - [ - 0.09683, - 0.03652, - 0.2575820050004478 - ], - [ - 0.14755, - 0.04712, - 0.15454920300026875 - ], - [ - 0.19909, - 0.05245, - 0.051516401000089584 - ], - [ - 0.25091, - 0.05245, - 6.231668906179497 - ], - [ - 0.30245, - 0.04712, - 6.128636104179318 - ], - [ - 0.35317, - 0.03652, - 6.025603302179138 - ], - [ - 0.40253, - 0.02076, - 5.922570500178959 - ], - [ - 0.45, - 0.0, - 5.81953769817878 - ] - ] - }, - { - "trajectory_id": 6, - "start_angle_index": 1, - "end_angle_index": 15, - "left_turn": false, - "trajectory_radius": 0.61492, - "trajectory_length": 0.57021, - "arc_length": 0.57021, - "straight_length": 0.0, - "poses": [ - [ - 0.04729, - 0.0212, - 0.37934804372793224 - ], - [ - 0.09619, - 0.03835, - 0.29504847845505844 - ], - [ - 0.14636, - 0.05131, - 0.21074891318218458 - ], - [ - 0.19745, - 0.06001, - 0.12644934790931073 - ], - [ - 0.24909, - 0.06437, - 0.04214978263643687 - ], - [ - 0.30091, - 0.06437, - 6.2410355245431495 - ], - [ - 0.35255, - 0.06001, - 6.156735959270275 - ], - [ - 0.40364, - 0.05131, - 6.0724363939974015 - ], - [ - 0.45381, - 0.03835, - 5.988136828724528 - ], - [ - 0.50271, - 0.0212, - 5.903837263451654 - ], - [ - 0.55, - 0.0, - 5.81953769817878 - ] - ] - }, - { - "trajectory_id": 7, - "start_angle_index": 1, - "end_angle_index": 0, - "left_turn": false, - "trajectory_radius": 0.63541, - "trajectory_length": 0.36821, - "arc_length": 0.29461, - "straight_length": 0.07361, - "poses": [ - [ - 0.04705, - 0.02352, - 0.4636476090008061 - ], - [ - 0.09444, - 0.04634, - 0.413920736200902 - ], - [ - 0.14342, - 0.06548, - 0.3311365889607216 - ], - [ - 0.19381, - 0.0805, - 0.24835244172054122 - ], - [ - 0.24528, - 0.09131, - 0.16556829448036087 - ], - [ - 0.29746, - 0.09782, - 0.08278414724018052 - ], - [ - 0.35, - 0.1, - 0.0 - ] - ] - }, - { - "trajectory_id": 8, - "start_angle_index": 1, - "end_angle_index": 1, - "left_turn": true, - "trajectory_radius": 0.0, - "trajectory_length": 0.22361, - "arc_length": 0.0, - "straight_length": 0.22361, - "poses": [ - [ - 0.05, - 0.025, - 0.4636476090008061 - ], - [ - 0.1, - 0.05, - 0.4636476090008061 - ], - [ - 0.15, - 0.075, - 0.4636476090008061 - ], - [ - 0.2, - 0.1, - 0.4636476090008061 - ] - ] - }, - { - "trajectory_id": 9, - "start_angle_index": 1, - "end_angle_index": 2, - "left_turn": true, - "trajectory_radius": 0.68895, - "trajectory_length": 0.25129, - "arc_length": 0.22167, - "straight_length": 0.02962, - "poses": [ - [ - 0.04409, - 0.0241, - 0.536596705651906 - ], - [ - 0.08631, - 0.05134, - 0.6095458023030058 - ], - [ - 0.12643, - 0.08159, - 0.6824948989541056 - ], - [ - 0.16424, - 0.11469, - 0.7554439956052055 - ], - [ - 0.2, - 0.15, - 0.7853981633974483 - ] - ] - }, - { - "trajectory_id": 10, - "start_angle_index": 2, - "end_angle_index": 0, - "left_turn": false, - "trajectory_radius": 0.51213, - "trajectory_length": 0.4401, - "arc_length": 0.40223, - "straight_length": 0.03787, - "poses": [ - [ - 0.03617, - 0.03288, - 0.6899154709776123 - ], - [ - 0.07532, - 0.06215, - 0.5944327785577764 - ], - [ - 0.11707, - 0.08756, - 0.4989500861379405 - ], - [ - 0.16106, - 0.10888, - 0.4034673937181046 - ], - [ - 0.20688, - 0.1259, - 0.30798470129826866 - ], - [ - 0.25412, - 0.13848, - 0.21250200887843262 - ], - [ - 0.30234, - 0.1465, - 0.11701931645859664 - ], - [ - 0.3511, - 0.14988, - 0.021536624038760666 - ], - [ - 0.4, - 0.15, - 0.0 - ] - ] - }, - { - "trajectory_id": 11, - "start_angle_index": 2, - "end_angle_index": 1, - "left_turn": false, - "trajectory_radius": 0.68895, - "trajectory_length": 0.25129, - "arc_length": 0.22167, - "straight_length": 0.02962, - "poses": [ - [ - 0.03576, - 0.03531, - 0.7554439956052055 - ], - [ - 0.07357, - 0.06841, - 0.6824948989541056 - ], - [ - 0.11369, - 0.09866, - 0.6095458023030058 - ], - [ - 0.15591, - 0.1259, - 0.5365967056519059 - ], - [ - 0.2, - 0.15, - 0.4636476090008061 - ] - ] - }, - { - "trajectory_id": 12, - "start_angle_index": 2, - "end_angle_index": 2, - "left_turn": true, - "trajectory_radius": 0.0, - "trajectory_length": 0.21213, - "arc_length": 0.0, - "straight_length": 0.21213, - "poses": [ - [ - 0.0375, - 0.0375, - 0.7853981633974483 - ], - [ - 0.075, - 0.075, - 0.7853981633974483 - ], - [ - 0.1125, - 0.1125, - 0.7853981633974483 - ], - [ - 0.15, - 0.15, - 0.7853981633974483 - ] - ] - }, - { - "trajectory_id": 13, - "start_angle_index": 2, - "end_angle_index": 3, - "left_turn": true, - "trajectory_radius": 0.68895, - "trajectory_length": 0.25129, - "arc_length": 0.22167, - "straight_length": 0.02962, - "poses": [ - [ - 0.03531, - 0.03576, - 0.8153523311896911 - ], - [ - 0.06841, - 0.07357, - 0.8883014278407909 - ], - [ - 0.09866, - 0.11369, - 0.9612505244918907 - ], - [ - 0.1259, - 0.15591, - 1.0341996211429907 - ], - [ - 0.15, - 0.2, - 1.1071487177940904 - ] - ] - }, - { - "trajectory_id": 14, - "start_angle_index": 2, - "end_angle_index": 4, - "left_turn": true, - "trajectory_radius": 0.51213, - "trajectory_length": 0.4401, - "arc_length": 0.40223, - "straight_length": 0.03787, - "poses": [ - [ - 0.03288, - 0.03617, - 0.8808808558172843 - ], - [ - 0.06215, - 0.07532, - 0.9763635482371201 - ], - [ - 0.08756, - 0.11707, - 1.071846240656956 - ], - [ - 0.10888, - 0.16106, - 1.167328933076792 - ], - [ - 0.1259, - 0.20688, - 1.262811625496628 - ], - [ - 0.13848, - 0.25412, - 1.358294317916464 - ], - [ - 0.1465, - 0.30234, - 1.4537770103363 - ], - [ - 0.14988, - 0.3511, - 1.549259702756136 - ], - [ - 0.15, - 0.4, - 1.5707963267948966 - ] - ] - }, - { - "trajectory_id": 15, - "start_angle_index": 3, - "end_angle_index": 2, - "left_turn": false, - "trajectory_radius": 0.68895, - "trajectory_length": 0.25129, - "arc_length": 0.22167, - "straight_length": 0.02962, - "poses": [ - [ - 0.0241, - 0.04409, - 1.0341996211429905 - ], - [ - 0.05134, - 0.08631, - 0.9612505244918907 - ], - [ - 0.08159, - 0.12643, - 0.8883014278407909 - ], - [ - 0.11469, - 0.16424, - 0.8153523311896911 - ], - [ - 0.15, - 0.2, - 0.7853981633974483 - ] - ] - }, - { - "trajectory_id": 16, - "start_angle_index": 3, - "end_angle_index": 3, - "left_turn": true, - "trajectory_radius": 0.0, - "trajectory_length": 0.22361, - "arc_length": 0.0, - "straight_length": 0.22361, - "poses": [ - [ - 0.025, - 0.05, - 1.1071487177940904 - ], - [ - 0.05, - 0.1, - 1.1071487177940904 - ], - [ - 0.075, - 0.15, - 1.1071487177940904 - ], - [ - 0.1, - 0.2, - 1.1071487177940904 - ] - ] - }, - { - "trajectory_id": 17, - "start_angle_index": 3, - "end_angle_index": 4, - "left_turn": true, - "trajectory_radius": 0.63541, - "trajectory_length": 0.36821, - "arc_length": 0.29461, - "straight_length": 0.07361, - "poses": [ - [ - 0.02352, - 0.04705, - 1.1071487177940904 - ], - [ - 0.04634, - 0.09444, - 1.1568755905939945 - ], - [ - 0.06548, - 0.14342, - 1.239659737834175 - ], - [ - 0.0805, - 0.19381, - 1.3224438850743554 - ], - [ - 0.09131, - 0.24528, - 1.4052280323145356 - ], - [ - 0.09782, - 0.29746, - 1.488012179554716 - ], - [ - 0.1, - 0.35, - 1.5707963267948966 - ] - ] - }, - { - "trajectory_id": 18, - "start_angle_index": 3, - "end_angle_index": 5, - "left_turn": true, - "trajectory_radius": 0.50312, - "trajectory_length": 0.46654, - "arc_length": 0.46654, - "straight_length": 0.0, - "poses": [ - [ - 0.02076, - 0.04747, - 1.2101815197942696 - ], - [ - 0.03652, - 0.09683, - 1.3132143217944487 - ], - [ - 0.04712, - 0.14755, - 1.4162471237946277 - ], - [ - 0.05245, - 0.19909, - 1.519279925794807 - ], - [ - 0.05245, - 0.25091, - 1.622312727794986 - ], - [ - 0.04712, - 0.30245, - 1.7253455297951654 - ], - [ - 0.03652, - 0.35317, - 1.8283783317953444 - ], - [ - 0.02076, - 0.40253, - 1.9314111337955238 - ], - [ - 0.0, - 0.45, - 2.0344439357957027 - ] - ] - }, - { - "trajectory_id": 19, - "start_angle_index": 3, - "end_angle_index": 5, - "left_turn": true, - "trajectory_radius": 0.61492, - "trajectory_length": 0.57021, - "arc_length": 0.57021, - "straight_length": 0.0, - "poses": [ - [ - 0.0212, - 0.04729, - 1.1914482830669642 - ], - [ - 0.03835, - 0.09619, - 1.2757478483398381 - ], - [ - 0.05131, - 0.14636, - 1.3600474136127119 - ], - [ - 0.06001, - 0.19745, - 1.4443469788855858 - ], - [ - 0.06437, - 0.24909, - 1.5286465441584596 - ], - [ - 0.06437, - 0.30091, - 1.6129461094313333 - ], - [ - 0.06001, - 0.35255, - 1.6972456747042073 - ], - [ - 0.05131, - 0.40364, - 1.7815452399770813 - ], - [ - 0.03835, - 0.45381, - 1.8658448052499552 - ], - [ - 0.0212, - 0.50271, - 1.9501443705228287 - ], - [ - 0.0, - 0.55, - 2.0344439357957027 - ] - ] - }, - { - "trajectory_id": 20, - "start_angle_index": 4, - "end_angle_index": 1, - "left_turn": false, - "trajectory_radius": 0.52586, - "trajectory_length": 0.64852, - "arc_length": 0.58221, - "straight_length": 0.06631, - "poses": [ - [ - 0.00236, - 0.04981, - 1.4759309018350997 - ], - [ - 0.00944, - 0.09917, - 1.381065476875303 - ], - [ - 0.02115, - 0.14765, - 1.2862000519155061 - ], - [ - 0.03741, - 0.19479, - 1.1913346269557095 - ], - [ - 0.05805, - 0.24018, - 1.0964692019959126 - ], - [ - 0.08291, - 0.28341, - 1.0016037770361157 - ], - [ - 0.11175, - 0.3241, - 0.9067383520763189 - ], - [ - 0.14431, - 0.36187, - 0.8118729271165221 - ], - [ - 0.1803, - 0.39638, - 0.7170075021567255 - ], - [ - 0.2194, - 0.42733, - 0.6221420771969288 - ], - [ - 0.26126, - 0.45444, - 0.5272766522371322 - ], - [ - 0.30538, - 0.47769, - 0.4636476090008061 - ], - [ - 0.35, - 0.5, - 0.4636476090008061 - ] - ] - }, - { - "trajectory_id": 21, - "start_angle_index": 4, - "end_angle_index": 3, - "left_turn": false, - "trajectory_radius": 0.63541, - "trajectory_length": 0.36821, - "arc_length": 0.29461, - "straight_length": 0.07361, - "poses": [ - [ - 0.00218, - 0.05254, - 1.488012179554716 - ], - [ - 0.00869, - 0.10472, - 1.4052280323145356 - ], - [ - 0.0195, - 0.15619, - 1.3224438850743554 - ], - [ - 0.03452, - 0.20658, - 1.239659737834175 - ], - [ - 0.05366, - 0.25556, - 1.1568755905939945 - ], - [ - 0.07648, - 0.30295, - 1.1071487177940904 - ], - [ - 0.1, - 0.35, - 1.1071487177940904 - ] - ] - }, - { - "trajectory_id": 22, - "start_angle_index": 4, - "end_angle_index": 4, - "left_turn": true, - "trajectory_radius": 0.0, - "trajectory_length": 0.15, - "arc_length": 0.0, - "straight_length": 0.15, - "poses": [ - [ - 0.0, - 0.05, - 1.5707963267948966 - ], - [ - 0.0, - 0.1, - 1.5707963267948966 - ], - [ - 0.0, - 0.15, - 1.5707963267948966 - ] - ] - }, - { - "trajectory_id": 23, - "start_angle_index": 4, - "end_angle_index": 5, - "left_turn": true, - "trajectory_radius": 0.63541, - "trajectory_length": 0.36821, - "arc_length": 0.29461, - "straight_length": 0.07361, - "poses": [ - [ - -0.00218, - 0.05254, - 1.653580474035077 - ], - [ - -0.00869, - 0.10472, - 1.7363646212752575 - ], - [ - -0.0195, - 0.15619, - 1.8191487685154377 - ], - [ - -0.03452, - 0.20658, - 1.9019329157556182 - ], - [ - -0.05366, - 0.25556, - 1.9847170629957986 - ], - [ - -0.07648, - 0.30295, - 2.0344439357957027 - ], - [ - -0.1, - 0.35, - 2.0344439357957027 - ] - ] - }, - { - "trajectory_id": 24, - "start_angle_index": 4, - "end_angle_index": 7, - "left_turn": true, - "trajectory_radius": 0.52586, - "trajectory_length": 0.64852, - "arc_length": 0.58221, - "straight_length": 0.06631, - "poses": [ - [ - -0.00236, - 0.04981, - 1.6656617517546934 - ], - [ - -0.00944, - 0.09917, - 1.76052717671449 - ], - [ - -0.02115, - 0.14765, - 1.855392601674287 - ], - [ - -0.03741, - 0.19479, - 1.9502580266340837 - ], - [ - -0.05805, - 0.24018, - 2.0451234515938803 - ], - [ - -0.08291, - 0.28341, - 2.1399888765536774 - ], - [ - -0.11175, - 0.3241, - 2.234854301513474 - ], - [ - -0.14431, - 0.36187, - 2.3297197264732707 - ], - [ - -0.1803, - 0.39638, - 2.4245851514330674 - ], - [ - -0.2194, - 0.42733, - 2.519450576392864 - ], - [ - -0.26126, - 0.45444, - 2.6143160013526607 - ], - [ - -0.30538, - 0.47769, - 2.677945044588987 - ], - [ - -0.35, - 0.5, - 2.677945044588987 - ] - ] - }, - { - "trajectory_id": 25, - "start_angle_index": 5, - "end_angle_index": 3, - "left_turn": false, - "trajectory_radius": 0.50312, - "trajectory_length": 0.46654, - "arc_length": 0.46654, - "straight_length": 0.0, - "poses": [ - [ - -0.02076, - 0.04747, - 1.9314111337955235 - ], - [ - -0.03652, - 0.09683, - 1.8283783317953444 - ], - [ - -0.04712, - 0.14755, - 1.7253455297951654 - ], - [ - -0.05245, - 0.19909, - 1.622312727794986 - ], - [ - -0.05245, - 0.25091, - 1.519279925794807 - ], - [ - -0.04712, - 0.30245, - 1.4162471237946277 - ], - [ - -0.03652, - 0.35317, - 1.3132143217944487 - ], - [ - -0.02076, - 0.40253, - 1.2101815197942694 - ], - [ - 0.0, - 0.45, - 1.1071487177940904 - ] - ] - }, - { - "trajectory_id": 26, - "start_angle_index": 5, - "end_angle_index": 3, - "left_turn": false, - "trajectory_radius": 0.61492, - "trajectory_length": 0.57021, - "arc_length": 0.57021, - "straight_length": 0.0, - "poses": [ - [ - -0.0212, - 0.04729, - 1.950144370522829 - ], - [ - -0.03835, - 0.09619, - 1.865844805249955 - ], - [ - -0.05131, - 0.14636, - 1.7815452399770813 - ], - [ - -0.06001, - 0.19745, - 1.6972456747042073 - ], - [ - -0.06437, - 0.24909, - 1.6129461094313335 - ], - [ - -0.06437, - 0.30091, - 1.5286465441584598 - ], - [ - -0.06001, - 0.35255, - 1.4443469788855858 - ], - [ - -0.05131, - 0.40364, - 1.3600474136127119 - ], - [ - -0.03835, - 0.45381, - 1.275747848339838 - ], - [ - -0.0212, - 0.50271, - 1.1914482830669644 - ], - [ - 0.0, - 0.55, - 1.1071487177940904 - ] - ] - }, - { - "trajectory_id": 27, - "start_angle_index": 5, - "end_angle_index": 4, - "left_turn": false, - "trajectory_radius": 0.63541, - "trajectory_length": 0.36821, - "arc_length": 0.29461, - "straight_length": 0.07361, - "poses": [ - [ - -0.02352, - 0.04705, - 2.0344439357957027 - ], - [ - -0.04634, - 0.09444, - 1.9847170629957986 - ], - [ - -0.06548, - 0.14342, - 1.9019329157556182 - ], - [ - -0.0805, - 0.19381, - 1.8191487685154377 - ], - [ - -0.09131, - 0.24528, - 1.7363646212752575 - ], - [ - -0.09782, - 0.29746, - 1.653580474035077 - ], - [ - -0.1, - 0.35, - 1.5707963267948966 - ] - ] - }, - { - "trajectory_id": 28, - "start_angle_index": 5, - "end_angle_index": 5, - "left_turn": true, - "trajectory_radius": 0.0, - "trajectory_length": 0.22361, - "arc_length": 0.0, - "straight_length": 0.22361, - "poses": [ - [ - -0.025, - 0.05, - 2.0344439357957027 - ], - [ - -0.05, - 0.1, - 2.0344439357957027 - ], - [ - -0.075, - 0.15, - 2.0344439357957027 - ], - [ - -0.1, - 0.2, - 2.0344439357957027 - ] - ] - }, - { - "trajectory_id": 29, - "start_angle_index": 5, - "end_angle_index": 6, - "left_turn": true, - "trajectory_radius": 0.68895, - "trajectory_length": 0.25129, - "arc_length": 0.22167, - "straight_length": 0.02962, - "poses": [ - [ - -0.0241, - 0.04409, - 2.1073930324468026 - ], - [ - -0.05134, - 0.08631, - 2.1803421290979026 - ], - [ - -0.08159, - 0.12643, - 2.253291225749002 - ], - [ - -0.11469, - 0.16424, - 2.326240322400102 - ], - [ - -0.15, - 0.2, - 2.356194490192345 - ] - ] - }, - { - "trajectory_id": 30, - "start_angle_index": 6, - "end_angle_index": 4, - "left_turn": false, - "trajectory_radius": 0.51213, - "trajectory_length": 0.4401, - "arc_length": 0.40223, - "straight_length": 0.03787, - "poses": [ - [ - -0.03288, - 0.03617, - 2.260711797772509 - ], - [ - -0.06215, - 0.07532, - 2.165229105352673 - ], - [ - -0.08756, - 0.11707, - 2.069746412932837 - ], - [ - -0.10888, - 0.16106, - 1.9742637205130011 - ], - [ - -0.1259, - 0.20688, - 1.8787810280931652 - ], - [ - -0.13848, - 0.25412, - 1.7832983356733292 - ], - [ - -0.1465, - 0.30234, - 1.6878156432534932 - ], - [ - -0.14988, - 0.3511, - 1.5923329508336572 - ], - [ - -0.15, - 0.4, - 1.5707963267948966 - ] - ] - }, - { - "trajectory_id": 31, - "start_angle_index": 6, - "end_angle_index": 5, - "left_turn": false, - "trajectory_radius": 0.68895, - "trajectory_length": 0.25129, - "arc_length": 0.22167, - "straight_length": 0.02962, - "poses": [ - [ - -0.03531, - 0.03576, - 2.326240322400102 - ], - [ - -0.06841, - 0.07357, - 2.253291225749002 - ], - [ - -0.09866, - 0.11369, - 2.1803421290979026 - ], - [ - -0.1259, - 0.15591, - 2.1073930324468026 - ], - [ - -0.15, - 0.2, - 2.0344439357957027 - ] - ] - }, - { - "trajectory_id": 32, - "start_angle_index": 6, - "end_angle_index": 6, - "left_turn": true, - "trajectory_radius": 0.0, - "trajectory_length": 0.21213, - "arc_length": 0.0, - "straight_length": 0.21213, - "poses": [ - [ - -0.0375, - 0.0375, - 2.356194490192345 - ], - [ - -0.075, - 0.075, - 2.356194490192345 - ], - [ - -0.1125, - 0.1125, - 2.356194490192345 - ], - [ - -0.15, - 0.15, - 2.356194490192345 - ] - ] - }, - { - "trajectory_id": 33, - "start_angle_index": 6, - "end_angle_index": 7, - "left_turn": true, - "trajectory_radius": 0.68895, - "trajectory_length": 0.25129, - "arc_length": 0.22167, - "straight_length": 0.02962, - "poses": [ - [ - -0.03576, - 0.03531, - 2.3861486579845876 - ], - [ - -0.07357, - 0.06841, - 2.4590977546356876 - ], - [ - -0.11369, - 0.09866, - 2.532046851286787 - ], - [ - -0.15591, - 0.1259, - 2.604995947937887 - ], - [ - -0.2, - 0.15, - 2.677945044588987 - ] - ] - }, - { - "trajectory_id": 34, - "start_angle_index": 6, - "end_angle_index": 8, - "left_turn": true, - "trajectory_radius": 0.51213, - "trajectory_length": 0.4401, - "arc_length": 0.40223, - "straight_length": 0.03787, - "poses": [ - [ - -0.03617, - 0.03288, - 2.4516771826121806 - ], - [ - -0.07532, - 0.06215, - 2.547159875032017 - ], - [ - -0.11707, - 0.08756, - 2.6426425674518526 - ], - [ - -0.16106, - 0.10888, - 2.7381252598716888 - ], - [ - -0.20688, - 0.1259, - 2.8336079522915245 - ], - [ - -0.25412, - 0.13848, - 2.9290906447113603 - ], - [ - -0.30234, - 0.1465, - 3.0245733371311965 - ], - [ - -0.3511, - 0.14988, - 3.1200560295510327 - ], - [ - -0.4, - 0.15, - 3.141592653589793 - ] - ] - }, - { - "trajectory_id": 35, - "start_angle_index": 7, - "end_angle_index": 9, - "left_turn": true, - "trajectory_radius": 0.50312, - "trajectory_length": 0.46654, - "arc_length": 0.46654, - "straight_length": 0.0, - "poses": [ - [ - -0.04747, - 0.02076, - 2.780977846589166 - ], - [ - -0.09683, - 0.03652, - 2.8840106485893453 - ], - [ - -0.14755, - 0.04712, - 2.9870434505895243 - ], - [ - -0.19909, - 0.05245, - 3.0900762525897036 - ], - [ - -0.25091, - 0.05245, - 3.1931090545898826 - ], - [ - -0.30245, - 0.04712, - 3.296141856590062 - ], - [ - -0.35317, - 0.03652, - 3.399174658590241 - ], - [ - -0.40253, - 0.02076, - 3.5022074605904203 - ], - [ - -0.45, - 0.0, - 3.6052402625905993 - ] - ] - }, - { - "trajectory_id": 36, - "start_angle_index": 7, - "end_angle_index": 9, - "left_turn": true, - "trajectory_radius": 0.61492, - "trajectory_length": 0.57021, - "arc_length": 0.57021, - "straight_length": 0.0, - "poses": [ - [ - -0.04729, - 0.0212, - 2.762244609861861 - ], - [ - -0.09619, - 0.03835, - 2.8465441751347345 - ], - [ - -0.14636, - 0.05131, - 2.9308437404076084 - ], - [ - -0.19745, - 0.06001, - 3.0151433056804824 - ], - [ - -0.24909, - 0.06437, - 3.0994428709533564 - ], - [ - -0.30091, - 0.06437, - 3.18374243622623 - ], - [ - -0.35255, - 0.06001, - 3.268042001499104 - ], - [ - -0.40364, - 0.05131, - 3.352341566771978 - ], - [ - -0.45381, - 0.03835, - 3.4366411320448518 - ], - [ - -0.50271, - 0.0212, - 3.5209406973177253 - ], - [ - -0.55, - 0.0, - 3.6052402625905993 - ] - ] - }, - { - "trajectory_id": 37, - "start_angle_index": 7, - "end_angle_index": 6, - "left_turn": false, - "trajectory_radius": 0.68895, - "trajectory_length": 0.25129, - "arc_length": 0.22167, - "straight_length": 0.02962, - "poses": [ - [ - -0.04409, - 0.0241, - 2.604995947937887 - ], - [ - -0.08631, - 0.05134, - 2.532046851286787 - ], - [ - -0.12643, - 0.08159, - 2.4590977546356876 - ], - [ - -0.16424, - 0.11469, - 2.3861486579845876 - ], - [ - -0.2, - 0.15, - 2.356194490192345 - ] - ] - }, - { - "trajectory_id": 38, - "start_angle_index": 7, - "end_angle_index": 7, - "left_turn": true, - "trajectory_radius": 0.0, - "trajectory_length": 0.22361, - "arc_length": 0.0, - "straight_length": 0.22361, - "poses": [ - [ - -0.05, - 0.025, - 2.677945044588987 - ], - [ - -0.1, - 0.05, - 2.677945044588987 - ], - [ - -0.15, - 0.075, - 2.677945044588987 - ], - [ - -0.2, - 0.1, - 2.677945044588987 - ] - ] - }, - { - "trajectory_id": 39, - "start_angle_index": 7, - "end_angle_index": 8, - "left_turn": true, - "trajectory_radius": 0.63541, - "trajectory_length": 0.36821, - "arc_length": 0.29461, - "straight_length": 0.07361, - "poses": [ - [ - -0.04705, - 0.02352, - 2.677945044588987 - ], - [ - -0.09444, - 0.04634, - 2.727671917388891 - ], - [ - -0.14342, - 0.06548, - 2.8104560646290713 - ], - [ - -0.19381, - 0.0805, - 2.893240211869252 - ], - [ - -0.24528, - 0.09131, - 2.976024359109432 - ], - [ - -0.29746, - 0.09782, - 3.058808506349613 - ], - [ - -0.35, - 0.1, - 3.141592653589793 - ] - ] - }, - { - "trajectory_id": 40, - "start_angle_index": 8, - "end_angle_index": 9, - "left_turn": true, - "trajectory_radius": 0.63541, - "trajectory_length": 0.36821, - "arc_length": 0.29461, - "straight_length": 0.07361, - "poses": [ - [ - -0.05254, - -0.00218, - 3.2243768008299734 - ], - [ - -0.10472, - -0.00869, - 3.307160948070154 - ], - [ - -0.15619, - -0.0195, - 3.3899450953103343 - ], - [ - -0.20658, - -0.03452, - 3.472729242550515 - ], - [ - -0.25556, - -0.05366, - 3.555513389790695 - ], - [ - -0.30295, - -0.07648, - 3.6052402625905993 - ], - [ - -0.35, - -0.1, - 3.6052402625905993 - ] - ] - }, - { - "trajectory_id": 41, - "start_angle_index": 8, - "end_angle_index": 11, - "left_turn": true, - "trajectory_radius": 0.52586, - "trajectory_length": 0.64852, - "arc_length": 0.58221, - "straight_length": 0.06631, - "poses": [ - [ - -0.04981, - -0.00236, - 3.23645807854959 - ], - [ - -0.09917, - -0.00944, - 3.331323503509387 - ], - [ - -0.14765, - -0.02115, - 3.4261889284691835 - ], - [ - -0.19479, - -0.03741, - 3.52105435342898 - ], - [ - -0.24018, - -0.05805, - 3.615919778388777 - ], - [ - -0.28341, - -0.08291, - 3.710785203348574 - ], - [ - -0.3241, - -0.11175, - 3.8056506283083706 - ], - [ - -0.36187, - -0.14431, - 3.9005160532681673 - ], - [ - -0.39638, - -0.1803, - 3.995381478227964 - ], - [ - -0.42733, - -0.2194, - 4.090246903187761 - ], - [ - -0.45444, - -0.26126, - 4.185112328147557 - ], - [ - -0.47769, - -0.30538, - 4.2487413713838835 - ], - [ - -0.5, - -0.35, - 4.2487413713838835 - ] - ] - }, - { - "trajectory_id": 42, - "start_angle_index": 8, - "end_angle_index": 5, - "left_turn": false, - "trajectory_radius": 0.52586, - "trajectory_length": 0.64852, - "arc_length": 0.58221, - "straight_length": 0.06631, - "poses": [ - [ - -0.04981, - 0.00236, - 3.0467272286299965 - ], - [ - -0.09917, - 0.00944, - 2.9518618036701993 - ], - [ - -0.14765, - 0.02115, - 2.8569963787104027 - ], - [ - -0.19479, - 0.03741, - 2.762130953750606 - ], - [ - -0.24018, - 0.05805, - 2.6672655287908094 - ], - [ - -0.28341, - 0.08291, - 2.5724001038310123 - ], - [ - -0.3241, - 0.11175, - 2.4775346788712156 - ], - [ - -0.36187, - 0.14431, - 2.382669253911419 - ], - [ - -0.39638, - 0.1803, - 2.2878038289516223 - ], - [ - -0.42733, - 0.2194, - 2.1929384039918256 - ], - [ - -0.45444, - 0.26126, - 2.098072979032029 - ], - [ - -0.47769, - 0.30538, - 2.0344439357957027 - ], - [ - -0.5, - 0.35, - 2.0344439357957027 - ] - ] - }, - { - "trajectory_id": 43, - "start_angle_index": 8, - "end_angle_index": 7, - "left_turn": false, - "trajectory_radius": 0.63541, - "trajectory_length": 0.36821, - "arc_length": 0.29461, - "straight_length": 0.07361, - "poses": [ - [ - -0.05254, - 0.00218, - 3.058808506349613 - ], - [ - -0.10472, - 0.00869, - 2.976024359109432 - ], - [ - -0.15619, - 0.0195, - 2.893240211869252 - ], - [ - -0.20658, - 0.03452, - 2.8104560646290713 - ], - [ - -0.25556, - 0.05366, - 2.727671917388891 - ], - [ - -0.30295, - 0.07648, - 2.677945044588987 - ], - [ - -0.35, - 0.1, - 2.677945044588987 - ] - ] - }, - { - "trajectory_id": 44, - "start_angle_index": 8, - "end_angle_index": 8, - "left_turn": true, - "trajectory_radius": 0.0, - "trajectory_length": 0.15, - "arc_length": 0.0, - "straight_length": 0.15, - "poses": [ - [ - -0.05, - 0.0, - 3.141592653589793 - ], - [ - -0.1, - 0.0, - 3.141592653589793 - ], - [ - -0.15, - 0.0, - 3.141592653589793 - ] - ] - }, - { - "trajectory_id": 45, - "start_angle_index": 9, - "end_angle_index": 9, - "left_turn": true, - "trajectory_radius": 0.0, - "trajectory_length": 0.22361, - "arc_length": 0.0, - "straight_length": 0.22361, - "poses": [ - [ - -0.05, - -0.025, - 3.6052402625905993 - ], - [ - -0.1, - -0.05, - 3.6052402625905993 - ], - [ - -0.15, - -0.075, - 3.6052402625905993 - ], - [ - -0.2, - -0.1, - 3.6052402625905993 - ] - ] - }, - { - "trajectory_id": 46, - "start_angle_index": 9, - "end_angle_index": 10, - "left_turn": true, - "trajectory_radius": 0.68895, - "trajectory_length": 0.25129, - "arc_length": 0.22167, - "straight_length": 0.02962, - "poses": [ - [ - -0.04409, - -0.0241, - 3.678189359241699 - ], - [ - -0.08631, - -0.05134, - 3.751138455892799 - ], - [ - -0.12643, - -0.08159, - 3.8240875525438986 - ], - [ - -0.16424, - -0.11469, - 3.8970366491949986 - ], - [ - -0.2, - -0.15, - 3.9269908169872414 - ] - ] - }, - { - "trajectory_id": 47, - "start_angle_index": 9, - "end_angle_index": 7, - "left_turn": false, - "trajectory_radius": 0.50312, - "trajectory_length": 0.46654, - "arc_length": 0.46654, - "straight_length": 0.0, - "poses": [ - [ - -0.04747, - -0.02076, - 3.5022074605904203 - ], - [ - -0.09683, - -0.03652, - 3.399174658590241 - ], - [ - -0.14755, - -0.04712, - 3.296141856590062 - ], - [ - -0.19909, - -0.05245, - 3.1931090545898826 - ], - [ - -0.25091, - -0.05245, - 3.0900762525897036 - ], - [ - -0.30245, - -0.04712, - 2.9870434505895243 - ], - [ - -0.35317, - -0.03652, - 2.8840106485893453 - ], - [ - -0.40253, - -0.02076, - 2.780977846589166 - ], - [ - -0.45, - 0.0, - 2.677945044588987 - ] - ] - }, - { - "trajectory_id": 48, - "start_angle_index": 9, - "end_angle_index": 7, - "left_turn": false, - "trajectory_radius": 0.61492, - "trajectory_length": 0.57021, - "arc_length": 0.57021, - "straight_length": 0.0, - "poses": [ - [ - -0.04729, - -0.0212, - 3.5209406973177253 - ], - [ - -0.09619, - -0.03835, - 3.4366411320448518 - ], - [ - -0.14636, - -0.05131, - 3.352341566771978 - ], - [ - -0.19745, - -0.06001, - 3.268042001499104 - ], - [ - -0.24909, - -0.06437, - 3.18374243622623 - ], - [ - -0.30091, - -0.06437, - 3.0994428709533564 - ], - [ - -0.35255, - -0.06001, - 3.0151433056804824 - ], - [ - -0.40364, - -0.05131, - 2.9308437404076084 - ], - [ - -0.45381, - -0.03835, - 2.8465441751347345 - ], - [ - -0.50271, - -0.0212, - 2.762244609861861 - ], - [ - -0.55, - 0.0, - 2.677945044588987 - ] - ] - }, - { - "trajectory_id": 49, - "start_angle_index": 9, - "end_angle_index": 8, - "left_turn": false, - "trajectory_radius": 0.63541, - "trajectory_length": 0.36821, - "arc_length": 0.29461, - "straight_length": 0.07361, - "poses": [ - [ - -0.04705, - -0.02352, - 3.6052402625905993 - ], - [ - -0.09444, - -0.04634, - 3.555513389790695 - ], - [ - -0.14342, - -0.06548, - 3.472729242550515 - ], - [ - -0.19381, - -0.0805, - 3.3899450953103343 - ], - [ - -0.24528, - -0.09131, - 3.307160948070154 - ], - [ - -0.29746, - -0.09782, - 3.2243768008299734 - ], - [ - -0.35, - -0.1, - 3.141592653589793 - ] - ] - }, - { - "trajectory_id": 50, - "start_angle_index": 10, - "end_angle_index": 9, - "left_turn": false, - "trajectory_radius": 0.68895, - "trajectory_length": 0.25129, - "arc_length": 0.22167, - "straight_length": 0.02962, - "poses": [ - [ - -0.03576, - -0.03531, - 3.8970366491949986 - ], - [ - -0.07357, - -0.06841, - 3.8240875525438986 - ], - [ - -0.11369, - -0.09866, - 3.751138455892799 - ], - [ - -0.15591, - -0.1259, - 3.678189359241699 - ], - [ - -0.2, - -0.15, - 3.6052402625905993 - ] - ] - }, - { - "trajectory_id": 51, - "start_angle_index": 10, - "end_angle_index": 10, - "left_turn": true, - "trajectory_radius": 0.0, - "trajectory_length": 0.21213, - "arc_length": 0.0, - "straight_length": 0.21213, - "poses": [ - [ - -0.0375, - -0.0375, - 3.9269908169872414 - ], - [ - -0.075, - -0.075, - 3.9269908169872414 - ], - [ - -0.1125, - -0.1125, - 3.9269908169872414 - ], - [ - -0.15, - -0.15, - 3.9269908169872414 - ] - ] - }, - { - "trajectory_id": 52, - "start_angle_index": 10, - "end_angle_index": 11, - "left_turn": true, - "trajectory_radius": 0.68895, - "trajectory_length": 0.25129, - "arc_length": 0.22167, - "straight_length": 0.02962, - "poses": [ - [ - -0.03531, - -0.03576, - 3.956944984779484 - ], - [ - -0.06841, - -0.07357, - 4.029894081430584 - ], - [ - -0.09866, - -0.11369, - 4.102843178081684 - ], - [ - -0.1259, - -0.15591, - 4.175792274732784 - ], - [ - -0.15, - -0.2, - 4.2487413713838835 - ] - ] - }, - { - "trajectory_id": 53, - "start_angle_index": 10, - "end_angle_index": 12, - "left_turn": true, - "trajectory_radius": 0.51213, - "trajectory_length": 0.4401, - "arc_length": 0.40223, - "straight_length": 0.03787, - "poses": [ - [ - -0.03288, - -0.03617, - 4.022473509407077 - ], - [ - -0.06215, - -0.07532, - 4.117956201826914 - ], - [ - -0.08756, - -0.11707, - 4.2134388942467496 - ], - [ - -0.10888, - -0.16106, - 4.308921586666585 - ], - [ - -0.1259, - -0.20688, - 4.404404279086421 - ], - [ - -0.13848, - -0.25412, - 4.499886971506257 - ], - [ - -0.1465, - -0.30234, - 4.595369663926093 - ], - [ - -0.14988, - -0.3511, - 4.690852356345929 - ], - [ - -0.15, - -0.4, - 4.71238898038469 - ] - ] - }, - { - "trajectory_id": 54, - "start_angle_index": 10, - "end_angle_index": 8, - "left_turn": false, - "trajectory_radius": 0.51213, - "trajectory_length": 0.4401, - "arc_length": 0.40223, - "straight_length": 0.03787, - "poses": [ - [ - -0.03617, - -0.03288, - 3.8315081245674056 - ], - [ - -0.07532, - -0.06215, - 3.7360254321475694 - ], - [ - -0.11707, - -0.08756, - 3.6405427397277337 - ], - [ - -0.16106, - -0.10888, - 3.5450600473078975 - ], - [ - -0.20688, - -0.1259, - 3.4495773548880617 - ], - [ - -0.25412, - -0.13848, - 3.354094662468226 - ], - [ - -0.30234, - -0.1465, - 3.2586119700483898 - ], - [ - -0.3511, - -0.14988, - 3.1631292776285536 - ], - [ - -0.4, - -0.15, - 3.141592653589793 - ] - ] - }, - { - "trajectory_id": 55, - "start_angle_index": 11, - "end_angle_index": 10, - "left_turn": false, - "trajectory_radius": 0.68895, - "trajectory_length": 0.25129, - "arc_length": 0.22167, - "straight_length": 0.02962, - "poses": [ - [ - -0.0241, - -0.04409, - 4.175792274732784 - ], - [ - -0.05134, - -0.08631, - 4.102843178081684 - ], - [ - -0.08159, - -0.12643, - 4.029894081430584 - ], - [ - -0.11469, - -0.16424, - 3.956944984779484 - ], - [ - -0.15, - -0.2, - 3.9269908169872414 - ] - ] - }, - { - "trajectory_id": 56, - "start_angle_index": 11, - "end_angle_index": 11, - "left_turn": true, - "trajectory_radius": 0.0, - "trajectory_length": 0.22361, - "arc_length": 0.0, - "straight_length": 0.22361, - "poses": [ - [ - -0.025, - -0.05, - 4.2487413713838835 - ], - [ - -0.05, - -0.1, - 4.2487413713838835 - ], - [ - -0.075, - -0.15, - 4.2487413713838835 - ], - [ - -0.1, - -0.2, - 4.2487413713838835 - ] - ] - }, - { - "trajectory_id": 57, - "start_angle_index": 11, - "end_angle_index": 12, - "left_turn": true, - "trajectory_radius": 0.63541, - "trajectory_length": 0.36821, - "arc_length": 0.29461, - "straight_length": 0.07361, - "poses": [ - [ - -0.02352, - -0.04705, - 4.2487413713838835 - ], - [ - -0.04634, - -0.09444, - 4.298468244183788 - ], - [ - -0.06548, - -0.14342, - 4.381252391423968 - ], - [ - -0.0805, - -0.19381, - 4.464036538664148 - ], - [ - -0.09131, - -0.24528, - 4.546820685904329 - ], - [ - -0.09782, - -0.29746, - 4.629604833144509 - ], - [ - -0.1, - -0.35, - 4.71238898038469 - ] - ] - }, - { - "trajectory_id": 58, - "start_angle_index": 11, - "end_angle_index": 13, - "left_turn": true, - "trajectory_radius": 0.50312, - "trajectory_length": 0.46654, - "arc_length": 0.46654, - "straight_length": 0.0, - "poses": [ - [ - -0.02076, - -0.04747, - 4.3517741733840625 - ], - [ - -0.03652, - -0.09683, - 4.454806975384242 - ], - [ - -0.04712, - -0.14755, - 4.557839777384421 - ], - [ - -0.05245, - -0.19909, - 4.6608725793846 - ], - [ - -0.05245, - -0.25091, - 4.763905381384779 - ], - [ - -0.04712, - -0.30245, - 4.866938183384958 - ], - [ - -0.03652, - -0.35317, - 4.969970985385137 - ], - [ - -0.02076, - -0.40253, - 5.073003787385317 - ], - [ - 0.0, - -0.45, - 5.176036589385496 - ] - ] - }, - { - "trajectory_id": 59, - "start_angle_index": 11, - "end_angle_index": 13, - "left_turn": true, - "trajectory_radius": 0.61492, - "trajectory_length": 0.57021, - "arc_length": 0.57021, - "straight_length": 0.0, - "poses": [ - [ - -0.0212, - -0.04729, - 4.333040936656757 - ], - [ - -0.03835, - -0.09619, - 4.4173405019296315 - ], - [ - -0.05131, - -0.14636, - 4.501640067202505 - ], - [ - -0.06001, - -0.19745, - 4.5859396324753785 - ], - [ - -0.06437, - -0.24909, - 4.670239197748253 - ], - [ - -0.06437, - -0.30091, - 4.754538763021126 - ], - [ - -0.06001, - -0.35255, - 4.838838328294001 - ], - [ - -0.05131, - -0.40364, - 4.923137893566874 - ], - [ - -0.03835, - -0.45381, - 5.007437458839748 - ], - [ - -0.0212, - -0.50271, - 5.091737024112621 - ], - [ - 0.0, - -0.55, - 5.176036589385496 - ] - ] - }, - { - "trajectory_id": 60, - "start_angle_index": 12, - "end_angle_index": 9, - "left_turn": false, - "trajectory_radius": 0.52586, - "trajectory_length": 0.64852, - "arc_length": 0.58221, - "straight_length": 0.06631, - "poses": [ - [ - -0.00236, - -0.04981, - 4.617523555424893 - ], - [ - -0.00944, - -0.09917, - 4.522658130465096 - ], - [ - -0.02115, - -0.14765, - 4.427792705505299 - ], - [ - -0.03741, - -0.19479, - 4.332927280545503 - ], - [ - -0.05805, - -0.24018, - 4.2380618555857055 - ], - [ - -0.08291, - -0.28341, - 4.143196430625909 - ], - [ - -0.11175, - -0.3241, - 4.048331005666112 - ], - [ - -0.14431, - -0.36187, - 3.9534655807063155 - ], - [ - -0.1803, - -0.39638, - 3.858600155746519 - ], - [ - -0.2194, - -0.42733, - 3.763734730786722 - ], - [ - -0.26126, - -0.45444, - 3.6688693058269255 - ], - [ - -0.30538, - -0.47769, - 3.6052402625905993 - ], - [ - -0.35, - -0.5, - 3.6052402625905993 - ] - ] - }, - { - "trajectory_id": 61, - "start_angle_index": 12, - "end_angle_index": 11, - "left_turn": false, - "trajectory_radius": 0.63541, - "trajectory_length": 0.36821, - "arc_length": 0.29461, - "straight_length": 0.07361, - "poses": [ - [ - -0.00218, - -0.05254, - 4.629604833144509 - ], - [ - -0.00869, - -0.10472, - 4.546820685904329 - ], - [ - -0.0195, - -0.15619, - 4.464036538664148 - ], - [ - -0.03452, - -0.20658, - 4.381252391423968 - ], - [ - -0.05366, - -0.25556, - 4.298468244183788 - ], - [ - -0.07648, - -0.30295, - 4.2487413713838835 - ], - [ - -0.1, - -0.35, - 4.2487413713838835 - ] - ] - }, - { - "trajectory_id": 62, - "start_angle_index": 12, - "end_angle_index": 12, - "left_turn": true, - "trajectory_radius": 0.0, - "trajectory_length": 0.15, - "arc_length": 0.0, - "straight_length": 0.15, - "poses": [ - [ - 0.0, - -0.05, - 4.71238898038469 - ], - [ - 0.0, - -0.1, - 4.71238898038469 - ], - [ - 0.0, - -0.15, - 4.71238898038469 - ] - ] - }, - { - "trajectory_id": 63, - "start_angle_index": 12, - "end_angle_index": 13, - "left_turn": true, - "trajectory_radius": 0.63541, - "trajectory_length": 0.36821, - "arc_length": 0.29461, - "straight_length": 0.07361, - "poses": [ - [ - 0.00218, - -0.05254, - 4.79517312762487 - ], - [ - 0.00869, - -0.10472, - 4.87795727486505 - ], - [ - 0.0195, - -0.15619, - 4.960741422105231 - ], - [ - 0.03452, - -0.20658, - 5.0435255693454115 - ], - [ - 0.05366, - -0.25556, - 5.126309716585592 - ], - [ - 0.07648, - -0.30295, - 5.176036589385496 - ], - [ - 0.1, - -0.35, - 5.176036589385496 - ] - ] - }, - { - "trajectory_id": 64, - "start_angle_index": 12, - "end_angle_index": 15, - "left_turn": true, - "trajectory_radius": 0.52586, - "trajectory_length": 0.64852, - "arc_length": 0.58221, - "straight_length": 0.06631, - "poses": [ - [ - 0.00236, - -0.04981, - 4.807254405344486 - ], - [ - 0.00944, - -0.09917, - 4.902119830304283 - ], - [ - 0.02115, - -0.14765, - 4.9969852552640805 - ], - [ - 0.03741, - -0.19479, - 5.091850680223876 - ], - [ - 0.05805, - -0.24018, - 5.186716105183674 - ], - [ - 0.08291, - -0.28341, - 5.2815815301434705 - ], - [ - 0.11175, - -0.3241, - 5.376446955103267 - ], - [ - 0.14431, - -0.36187, - 5.471312380063064 - ], - [ - 0.1803, - -0.39638, - 5.5661778050228605 - ], - [ - 0.2194, - -0.42733, - 5.661043229982657 - ], - [ - 0.26126, - -0.45444, - 5.755908654942454 - ], - [ - 0.30538, - -0.47769, - 5.81953769817878 - ], - [ - 0.35, - -0.5, - 5.81953769817878 - ] - ] - }, - { - "trajectory_id": 65, - "start_angle_index": 13, - "end_angle_index": 11, - "left_turn": false, - "trajectory_radius": 0.50312, - "trajectory_length": 0.46654, - "arc_length": 0.46654, - "straight_length": 0.0, - "poses": [ - [ - 0.02076, - -0.04747, - 5.073003787385317 - ], - [ - 0.03652, - -0.09683, - 4.969970985385137 - ], - [ - 0.04712, - -0.14755, - 4.866938183384958 - ], - [ - 0.05245, - -0.19909, - 4.763905381384779 - ], - [ - 0.05245, - -0.25091, - 4.6608725793846 - ], - [ - 0.04712, - -0.30245, - 4.557839777384421 - ], - [ - 0.03652, - -0.35317, - 4.454806975384242 - ], - [ - 0.02076, - -0.40253, - 4.3517741733840625 - ], - [ - 0.0, - -0.45, - 4.2487413713838835 - ] - ] - }, - { - "trajectory_id": 66, - "start_angle_index": 13, - "end_angle_index": 11, - "left_turn": false, - "trajectory_radius": 0.61492, - "trajectory_length": 0.57021, - "arc_length": 0.57021, - "straight_length": 0.0, - "poses": [ - [ - 0.0212, - -0.04729, - 5.091737024112622 - ], - [ - 0.03835, - -0.09619, - 5.007437458839748 - ], - [ - 0.05131, - -0.14636, - 4.923137893566874 - ], - [ - 0.06001, - -0.19745, - 4.838838328294001 - ], - [ - 0.06437, - -0.24909, - 4.754538763021126 - ], - [ - 0.06437, - -0.30091, - 4.670239197748253 - ], - [ - 0.06001, - -0.35255, - 4.5859396324753785 - ], - [ - 0.05131, - -0.40364, - 4.501640067202505 - ], - [ - 0.03835, - -0.45381, - 4.4173405019296315 - ], - [ - 0.0212, - -0.50271, - 4.333040936656758 - ], - [ - 0.0, - -0.55, - 4.2487413713838835 - ] - ] - }, - { - "trajectory_id": 67, - "start_angle_index": 13, - "end_angle_index": 12, - "left_turn": false, - "trajectory_radius": 0.63541, - "trajectory_length": 0.36821, - "arc_length": 0.29461, - "straight_length": 0.07361, - "poses": [ - [ - 0.02352, - -0.04705, - 5.176036589385496 - ], - [ - 0.04634, - -0.09444, - 5.126309716585592 - ], - [ - 0.06548, - -0.14342, - 5.0435255693454115 - ], - [ - 0.0805, - -0.19381, - 4.960741422105231 - ], - [ - 0.09131, - -0.24528, - 4.87795727486505 - ], - [ - 0.09782, - -0.29746, - 4.79517312762487 - ], - [ - 0.1, - -0.35, - 4.71238898038469 - ] - ] - }, - { - "trajectory_id": 68, - "start_angle_index": 13, - "end_angle_index": 13, - "left_turn": true, - "trajectory_radius": 0.0, - "trajectory_length": 0.22361, - "arc_length": 0.0, - "straight_length": 0.22361, - "poses": [ - [ - 0.025, - -0.05, - 5.176036589385496 - ], - [ - 0.05, - -0.1, - 5.176036589385496 - ], - [ - 0.075, - -0.15, - 5.176036589385496 - ], - [ - 0.1, - -0.2, - 5.176036589385496 - ] - ] - }, - { - "trajectory_id": 69, - "start_angle_index": 13, - "end_angle_index": 14, - "left_turn": true, - "trajectory_radius": 0.68895, - "trajectory_length": 0.25129, - "arc_length": 0.22167, - "straight_length": 0.02962, - "poses": [ - [ - 0.0241, - -0.04409, - 5.248985686036596 - ], - [ - 0.05134, - -0.08631, - 5.321934782687696 - ], - [ - 0.08159, - -0.12643, - 5.394883879338796 - ], - [ - 0.11469, - -0.16424, - 5.467832975989895 - ], - [ - 0.15, - -0.2, - 5.497787143782138 - ] - ] - }, - { - "trajectory_id": 70, - "start_angle_index": 14, - "end_angle_index": 12, - "left_turn": false, - "trajectory_radius": 0.51213, - "trajectory_length": 0.4401, - "arc_length": 0.40223, - "straight_length": 0.03787, - "poses": [ - [ - 0.03288, - -0.03617, - 5.402304451362302 - ], - [ - 0.06215, - -0.07532, - 5.306821758942466 - ], - [ - 0.08756, - -0.11707, - 5.21133906652263 - ], - [ - 0.10888, - -0.16106, - 5.115856374102794 - ], - [ - 0.1259, - -0.20688, - 5.020373681682958 - ], - [ - 0.13848, - -0.25412, - 4.9248909892631225 - ], - [ - 0.1465, - -0.30234, - 4.829408296843287 - ], - [ - 0.14988, - -0.3511, - 4.73392560442345 - ], - [ - 0.15, - -0.4, - 4.71238898038469 - ] - ] - }, - { - "trajectory_id": 71, - "start_angle_index": 14, - "end_angle_index": 13, - "left_turn": false, - "trajectory_radius": 0.68895, - "trajectory_length": 0.25129, - "arc_length": 0.22167, - "straight_length": 0.02962, - "poses": [ - [ - 0.03531, - -0.03576, - 5.467832975989895 - ], - [ - 0.06841, - -0.07357, - 5.394883879338796 - ], - [ - 0.09866, - -0.11369, - 5.321934782687696 - ], - [ - 0.1259, - -0.15591, - 5.248985686036596 - ], - [ - 0.15, - -0.2, - 5.176036589385496 - ] - ] - }, - { - "trajectory_id": 72, - "start_angle_index": 14, - "end_angle_index": 14, - "left_turn": true, - "trajectory_radius": 0.0, - "trajectory_length": 0.21213, - "arc_length": 0.0, - "straight_length": 0.21213, - "poses": [ - [ - 0.0375, - -0.0375, - 5.497787143782138 - ], - [ - 0.075, - -0.075, - 5.497787143782138 - ], - [ - 0.1125, - -0.1125, - 5.497787143782138 - ], - [ - 0.15, - -0.15, - 5.497787143782138 - ] - ] - }, - { - "trajectory_id": 73, - "start_angle_index": 14, - "end_angle_index": 15, - "left_turn": true, - "trajectory_radius": 0.68895, - "trajectory_length": 0.25129, - "arc_length": 0.22167, - "straight_length": 0.02962, - "poses": [ - [ - 0.03576, - -0.03531, - 5.527741311574381 - ], - [ - 0.07357, - -0.06841, - 5.60069040822548 - ], - [ - 0.11369, - -0.09866, - 5.67363950487658 - ], - [ - 0.15591, - -0.1259, - 5.74658860152768 - ], - [ - 0.2, - -0.15, - 5.81953769817878 - ] - ] - }, - { - "trajectory_id": 74, - "start_angle_index": 14, - "end_angle_index": 0, - "left_turn": true, - "trajectory_radius": 0.51213, - "trajectory_length": 0.4401, - "arc_length": 0.40223, - "straight_length": 0.03787, - "poses": [ - [ - 0.03617, - -0.03288, - 5.593269836201974 - ], - [ - 0.07532, - -0.06215, - 5.6887525286218095 - ], - [ - 0.11707, - -0.08756, - 5.784235221041646 - ], - [ - 0.16106, - -0.10888, - 5.879717913461482 - ], - [ - 0.20688, - -0.1259, - 5.975200605881318 - ], - [ - 0.25412, - -0.13848, - 6.070683298301153 - ], - [ - 0.30234, - -0.1465, - 6.166165990720989 - ], - [ - 0.3511, - -0.14988, - 6.261648683140826 - ], - [ - 0.4, - -0.15, - 0.0 - ] - ] - }, - { - "trajectory_id": 75, - "start_angle_index": 15, - "end_angle_index": 14, - "left_turn": false, - "trajectory_radius": 0.68895, - "trajectory_length": 0.25129, - "arc_length": 0.22167, - "straight_length": 0.02962, - "poses": [ - [ - 0.04409, - -0.0241, - 5.74658860152768 - ], - [ - 0.08631, - -0.05134, - 5.67363950487658 - ], - [ - 0.12643, - -0.08159, - 5.60069040822548 - ], - [ - 0.16424, - -0.11469, - 5.527741311574381 - ], - [ - 0.2, - -0.15, - 5.497787143782138 - ] - ] - }, - { - "trajectory_id": 76, - "start_angle_index": 15, - "end_angle_index": 15, - "left_turn": true, - "trajectory_radius": 0.0, - "trajectory_length": 0.22361, - "arc_length": 0.0, - "straight_length": 0.22361, - "poses": [ - [ - 0.05, - -0.025, - 5.81953769817878 - ], - [ - 0.1, - -0.05, - 5.81953769817878 - ], - [ - 0.15, - -0.075, - 5.81953769817878 - ], - [ - 0.2, - -0.1, - 5.81953769817878 - ] - ] - }, - { - "trajectory_id": 77, - "start_angle_index": 15, - "end_angle_index": 0, - "left_turn": true, - "trajectory_radius": 0.63541, - "trajectory_length": 0.36821, - "arc_length": 0.29461, - "straight_length": 0.07361, - "poses": [ - [ - 0.04705, - -0.02352, - 5.81953769817878 - ], - [ - 0.09444, - -0.04634, - 5.869264570978684 - ], - [ - 0.14342, - -0.06548, - 5.952048718218864 - ], - [ - 0.19381, - -0.0805, - 6.034832865459045 - ], - [ - 0.24528, - -0.09131, - 6.117617012699226 - ], - [ - 0.29746, - -0.09782, - 6.200401159939406 - ], - [ - 0.35, - -0.1, - 0.0 - ] - ] - }, - { - "trajectory_id": 78, - "start_angle_index": 15, - "end_angle_index": 1, - "left_turn": true, - "trajectory_radius": 0.50312, - "trajectory_length": 0.46654, - "arc_length": 0.46654, - "straight_length": 0.0, - "poses": [ - [ - 0.04747, - -0.02076, - 5.922570500178959 - ], - [ - 0.09683, - -0.03652, - 6.025603302179139 - ], - [ - 0.14755, - -0.04712, - 6.128636104179318 - ], - [ - 0.19909, - -0.05245, - 6.231668906179497 - ], - [ - 0.25091, - -0.05245, - 0.05151640100008953 - ], - [ - 0.30245, - -0.04712, - 0.1545492030002687 - ], - [ - 0.35317, - -0.03652, - 0.25758200500044787 - ], - [ - 0.40253, - -0.02076, - 0.36061480700062715 - ], - [ - 0.45, - 0.0, - 0.4636476090008061 - ] - ] - }, - { - "trajectory_id": 79, - "start_angle_index": 15, - "end_angle_index": 1, - "left_turn": true, - "trajectory_radius": 0.61492, - "trajectory_length": 0.57021, - "arc_length": 0.57021, - "straight_length": 0.0, - "poses": [ - [ - 0.04729, - -0.0212, - 5.903837263451654 - ], - [ - 0.09619, - -0.03835, - 5.988136828724528 - ], - [ - 0.14636, - -0.05131, - 6.0724363939974015 - ], - [ - 0.19745, - -0.06001, - 6.156735959270275 - ], - [ - 0.24909, - -0.06437, - 6.2410355245431495 - ], - [ - 0.30091, - -0.06437, - 0.04214978263643693 - ], - [ - 0.35255, - -0.06001, - 0.1264493479093109 - ], - [ - 0.40364, - -0.05131, - 0.21074891318218475 - ], - [ - 0.45381, - -0.03835, - 0.2950484784550586 - ], - [ - 0.50271, - -0.0212, - 0.37934804372793235 - ], - [ - 0.55, - 0.0, - 0.4636476090008061 - ] - ] - } - ] -} diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index 2762ae37c8a..52b94db14af 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -43,8 +43,9 @@ AStarAlgorithm::AStarAlgorithm( _x_size(0), _y_size(0), _search_info(search_info), + _goal_coordinates(Coordinates()), _start(nullptr), - _goal_manager(GoalManagerT()), + _goal(nullptr), _motion_model(motion_model) { _graph.reserve(100000); @@ -191,102 +192,35 @@ template<> void AStarAlgorithm::setGoal( const float & mx, const float & my, - const unsigned int & dim_3, - const GoalHeadingMode & /*goal_heading_mode*/, - const int & /*coarse_search_resolution*/) + const unsigned int & dim_3) { if (dim_3 != 0) { throw std::runtime_error("Node type Node2D cannot be given non-zero goal dim 3."); } - NodePtr goal = addToGraph( + _goal = addToGraph( Node2D::getIndex( static_cast(mx), static_cast(my), getSizeX())); - - goal->setPose(Node2D::Coordinates(mx, my)); - GoalStateVector goals_state; - goals_state.push_back({goal, true}); - _goal_manager.setGoalStates(goals_state); - _coarse_search_resolution = 1; + _goal_coordinates = Node2D::Coordinates(mx, my); } template void AStarAlgorithm::setGoal( const float & mx, const float & my, - const unsigned int & dim_3, - const GoalHeadingMode & goal_heading_mode, - const int & coarse_search_resolution) + const unsigned int & dim_3) { - // Default to minimal resolution unless overridden for ALL_DIRECTION - _coarse_search_resolution = 1; - - unsigned int num_bins = NodeT::motion_table.num_angle_quantization; - GoalStateVector goals_state; - - // set goal based on heading mode - switch (goal_heading_mode) { - case GoalHeadingMode::DEFAULT: { - // add a single goal node with single heading - auto goal = addToGraph(NodeT::getIndex(mx, my, dim_3)); - goal->setPose(typename NodeT::Coordinates( - static_cast(mx), static_cast(my), static_cast(dim_3))); - goals_state.push_back({goal, true}); - break; - } - - case GoalHeadingMode::BIDIRECTIONAL: { - // Add two goals, one for each direction - // add goal in original direction - auto goal = addToGraph(NodeT::getIndex(mx, my, dim_3)); - goal->setPose(typename NodeT::Coordinates( - static_cast(mx), static_cast(my), static_cast(dim_3))); - goals_state.push_back({goal, true}); - - // Add goal node in opposite (180°) direction - unsigned int opposite_heading = (dim_3 + (num_bins / 2)) % num_bins; - auto opposite_goal = addToGraph(NodeT::getIndex(mx, my, opposite_heading)); - opposite_goal->setPose(typename NodeT::Coordinates( - static_cast(mx), static_cast(my), static_cast(opposite_heading))); - goals_state.push_back({opposite_goal, true}); - break; - } - - case GoalHeadingMode::ALL_DIRECTION: { - // Set the coarse search resolution only for all direction - _coarse_search_resolution = coarse_search_resolution; - - // Add goal nodes for all headings - for (unsigned int i = 0; i < num_bins; ++i) { - auto goal = addToGraph(NodeT::getIndex(mx, my, i)); - goal->setPose(typename NodeT::Coordinates( - static_cast(mx), static_cast(my), static_cast(i))); - goals_state.push_back({goal, true}); - } - break; - } - case GoalHeadingMode::UNKNOWN: - throw std::runtime_error("Goal heading is UNKNOWN."); - } - - // check if we need to reset the obstacle heuristic, we only need to - // check that the x and y component has changed - const auto & previous_goals = _goal_manager.getGoalsState(); - - // Lambda to check if goal has changed - auto goalHasChanged = [&]() -> bool { - if (previous_goals.empty()) { - return true; - } + _goal = addToGraph( + NodeT::getIndex( + static_cast(mx), + static_cast(my), + dim_3)); - const auto & prev = previous_goals.front().goal; - const auto & curr = goals_state.front().goal; - return (prev->pose.x != curr->pose.x) || (prev->pose.y != curr->pose.y); - }; + typename NodeT::Coordinates goal_coords(mx, my, dim_3); - if (!_search_info.cache_obstacle_heuristic || goalHasChanged()) { + if (!_search_info.cache_obstacle_heuristic || goal_coords != _goal_coordinates) { if (!_start) { throw std::runtime_error("Start must be set before goal."); } @@ -295,8 +229,8 @@ void AStarAlgorithm::setGoal( _collision_checker->getCostmapROS(), _start->pose.x, _start->pose.y, mx, my); } - // assign the goals state - _goal_manager.setGoalStates(goals_state); + _goal_coordinates = goal_coords; + _goal->setPose(_goal_coordinates); } template @@ -308,15 +242,14 @@ bool AStarAlgorithm::areInputsValid() } // Check if points were filled in - if (!_start || _goal_manager.goalsIsEmpty()) { + if (!_start || !_goal) { throw std::runtime_error("Failed to compute path, no valid start or goal given."); } - // remove invalid goals - _goal_manager.removeInvalidGoals(getToleranceHeuristic(), _collision_checker, _traverse_unknown); - // Check if ending point is valid - if (_goal_manager.getGoalsSet().empty()) { + if (getToleranceHeuristic() < 0.001 && + !_goal->isNodeValid(_traverse_unknown, _collision_checker)) + { throw nav2_core::GoalOccupied("Goal was in lethal cost"); } @@ -340,10 +273,6 @@ bool AStarAlgorithm::createPath( return false; } - NodeVector coarse_check_goals, fine_check_goals; - _goal_manager.prepareGoalsForAnalyticExpansion(coarse_check_goals, fine_check_goals, - _coarse_search_resolution); - // 0) Add starting point to the open set addNode(0.0, getStart()); getStart()->setAccumulatedCost(0.0); @@ -410,14 +339,13 @@ bool AStarAlgorithm::createPath( // 2.1) Use an analytic expansion (if available) to generate a path expansion_result = nullptr; expansion_result = _expander->tryAnalyticExpansion( - current_node, coarse_check_goals, fine_check_goals, - _goal_manager.getGoalsCoordinates(), neighborGetter, analytic_iterations, closest_distance); + current_node, getGoal(), neighborGetter, analytic_iterations, closest_distance); if (expansion_result != nullptr) { current_node = expansion_result; } // 3) Check if we're at the goal, backtrace if required - if (_goal_manager.isGoal(current_node)) { + if (isGoal(current_node)) { return current_node->backtracePath(path); } else if (_best_heuristic_node.first < getToleranceHeuristic()) { // Optimization: Let us find when in tolerance and refine within reason @@ -458,12 +386,24 @@ bool AStarAlgorithm::createPath( return false; } +template +bool AStarAlgorithm::isGoal(NodePtr & node) +{ + return node == getGoal(); +} + template typename AStarAlgorithm::NodePtr & AStarAlgorithm::getStart() { return _start; } +template +typename AStarAlgorithm::NodePtr & AStarAlgorithm::getGoal() +{ + return _goal; +} + template typename AStarAlgorithm::NodePtr AStarAlgorithm::getNextNode() { @@ -486,7 +426,9 @@ float AStarAlgorithm::getHeuristicCost(const NodePtr & node) { const Coordinates node_coords = NodeT::getCoords(node->getIndex(), getSizeX(), getSizeDim3()); - float heuristic = NodeT::getHeuristicCost(node_coords, _goal_manager.getGoalsCoordinates()); + float heuristic = NodeT::getHeuristicCost( + node_coords, _goal_coordinates); + if (heuristic < _best_heuristic_node.first) { _best_heuristic_node = {heuristic, node->getIndex()}; } @@ -551,18 +493,6 @@ unsigned int & AStarAlgorithm::getSizeDim3() return _dim3_size; } -template -unsigned int AStarAlgorithm::getCoarseSearchResolution() -{ - return _coarse_search_resolution; -} - -template -typename AStarAlgorithm::GoalManagerT AStarAlgorithm::getGoalManager() -{ - return _goal_manager; -} - // Instantiate algorithm for the supported template types template class AStarAlgorithm; template class AStarAlgorithm; diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp index 13df5feb6af..ec0ccc31f02 100644 --- a/nav2_smac_planner/src/analytic_expansion.cpp +++ b/nav2_smac_planner/src/analytic_expansion.cpp @@ -12,6 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. Reserved. +#include +#include +#include + #include #include #include @@ -44,10 +48,7 @@ void AnalyticExpansion::setCollisionChecker( template typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalyticExpansion( - const NodePtr & current_node, - const NodeVector & coarse_check_goals, - const NodeVector & fine_check_goals, - const CoordinateVector & goals_coords, + const NodePtr & current_node, const NodePtr & goal_node, const NodeGetter & getter, int & analytic_iterations, int & closest_distance) { @@ -59,15 +60,10 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic const Coordinates node_coords = NodeT::getCoords( current_node->getIndex(), _collision_checker->getCostmap()->getSizeInCellsX(), _dim_3_size); - - AnalyticExpansionNodes current_best_analytic_nodes; - NodePtr current_best_goal = nullptr; - NodePtr current_best_node = nullptr; - float current_best_score = std::numeric_limits::max(); - closest_distance = std::min( closest_distance, - static_cast(NodeT::getHeuristicCost(node_coords, goals_coords))); + static_cast(NodeT::getHeuristicCost(node_coords, goal_node->pose))); + // We want to expand at a rate of d/expansion_ratio, // but check to see if we are so close that we would be expanding every iteration // If so, limit it to the expansion ratio (rounded up) @@ -84,57 +80,81 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic if (analytic_iterations <= 0) { // Reset the counter and try the analytic path expansion analytic_iterations = desired_iterations; - bool found_valid_expansion = false; - - // First check the coarse search resolution goals - for (auto & current_goal_node : coarse_check_goals) { - AnalyticExpansionNodes analytic_nodes = - getAnalyticPath( - current_node, current_goal_node, getter, - current_node->motion_table.state_space); - if (!analytic_nodes.nodes.empty()) { - found_valid_expansion = true; - NodePtr node = current_node; - float score = refineAnalyticPath( - node, current_goal_node, getter, analytic_nodes); - // Update the best score if we found a better path - if (score < current_best_score) { - current_best_analytic_nodes = analytic_nodes; - current_best_goal = current_goal_node; - current_best_score = score; - current_best_node = node; + AnalyticExpansionNodes analytic_nodes = + getAnalyticPath(current_node, goal_node, getter, current_node->motion_table.state_space); + if (!analytic_nodes.empty()) { + // If we have a valid path, attempt to refine it + NodePtr node = current_node; + NodePtr test_node = current_node; + AnalyticExpansionNodes refined_analytic_nodes; + for (int i = 0; i < 8; i++) { + // Attempt to create better paths in 5 node increments, need to make sure + // they exist for each in order to do so (maximum of 40 points back). + if (test_node->parent && test_node->parent->parent && test_node->parent->parent->parent && + test_node->parent->parent->parent->parent && + test_node->parent->parent->parent->parent->parent) + { + test_node = test_node->parent->parent->parent->parent->parent; + refined_analytic_nodes = + getAnalyticPath(test_node, goal_node, getter, test_node->motion_table.state_space); + if (refined_analytic_nodes.empty()) { + break; + } + analytic_nodes = refined_analytic_nodes; + node = test_node; + } else { + break; } } - } - // perform a final search if we found a goal - if (found_valid_expansion) { - for (auto & current_goal_node : fine_check_goals) { - AnalyticExpansionNodes analytic_nodes = - getAnalyticPath( - current_node, current_goal_node, getter, - current_node->motion_table.state_space); - if (!analytic_nodes.nodes.empty()) { - NodePtr node = current_node; - float score = refineAnalyticPath( - node, current_goal_node, getter, analytic_nodes); - // Update the best score if we found a better path - if (score < current_best_score) { - current_best_analytic_nodes = analytic_nodes; - current_best_goal = current_goal_node; - current_best_score = score; - current_best_node = node; + // The analytic expansion can short-cut near obstacles when closer to a goal + // So, we can attempt to refine it more by increasing the possible radius + // higher than the minimum turning radius and use the best solution based on + // a scoring function similar to that used in traversal cost estimation. + auto scoringFn = [&](const AnalyticExpansionNodes & expansion) { + if (expansion.size() < 2) { + return std::numeric_limits::max(); } + + float score = 0.0; + float normalized_cost = 0.0; + // Analytic expansions are consistently spaced + const float distance = hypotf( + expansion[1].proposed_coords.x - expansion[0].proposed_coords.x, + expansion[1].proposed_coords.y - expansion[0].proposed_coords.y); + const float & weight = expansion[0].node->motion_table.cost_penalty; + for (auto iter = expansion.begin(); iter != expansion.end(); ++iter) { + normalized_cost = iter->node->getCost() / 252.0f; + // Search's Traversal Cost Function + score += distance * (1.0 + weight * normalized_cost); + } + return score; + }; + + float best_score = scoringFn(analytic_nodes); + float score = std::numeric_limits::max(); + float min_turn_rad = node->motion_table.min_turning_radius; + const float max_min_turn_rad = 4.0 * min_turn_rad; // Up to 4x the turning radius + while (min_turn_rad < max_min_turn_rad) { + min_turn_rad += 0.5; // In Grid Coords, 1/2 cell steps + ompl::base::StateSpacePtr state_space; + if (node->motion_table.motion_model == MotionModel::DUBIN) { + state_space = std::make_shared(min_turn_rad); + } else { + state_space = std::make_shared(min_turn_rad); + } + refined_analytic_nodes = getAnalyticPath(node, goal_node, getter, state_space); + score = scoringFn(refined_analytic_nodes); + if (score <= best_score) { + analytic_nodes = refined_analytic_nodes; + best_score = score; } } + + return setAnalyticPath(node, goal_node, analytic_nodes); } } - if (!current_best_analytic_nodes.nodes.empty()) { - return setAnalyticPath( - current_best_node, current_best_goal, - current_best_analytic_nodes); - } analytic_iterations--; } @@ -142,28 +162,6 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic return NodePtr(nullptr); } -template -int AnalyticExpansion::countDirectionChanges( - const ompl::base::ReedsSheppStateSpace::ReedsSheppPath & path) -{ - const double * lengths = path.length_; - int changes = 0; - int last_dir = 0; - for (int i = 0; i < 5; ++i) { - if (lengths[i] == 0.0) { - continue; - } - - int currentDirection = (lengths[i] > 0.0) ? 1 : -1; - if (last_dir != 0 && currentDirection != last_dir) { - ++changes; - } - last_dir = currentDirection; - } - - return changes; -} - template typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansion::getAnalyticPath( const NodePtr & node, @@ -181,12 +179,6 @@ typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansiondistance(from(), to()); - auto rs_state_space = dynamic_cast(state_space.get()); - int direction_changes = 0; - if (rs_state_space) { - direction_changes = countDirectionChanges(rs_state_space->reedsShepp(from.get(), to.get())); - } - // A move of sqrt(2) is guaranteed to be in a new cell static const float sqrt_2 = sqrtf(2.0f); @@ -203,7 +195,7 @@ typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansion reals; double theta; @@ -238,7 +230,7 @@ typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansionsetPose(proposed_coordinates); if (next->isNodeValid(_traverse_unknown, _collision_checker) && next != prev) { // Save the node, and its previous coordinates in case we need to abort - possible_nodes.add(next, initial_node_coords, proposed_coordinates); + possible_nodes.emplace_back(next, initial_node_coords, proposed_coordinates); node_costs.emplace_back(next->getCost()); prev = next; } else { @@ -288,7 +280,7 @@ typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansionsetPose(node_pose.initial_coords); } @@ -297,109 +289,9 @@ typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansion -float AnalyticExpansion::refineAnalyticPath( - NodePtr & node, - const NodePtr & goal_node, - const NodeGetter & getter, - AnalyticExpansionNodes & analytic_nodes) -{ - NodePtr test_node = node; - AnalyticExpansionNodes refined_analytic_nodes; - for (int i = 0; i < 8; i++) { - // Attempt to create better paths in 5 node increments, need to make sure - // they exist for each in order to do so (maximum of 40 points back). - if (test_node->parent && test_node->parent->parent && - test_node->parent->parent->parent && - test_node->parent->parent->parent->parent && - test_node->parent->parent->parent->parent->parent) - { - test_node = test_node->parent->parent->parent->parent->parent; - // print the goals pose - refined_analytic_nodes = - getAnalyticPath( - test_node, goal_node, getter, - test_node->motion_table.state_space); - if (refined_analytic_nodes.nodes.empty()) { - break; - } - if (refined_analytic_nodes.direction_changes > analytic_nodes.direction_changes) { - // If the direction changes are worse, we don't want to use this path - continue; - } - analytic_nodes = refined_analytic_nodes; - node = test_node; - } else { - break; - } - } - - // The analytic expansion can short-cut near obstacles when closer to a goal - // So, we can attempt to refine it more by increasing the possible radius - // higher than the minimum turning radius and use the best solution based on - // a scoring function similar to that used in traversal cost estimation. - auto scoringFn = [&](const AnalyticExpansionNodes & expansion) { - if (expansion.nodes.size() < 2) { - return std::numeric_limits::max(); - } - - float score = 0.0; - float normalized_cost = 0.0; - // Analytic expansions are consistently spaced - const float distance = hypotf( - expansion.nodes[1].proposed_coords.x - expansion.nodes[0].proposed_coords.x, - expansion.nodes[1].proposed_coords.y - expansion.nodes[0].proposed_coords.y); - const float & weight = expansion.nodes[0].node->motion_table.cost_penalty; - for (auto iter = expansion.nodes.begin(); iter != expansion.nodes.end(); ++iter) { - normalized_cost = iter->node->getCost() / 252.0f; - // Search's Traversal Cost Function - score += distance * (1.0 + weight * normalized_cost); - } - return score; - }; - - float original_score = scoringFn(analytic_nodes); - float best_score = original_score; - float score = std::numeric_limits::max(); - float min_turn_rad = node->motion_table.min_turning_radius; - const float max_min_turn_rad = 4.0 * min_turn_rad; // Up to 4x the turning radius - while (min_turn_rad < max_min_turn_rad) { - min_turn_rad += 0.5; // In Grid Coords, 1/2 cell steps - ompl::base::StateSpacePtr state_space; - if (node->motion_table.motion_model == MotionModel::DUBIN) { - state_space = std::make_shared(min_turn_rad); - } else { - state_space = std::make_shared(min_turn_rad); - } - refined_analytic_nodes = getAnalyticPath(node, goal_node, getter, state_space); - score = scoringFn(refined_analytic_nodes); - - // Normal scoring: prioritize lower cost as long as not more directional changes - if (score <= best_score && - refined_analytic_nodes.direction_changes <= analytic_nodes.direction_changes) - { - analytic_nodes = refined_analytic_nodes; - best_score = score; - continue; - } - - // Special case: If we have a better score than original (only) and less directional changes - // the path quality is still better than the original and is less operationally complex - if (score <= original_score && - refined_analytic_nodes.direction_changes < analytic_nodes.direction_changes) - { - analytic_nodes = refined_analytic_nodes; - best_score = score; - } - } - - return best_score; -} - template typename AnalyticExpansion::NodePtr AnalyticExpansion::setAnalyticPath( const NodePtr & node, @@ -409,7 +301,7 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::setAnalytic _detached_nodes.clear(); // Legitimate final path - set the parent relationships, states, and poses NodePtr prev = node; - for (const auto & node_pose : expanded_nodes.nodes) { + for (const auto & node_pose : expanded_nodes) { auto n = node_pose.node; cleanNode(n); if (n->getIndex() != goal_node->getIndex()) { @@ -453,16 +345,6 @@ getAnalyticPath( return AnalyticExpansionNodes(); } -template<> -float AnalyticExpansion::refineAnalyticPath( - NodePtr &, - const NodePtr &, - const NodeGetter &, - AnalyticExpansionNodes &) -{ - return std::numeric_limits::max(); -} - template<> typename AnalyticExpansion::NodePtr AnalyticExpansion::setAnalyticPath( const NodePtr &, @@ -474,10 +356,7 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::setAnalyt template<> typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalyticExpansion( - const NodePtr &, - const NodeVector &, - const NodeVector &, - const CoordinateVector &, + const NodePtr &, const NodePtr &, const NodeGetter &, int &, int &) { diff --git a/nav2_smac_planner/src/node_2d.cpp b/nav2_smac_planner/src/node_2d.cpp index 11fdf8b3b41..bf64ae29e3e 100644 --- a/nav2_smac_planner/src/node_2d.cpp +++ b/nav2_smac_planner/src/node_2d.cpp @@ -85,12 +85,12 @@ float Node2D::getTraversalCost(const NodePtr & child) float Node2D::getHeuristicCost( const Coordinates & node_coords, - const CoordinateVector & goals_coords) + const Coordinates & goal_coordinates) { // Using Moore distance as it more accurately represents the distances // even a Van Neumann neighborhood robot can navigate. - auto dx = goals_coords[0].x - node_coords.x; - auto dy = goals_coords[0].y - node_coords.y; + auto dx = goal_coordinates.x - node_coords.x; + auto dy = goal_coordinates.y - node_coords.y; return std::sqrt(dx * dx + dy * dy); } diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp index bbbae87ae7c..9d46ab0f0ff 100644 --- a/nav2_smac_planner/src/node_hybrid.cpp +++ b/nav2_smac_planner/src/node_hybrid.cpp @@ -442,18 +442,12 @@ float NodeHybrid::getTraversalCost(const NodePtr & child) float NodeHybrid::getHeuristicCost( const Coordinates & node_coords, - const CoordinateVector & goals_coords) + const Coordinates & goal_coords) { - // obstacle heuristic does not depend on goal heading const float obstacle_heuristic = - getObstacleHeuristic(node_coords, goals_coords[0], motion_table.cost_penalty); - float distance_heuristic = std::numeric_limits::max(); - for (unsigned int i = 0; i < goals_coords.size(); i++) { - distance_heuristic = std::min( - distance_heuristic, - getDistanceHeuristic(node_coords, goals_coords[i], obstacle_heuristic)); - } - return std::max(obstacle_heuristic, distance_heuristic); + getObstacleHeuristic(node_coords, goal_coords, motion_table.cost_penalty); + const float dist_heuristic = getDistanceHeuristic(node_coords, goal_coords, obstacle_heuristic); + return std::max(obstacle_heuristic, dist_heuristic); } void NodeHybrid::initMotionModel( diff --git a/nav2_smac_planner/src/node_lattice.cpp b/nav2_smac_planner/src/node_lattice.cpp index d9a7c8ebfa5..fc03969e978 100644 --- a/nav2_smac_planner/src/node_lattice.cpp +++ b/nav2_smac_planner/src/node_lattice.cpp @@ -345,18 +345,13 @@ float NodeLattice::getTraversalCost(const NodePtr & child) float NodeLattice::getHeuristicCost( const Coordinates & node_coords, - const CoordinateVector & goals_coords) + const Coordinates & goal_coords) { // get obstacle heuristic value - // obstacle heuristic does not depend on goal heading const float obstacle_heuristic = getObstacleHeuristic( - node_coords, goals_coords[0], motion_table.cost_penalty); - float distance_heuristic = std::numeric_limits::max(); - for (unsigned int i = 0; i < goals_coords.size(); i++) { - distance_heuristic = std::min( - distance_heuristic, - getDistanceHeuristic(node_coords, goals_coords[i], obstacle_heuristic)); - } + node_coords, goal_coords, motion_table.cost_penalty); + const float distance_heuristic = + getDistanceHeuristic(node_coords, goal_coords, obstacle_heuristic); return std::max(obstacle_heuristic, distance_heuristic); } @@ -472,7 +467,7 @@ void NodeLattice::precomputeDistanceHeuristic( int dim_3_size_int = static_cast(dim_3_size); // Create a lookup table of Dubin/Reeds-Shepp distances in a window around the goal - // to help drive the search towards admissible approaches. Due to symmetries in the + // to help drive the search towards admissible approaches. Deu to symmetries in the // Heuristic space, we need to only store 2 of the 4 quadrants and simply mirror // around the X axis any relative node lookup. This reduces memory overhead and increases // the size of a window a platform can store in memory. diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index d6f4ec26fb8..f3ece68c565 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -171,24 +171,7 @@ void SmacPlannerHybrid::configure( nav2_util::declare_parameter_if_not_declared( node, "service_introspection_mode", rclcpp::ParameterValue("disabled")); - std::string goal_heading_type; - nav2_util::declare_parameter_if_not_declared( - node, name + ".goal_heading_mode", rclcpp::ParameterValue("DEFAULT")); - node->get_parameter(name + ".goal_heading_mode", goal_heading_type); - _goal_heading_mode = fromStringToGH(goal_heading_type); - - nav2_util::declare_parameter_if_not_declared( - node, name + ".coarse_search_resolution", rclcpp::ParameterValue(1)); - node->get_parameter(name + ".coarse_search_resolution", _coarse_search_resolution); - - if (_goal_heading_mode == GoalHeadingMode::UNKNOWN) { - std::string error_msg = "Unable to get GoalHeader type. Given '" + goal_heading_type + "' " - "Valid options are DEFAULT, BIDIRECTIONAL, ALL_DIRECTION. "; - throw nav2_core::PlannerException(error_msg); - } - _motion_model = fromString(_motion_model_for_search); - if (_motion_model == MotionModel::UNKNOWN) { RCLCPP_WARN( _logger, @@ -211,21 +194,6 @@ void SmacPlannerHybrid::configure( _max_iterations = std::numeric_limits::max(); } - if (_coarse_search_resolution <= 0) { - RCLCPP_WARN( - _logger, "coarse iteration resolution selected as <= 0, " - "disabling coarse iteration resolution search for goal heading" - ); - - _coarse_search_resolution = 1; - } - - if (_angle_quantizations % _coarse_search_resolution != 0) { - std::string error_msg = "coarse iteration should be an increment" - " of the number of angular bins configured"; - throw nav2_core::PlannerException(error_msg); - } - if (_minimum_turning_radius_global_coords < _costmap->getResolution() * _downsampling_factor) { RCLCPP_WARN( _logger, "Min turning radius cannot be less than the search grid cell resolution!"); @@ -445,8 +413,7 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( if (orientation_bin >= static_cast(_angle_quantizations)) { orientation_bin -= static_cast(_angle_quantizations); } - _a_star->setGoal(mx_goal, my_goal, static_cast(orientation_bin), - _goal_heading_mode, _coarse_search_resolution); + _a_star->setGoal(mx_goal, my_goal, static_cast(orientation_bin)); // Setup message nav_msgs::msg::Path plan; @@ -733,31 +700,6 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para int angle_quantizations = parameter.as_int(); _angle_bin_size = 2.0 * M_PI / angle_quantizations; _angle_quantizations = static_cast(angle_quantizations); - - if (_angle_quantizations % _coarse_search_resolution != 0) { - RCLCPP_WARN( - _logger, "coarse iteration should be an increment of the " - "number of angular bins configured. Disabling course research!" - ); - _coarse_search_resolution = 1; - } - } else if (param_name == _name + ".coarse_search_resolution") { - _coarse_search_resolution = parameter.as_int(); - if (_coarse_search_resolution <= 0) { - RCLCPP_WARN( - _logger, "coarse iteration resolution selected as <= 0. " - "Disabling course research!" - ); - _coarse_search_resolution = 1; - } - if (_angle_quantizations % _coarse_search_resolution != 0) { - RCLCPP_WARN( - _logger, - "coarse iteration should be an increment of the " - "number of angular bins configured. Disabling course research!" - ); - _coarse_search_resolution = 1; - } } } else if (param_type == ParameterType::PARAMETER_STRING) { if (param_name == _name + ".motion_model_for_search") { @@ -770,22 +712,6 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para "valid options are MOORE, VON_NEUMANN, DUBIN, REEDS_SHEPP.", _motion_model_for_search.c_str()); } - } else if (param_name == _name + ".goal_heading_mode") { - std::string goal_heading_type = parameter.as_string(); - GoalHeadingMode goal_heading_mode = fromStringToGH(goal_heading_type); - RCLCPP_INFO( - _logger, - "GoalHeadingMode type set to '%s'.", - goal_heading_type.c_str()); - if (goal_heading_mode == GoalHeadingMode::UNKNOWN) { - RCLCPP_WARN( - _logger, - "Unable to get GoalHeader type. Given '%s', " - "Valid options are DEFAULT, BIDIRECTIONAL, ALL_DIRECTION. ", - goal_heading_type.c_str()); - } else { - _goal_heading_mode = goal_heading_mode; - } } } } diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index 4968068e534..bd68ed8a3ed 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -140,22 +140,6 @@ void SmacPlannerLattice::configure( node, name + ".debug_visualizations", rclcpp::ParameterValue(false)); node->get_parameter(name + ".debug_visualizations", _debug_visualizations); - std::string goal_heading_type; - nav2_util::declare_parameter_if_not_declared( - node, name + ".goal_heading_mode", rclcpp::ParameterValue("DEFAULT")); - node->get_parameter(name + ".goal_heading_mode", goal_heading_type); - _goal_heading_mode = fromStringToGH(goal_heading_type); - - nav2_util::declare_parameter_if_not_declared( - node, name + ".coarse_search_resolution", rclcpp::ParameterValue(1)); - node->get_parameter(name + ".coarse_search_resolution", _coarse_search_resolution); - - if (_goal_heading_mode == GoalHeadingMode::UNKNOWN) { - std::string error_msg = "Unable to get GoalHeader type. Given '" + goal_heading_type + "' " - "Valid options are DEFAULT, BIDIRECTIONAL, ALL_DIRECTION. "; - throw nav2_core::PlannerException(error_msg); - } - _metadata = LatticeMotionTable::getLatticeMetadata(_search_info.lattice_filepath); _search_info.minimum_turning_radius = _metadata.min_turning_radius / (_costmap->getResolution()); @@ -175,20 +159,6 @@ void SmacPlannerLattice::configure( _max_iterations = std::numeric_limits::max(); } - if (_coarse_search_resolution <= 0) { - RCLCPP_WARN( - _logger, "coarse iteration resolution selected as <= 0, " - "disabling coarse iteration resolution search for goal heading" - ); - _coarse_search_resolution = 1; - } - - if (_metadata.number_of_headings % _coarse_search_resolution != 0) { - std::string error_msg = "coarse iteration should be an increment of" - " the number of angular bins configured"; - throw nav2_core::PlannerException(error_msg); - } - float lookup_table_dim = static_cast(_lookup_table_size) / static_cast(_costmap->getResolution()); @@ -355,8 +325,7 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( } _a_star->setGoal( mx_goal, my_goal, - NodeLattice::motion_table.getClosestAngularBin(tf2::getYaw(goal.pose.orientation)), - _goal_heading_mode, _coarse_search_resolution); + NodeLattice::motion_table.getClosestAngularBin(tf2::getYaw(goal.pose.orientation))); // Setup message nav_msgs::msg::Path plan; @@ -624,23 +593,6 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector par } else if (param_name == _name + ".terminal_checking_interval") { reinit_a_star = true; _terminal_checking_interval = parameter.as_int(); - } else if (param_name == _name + ".coarse_search_resolution") { - _coarse_search_resolution = parameter.as_int(); - if (_coarse_search_resolution <= 0) { - RCLCPP_WARN( - _logger, "coarse iteration resolution selected as <= 0. " - "Disabling course research!" - ); - _coarse_search_resolution = 1; - } - if (_metadata.number_of_headings % _coarse_search_resolution != 0) { - RCLCPP_WARN( - _logger, - "coarse iteration should be an increment of the number<" - " of angular bins configured. Disabling course research!" - ); - _coarse_search_resolution = 1; - } } } else if (param_type == ParameterType::PARAMETER_STRING) { if (param_name == _name + ".lattice_filepath") { @@ -652,29 +604,6 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector par _metadata = LatticeMotionTable::getLatticeMetadata(_search_info.lattice_filepath); _search_info.minimum_turning_radius = _metadata.min_turning_radius / (_costmap->getResolution()); - if (_metadata.number_of_headings % _coarse_search_resolution != 0) { - RCLCPP_WARN( - _logger, "coarse iteration should be an increment of the number " - "of angular bins configured. Disabling course research!" - ); - _coarse_search_resolution = 1; - } - } else if (param_name == _name + ".goal_heading_mode") { - std::string goal_heading_type = parameter.as_string(); - GoalHeadingMode goal_heading_mode = fromStringToGH(goal_heading_type); - RCLCPP_INFO( - _logger, - "GoalHeadingMode type set to '%s'.", - goal_heading_type.c_str()); - if (goal_heading_mode == GoalHeadingMode::UNKNOWN) { - RCLCPP_WARN( - _logger, - "Unable to get GoalHeader type. Given '%s', " - "Valid options are DEFAULT, BIDIRECTIONAL, ALL_DIRECTION. ", - goal_heading_type.c_str()); - } else { - _goal_heading_mode = goal_heading_mode; - } } } } diff --git a/nav2_smac_planner/test/CMakeLists.txt b/nav2_smac_planner/test/CMakeLists.txt index 24c86d9c24f..fc4abf20c75 100644 --- a/nav2_smac_planner/test/CMakeLists.txt +++ b/nav2_smac_planner/test/CMakeLists.txt @@ -135,13 +135,3 @@ target_link_libraries(test_lattice_node rclcpp::rclcpp rclcpp_lifecycle::rclcpp_lifecycle ) - - -# Test Goal Manager -ament_add_gtest(test_goal_manager test_goal_manager.cpp) -target_link_libraries(test_goal_manager - ${library_name} - nav2_costmap_2d::nav2_costmap_2d_core - rclcpp::rclcpp - rclcpp_lifecycle::rclcpp_lifecycle -) diff --git a/nav2_smac_planner/test/test_a_star.cpp b/nav2_smac_planner/test/test_a_star.cpp index 701e7ad01ac..df8223ef9b5 100644 --- a/nav2_smac_planner/test/test_a_star.cpp +++ b/nav2_smac_planner/test/test_a_star.cpp @@ -123,32 +123,13 @@ TEST(AStarTest, test_a_star_2d) } EXPECT_TRUE(a_star_2.getStart() != nullptr); - EXPECT_NE(a_star_2.getGoalManager().getGoalsState().size(), 0); + EXPECT_TRUE(a_star_2.getGoal() != nullptr); EXPECT_EQ(a_star_2.getSizeX(), 100u); EXPECT_EQ(a_star_2.getSizeY(), 100u); EXPECT_EQ(a_star_2.getSizeDim3(), 1u); EXPECT_EQ(a_star_2.getToleranceHeuristic(), 20.0); EXPECT_EQ(a_star_2.getOnApproachMaxIterations(), 10); - // test unused functions - nav2_smac_planner::AnalyticExpansion expander( - nav2_smac_planner::MotionModel::TWOD, info, false, 1); - - auto analytic_expansion_nodes = - nav2_smac_planner::AnalyticExpansion::AnalyticExpansionNodes(); - EXPECT_EQ(expander.setAnalyticPath(nullptr, nullptr, analytic_expansion_nodes), nullptr); - int dummy_int1 = 0; - int dummy_int2 = 0; - EXPECT_EQ(expander.tryAnalyticExpansion(nullptr, {}, {}, {}, - nullptr, dummy_int1, dummy_int2), nullptr); - - nav2_smac_planner::Node2D * start = nullptr; - EXPECT_EQ(expander.refineAnalyticPath(start, nullptr, nullptr, - analytic_expansion_nodes), std::numeric_limits::max()); - nav2_smac_planner::AnalyticExpansion::AnalyticExpansionNodes - expected_nodes = expander.getAnalyticPath(nullptr, nullptr, nullptr, nullptr); - EXPECT_EQ(expected_nodes.nodes.size(), 0); - delete costmapA; } @@ -410,17 +391,10 @@ TEST(AStarTest, test_se2_single_pose_path) }; // functional case testing a_star.setCollisionChecker(checker.get()); - nav2_smac_planner::NodeHybrid::CoordinateVector path; - - // test with no goals set nor start - EXPECT_THROW( - a_star.createPath( - path, num_it, tolerance, - dummy_cancel_checker), std::runtime_error); - a_star.setStart(10u, 10u, 0u); // Goal is one costmap cell away a_star.setGoal(12u, 10u, 0u); + nav2_smac_planner::NodeHybrid::CoordinateVector path; EXPECT_TRUE(a_star.createPath(path, num_it, tolerance, dummy_cancel_checker)); // Check that the path is length one @@ -432,102 +406,6 @@ TEST(AStarTest, test_se2_single_pose_path) nav2_smac_planner::NodeHybrid::destroyStaticAssets(); } -TEST(AStarTest, test_goal_heading_mode) -{ - auto lnode = std::make_shared("test"); - nav2_smac_planner::SearchInfo info; - info.change_penalty = 0.1; - info.non_straight_penalty = 1.1; - info.reverse_penalty = 2.0; - info.minimum_turning_radius = 8; // in grid coordinates - info.retrospective_penalty = 0.015; - info.analytic_expansion_max_length = 20.0; // in grid coordinates - info.analytic_expansion_ratio = 3.5; - unsigned int size_theta = 72; - info.cost_penalty = 1.7; - nav2_smac_planner::AStarAlgorithm a_star( - nav2_smac_planner::MotionModel::DUBIN, info); - int max_iterations = 10000; - float tolerance = 10.0; - int it_on_approach = 10; - int terminal_checking_interval = 5000; - double max_planning_time = 120.0; - int num_it = 0; - - // BIDIRECTIONAL goal heading mode - a_star.initialize( - false, max_iterations, it_on_approach, terminal_checking_interval, - max_planning_time, 401, size_theta); - - nav2_costmap_2d::Costmap2D * costmapA = - new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0); - - // Convert raw costmap into a costmap ros object - auto costmap_ros = std::make_shared(); - costmap_ros->on_configure(rclcpp_lifecycle::State()); - auto costmap = costmap_ros->getCostmap(); - *costmap = *costmapA; - - std::unique_ptr checker = - std::make_unique(costmap_ros, size_theta, lnode); - checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); - - a_star.setCollisionChecker(checker.get()); - - EXPECT_THROW( - a_star.setGoal( - 80u, 80u, 40u, - nav2_smac_planner::GoalHeadingMode::BIDIRECTIONAL), - std::runtime_error); - a_star.setStart(10u, 10u, 0u); - a_star.setGoal(80u, 80u, 40u, nav2_smac_planner::GoalHeadingMode::BIDIRECTIONAL); - nav2_smac_planner::NodeHybrid::CoordinateVector path; - std::unique_ptr>> expansions = nullptr; - expansions = std::make_unique>>(); - - auto dummy_cancel_checker = []() { - return false; - }; - EXPECT_TRUE(a_star.createPath(path, num_it, tolerance, dummy_cancel_checker, expansions.get())); - EXPECT_EQ(a_star.getGoalManager().getGoalsState().size(), 2); - EXPECT_EQ(a_star.getGoalManager().getGoalsState().size(), - a_star.getGoalManager().getGoalsCoordinates().size()); - - - // ALL_DIRECTION goal heading mode - unsigned int coarse_search_resolution = 16; - a_star.setCollisionChecker(checker.get()); - a_star.setStart(10u, 10u, 0u); - a_star.setGoal(80u, 80u, 40u, nav2_smac_planner::GoalHeadingMode::ALL_DIRECTION, - coarse_search_resolution); - EXPECT_TRUE(a_star.getCoarseSearchResolution() == coarse_search_resolution); - - unsigned int num_bins = nav2_smac_planner::NodeHybrid::motion_table.num_angle_quantization; - - // get number of valid goal states - unsigned int num_valid_goals = 0; - auto goals_state = a_star.getGoalManager().getGoalsState(); - for (unsigned int i = 0; i < goals_state.size(); i++) { - if(goals_state[i].is_valid) { - num_valid_goals++; - } - } - EXPECT_TRUE(a_star.getGoalManager().getGoalsState().size() == num_bins); - EXPECT_TRUE(a_star.getGoalManager().getGoalsState().size() == num_valid_goals); - EXPECT_TRUE(a_star.createPath(path, num_it, tolerance, dummy_cancel_checker, expansions.get())); - EXPECT_TRUE(a_star.getGoalManager().getGoalsState().size() == - a_star.getGoalManager().getGoalsCoordinates().size()); - - // UNKNOWN goal heading mode - a_star.setCollisionChecker(checker.get()); - a_star.setStart(10u, 10u, 0u); - - EXPECT_THROW( - a_star.setGoal( - 80u, 80u, 10u, - nav2_smac_planner::GoalHeadingMode::UNKNOWN), std::runtime_error); -} - TEST(AStarTest, test_constants) { nav2_smac_planner::MotionModel mm = nav2_smac_planner::MotionModel::UNKNOWN; // unknown @@ -539,15 +417,6 @@ TEST(AStarTest, test_constants) mm = nav2_smac_planner::MotionModel::REEDS_SHEPP; // reeds-shepp EXPECT_EQ(nav2_smac_planner::toString(mm), std::string("Reeds-Shepp")); - nav2_smac_planner::GoalHeadingMode gh = nav2_smac_planner::GoalHeadingMode::UNKNOWN; - EXPECT_EQ(nav2_smac_planner::toString(gh), std::string("Unknown")); - gh = nav2_smac_planner::GoalHeadingMode::DEFAULT; // default - EXPECT_EQ(nav2_smac_planner::toString(gh), std::string("DEFAULT")); - gh = nav2_smac_planner::GoalHeadingMode::BIDIRECTIONAL; // bidirectional - EXPECT_EQ(nav2_smac_planner::toString(gh), std::string("BIDIRECTIONAL")); - gh = nav2_smac_planner::GoalHeadingMode::ALL_DIRECTION; // all_direction - EXPECT_EQ(nav2_smac_planner::toString(gh), std::string("ALL_DIRECTION")); - EXPECT_EQ( nav2_smac_planner::fromString( "2D"), nav2_smac_planner::MotionModel::TWOD); @@ -556,18 +425,6 @@ TEST(AStarTest, test_constants) nav2_smac_planner::fromString( "REEDS_SHEPP"), nav2_smac_planner::MotionModel::REEDS_SHEPP); EXPECT_EQ(nav2_smac_planner::fromString("NONE"), nav2_smac_planner::MotionModel::UNKNOWN); - - EXPECT_EQ( - nav2_smac_planner::fromStringToGH( - "DEFAULT"), nav2_smac_planner::GoalHeadingMode::DEFAULT); - EXPECT_EQ( - nav2_smac_planner::fromStringToGH( - "BIDIRECTIONAL"), nav2_smac_planner::GoalHeadingMode::BIDIRECTIONAL); - EXPECT_EQ( - nav2_smac_planner::fromStringToGH( - "ALL_DIRECTION"), nav2_smac_planner::GoalHeadingMode::ALL_DIRECTION); - EXPECT_EQ( - nav2_smac_planner::fromStringToGH("NONE"), nav2_smac_planner::GoalHeadingMode::UNKNOWN); } int main(int argc, char ** argv) diff --git a/nav2_smac_planner/test/test_goal_manager.cpp b/nav2_smac_planner/test/test_goal_manager.cpp deleted file mode 100644 index cbd17b87895..00000000000 --- a/nav2_smac_planner/test/test_goal_manager.cpp +++ /dev/null @@ -1,177 +0,0 @@ -// Copyright (c) 2023 Open Navigation LLC -// Copyright (c) 2024 Stevedan Ogochukwu Omodolor Omodia -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include - -#include "gtest/gtest.h" -#include "rclcpp/rclcpp.hpp" -#include "nav2_smac_planner/goal_manager.hpp" -#include "nav2_costmap_2d/costmap_2d.hpp" -#include "nav2_costmap_2d/costmap_subscriber.hpp" -#include "nav2_util/lifecycle_node.hpp" -#include "nav2_smac_planner/collision_checker.hpp" - -using namespace nav2_smac_planner; // NOLINT - -using GoalManagerHybrid = GoalManager; -using NodePtr = NodeHybrid *; -using NodeVector = GoalManagerHybrid::NodeVector; -using CoordinateVector = GoalManagerHybrid::CoordinateVector; -using GoalStateVector = GoalManagerHybrid::GoalStateVector; - -TEST(GoalManagerTest, test_goal_manager) -{ - auto node = std::make_shared("test_node"); - - auto costmapA = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0); - - // Create an island of lethal cost in the middle - for (unsigned int i = 40; i <= 60; ++i) { - for (unsigned int j = 40; j <= 60; ++j) { - costmapA->setCost(i, j, 254); - } - } - - // Convert raw costmap into a costmap ros object - auto costmap_ros = std::make_shared(); - costmap_ros->on_configure(rclcpp_lifecycle::State()); - auto costmap = costmap_ros->getCostmap(); - *costmap = *costmapA; - - auto checker = std::make_unique( - costmap_ros, 72, node); - checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); - - GoalManagerHybrid goal_manager; - float tolerance = 20.0f; - bool allow_unknow = false; - - EXPECT_TRUE(goal_manager.goalsIsEmpty()); - - // Create two valid goals - NodePtr pose_a = new NodeHybrid(48); - NodePtr pose_b = new NodeHybrid(49); - pose_a->setPose(NodeHybrid::Coordinates(0, 0, 0)); - pose_b->setPose(NodeHybrid::Coordinates(0, 0, 10)); - - GoalStateVector goals_state = { - {pose_a, true}, - {pose_b, true} - }; - - goal_manager.setGoalStates(goals_state); - EXPECT_FALSE(goal_manager.goalsIsEmpty()); - EXPECT_EQ(goal_manager.getGoalsState().size(), 2u); - EXPECT_TRUE(goal_manager.getGoalsSet().empty()); - EXPECT_TRUE(goal_manager.getGoalsCoordinates().empty()); - - goal_manager.removeInvalidGoals(tolerance, checker.get(), allow_unknow); - - EXPECT_EQ(goal_manager.getGoalsSet().size(), 2); - EXPECT_EQ(goal_manager.getGoalsCoordinates().size(), 2); - for (const auto & goal_state : goal_manager.getGoalsState()) { - EXPECT_TRUE(goal_state.is_valid); - } - - // Test is goal - EXPECT_TRUE(goal_manager.isGoal(goals_state[0].goal)); - EXPECT_TRUE(goal_manager.isGoal(goals_state[1].goal)); - - // Re-populate and validate reset state - goal_manager.setGoalStates(goals_state); - EXPECT_EQ(goal_manager.getGoalsSet().size(), 0); - EXPECT_EQ(goal_manager.getGoalsCoordinates().size(), 0); - - // Add invalid goal - NodeHybrid pose_c(50); - pose_c.setPose(NodeHybrid::Coordinates(50, 50, 0)); // inside lethal zone - goals_state.push_back({&pose_c, true}); - - goal_manager.setGoalStates(goals_state); - EXPECT_EQ(goal_manager.getGoalsState().size(), 3); - - // Tolerance is high, one goal is invalid - // all goals are valid - goal_manager.removeInvalidGoals(tolerance, checker.get(), allow_unknow); - EXPECT_EQ(goal_manager.getGoalsSet().size(), 3); - EXPECT_EQ(goal_manager.getGoalsCoordinates().size(), 3); - - for (const auto & goal_state : goal_manager.getGoalsState()) { - EXPECT_TRUE(goal_state.is_valid); - } - - // Test with low tolerance, expect invalid goal to be filtered out - goal_manager.setGoalStates(goals_state); - int low_tolerance = 0.0f; - goal_manager.removeInvalidGoals(low_tolerance, checker.get(), allow_unknow); - - EXPECT_EQ(goal_manager.getGoalsSet().size(), 2); - EXPECT_EQ(goal_manager.getGoalsCoordinates().size(), 2); - - // expect last goal to be invalid - for (const auto & goal_state : goal_manager.getGoalsState()) { - if (goal_state.goal == goals_state[2].goal) { - EXPECT_FALSE(goal_state.is_valid); - } else { - EXPECT_TRUE(goal_state.is_valid); - } - } - - // Prepare goals for expansion - GoalStateVector expansion_goals; - unsigned int test_goal_size = 16; - - for (unsigned int i = 0; i < test_goal_size; ++i) { - auto goal = new NodeHybrid(i); - goal->setPose(NodeHybrid::Coordinates(i, i, 0)); - expansion_goals.push_back({goal, true}); - } - - goal_manager.setGoalStates(expansion_goals); - goal_manager.removeInvalidGoals(tolerance, checker.get(), allow_unknow); - - NodeVector coarse_check_goals; - NodeVector fine_check_goals; - - // Resolution 1: everything coarse - goal_manager.prepareGoalsForAnalyticExpansion(coarse_check_goals, fine_check_goals, 1); - EXPECT_EQ(coarse_check_goals.size(), test_goal_size); - EXPECT_TRUE(fine_check_goals.empty()); - - // Resolution 4: every 4th coarse - coarse_check_goals.clear(); - fine_check_goals.clear(); - - goal_manager.prepareGoalsForAnalyticExpansion(coarse_check_goals, fine_check_goals, 4); - EXPECT_EQ(coarse_check_goals.size(), 4u); // indices 0, 4, 8, 12 - EXPECT_EQ(fine_check_goals.size(), 12u); - - delete costmapA; - nav2_smac_planner::NodeHybrid::destroyStaticAssets(); -} - - -int main(int argc, char **argv) -{ - ::testing::InitGoogleTest(&argc, argv); - - rclcpp::init(0, nullptr); - - int result = RUN_ALL_TESTS(); - - rclcpp::shutdown(); - - return result; -} diff --git a/nav2_smac_planner/test/test_node2d.cpp b/nav2_smac_planner/test/test_node2d.cpp index 70706558809..816376b814b 100644 --- a/nav2_smac_planner/test/test_node2d.cpp +++ b/nav2_smac_planner/test/test_node2d.cpp @@ -67,10 +67,8 @@ TEST(Node2DTest, test_node_2d) // check heuristic cost computation nav2_smac_planner::Node2D::Coordinates A(0.0, 0.0); - nav2_smac_planner::Node2D::CoordinateVector B_vec; nav2_smac_planner::Node2D::Coordinates B(10.0, 5.0); - B_vec.push_back(B); - EXPECT_NEAR(testB.getHeuristicCost(A, B_vec), 11.18, 0.02); + EXPECT_NEAR(testB.getHeuristicCost(A, B), 11.18, 0.02); // check operator== works on index unsigned char costC = '2'; diff --git a/nav2_smac_planner/test/test_smac_hybrid.cpp b/nav2_smac_planner/test/test_smac_hybrid.cpp index 4870ec12ba0..4f521fa71b1 100644 --- a/nav2_smac_planner/test/test_smac_hybrid.cpp +++ b/nav2_smac_planner/test/test_smac_hybrid.cpp @@ -29,36 +29,6 @@ #include "nav2_smac_planner/smac_planner_hybrid.hpp" #include "nav2_smac_planner/smac_planner_2d.hpp" -// Simple wrapper to be able to call a private member -class HybridWrap : public nav2_smac_planner::SmacPlannerHybrid -{ -public: - void callDynamicParams(std::vector parameters) - { - dynamicParametersCallback(parameters); - } - - int getCoarseSearchResolution() - { - return _coarse_search_resolution; - } - - int getMaxIterations() - { - return _max_iterations; - } - - int getMaxOnApproachIterations() - { - return _max_on_approach_iterations; - } - - nav2_smac_planner::GoalHeadingMode getGoalHeadingMode() - { - return _goal_heading_mode; - } -}; - // SMAC smoke tests for plugin-level issues rather than algorithms // (covered by more extensively testing in other files) // System tests in nav2_system_tests will actually plan with this work @@ -89,46 +59,7 @@ TEST(SmacTest, test_smac_se2) goal.pose.position.x = 1.0; goal.pose.position.y = 1.0; goal.pose.orientation.w = 1.0; - auto planner = std::make_unique(); - - // invalid goal heading mode - nodeSE2->declare_parameter("test.goal_heading_mode", std::string("UNKNOWN")); - nodeSE2->set_parameter(rclcpp::Parameter("test.goal_heading_mode", std::string("UNKNOWN"))); - EXPECT_THROW(planner->configure(nodeSE2, "test", nullptr, costmap_ros), std::runtime_error); - nodeSE2->set_parameter(rclcpp::Parameter("test.goal_heading_mode", std::string("DEFAULT"))); - - // Invalid motion model should not change the default value - nodeSE2->set_parameter(rclcpp::Parameter("test.motion_model_for_search", std::string("invalid"))); - EXPECT_THROW(planner->configure(nodeSE2, "test", nullptr, costmap_ros), std::runtime_error); - nodeSE2->set_parameter(rclcpp::Parameter("test.motion_model_for_search", std::string("DUBIN"))); - - // invalid coarse search resolution - nodeSE2->set_parameter(rclcpp::Parameter("test.coarse_search_resolution", -1)); - nodeSE2->set_parameter(rclcpp::Parameter("test.max_on_approach_iterations", -1)); - nodeSE2->set_parameter(rclcpp::Parameter("test.max_iterations", -1)); - EXPECT_NO_THROW(planner->configure(nodeSE2, "test", nullptr, costmap_ros)); - - EXPECT_EQ(planner->getCoarseSearchResolution(), 1); - EXPECT_EQ(planner->getMaxIterations(), std::numeric_limits::max()); - EXPECT_EQ(planner->getMaxOnApproachIterations(), std::numeric_limits::max()); - - - // valid Configuration - nodeSE2->set_parameter(rclcpp::Parameter("test.coarse_search_resolution", 1)); - nodeSE2->set_parameter(rclcpp::Parameter("test.max_on_approach_iterations", 1000)); - nodeSE2->set_parameter(rclcpp::Parameter("test.max_iterations", 1000000)); - EXPECT_NO_THROW(planner->configure(nodeSE2, "test", nullptr, costmap_ros)); - - - // angle_quantizations not multiple of coarse search resolution would trigger a throw - nodeSE2->set_parameter(rclcpp::Parameter("test.angle_quantization_bins", 72)); - nodeSE2->set_parameter(rclcpp::Parameter("test.coarse_search_resolution", 5)); - EXPECT_THROW(planner->configure(nodeSE2, "test", nullptr, costmap_ros), std::runtime_error); - - // valid configuration - nodeSE2->set_parameter(rclcpp::Parameter("test.coarse_search_resolution", 4)); - EXPECT_NO_THROW(planner->configure(nodeSE2, "test", nullptr, costmap_ros)); - + auto planner = std::make_unique(); planner->configure(nodeSE2, "test", nullptr, costmap_ros); planner->activate(); @@ -162,7 +93,7 @@ TEST(SmacTest, test_smac_se2_reconfigure) std::make_shared("global_costmap"); costmap_ros->on_configure(rclcpp_lifecycle::State()); - auto planner = std::make_unique(); + auto planner = std::make_unique(); planner->configure(nodeSE2, "test", nullptr, costmap_ros); planner->activate(); @@ -176,7 +107,7 @@ TEST(SmacTest, test_smac_se2_reconfigure) auto results = rec_param->set_parameters_atomically( {rclcpp::Parameter("test.downsample_costmap", true), rclcpp::Parameter("test.downsampling_factor", 2), - rclcpp::Parameter("test.angle_quantization_bins", 72), + rclcpp::Parameter("test.angle_quantization_bins", 100), rclcpp::Parameter("test.allow_unknown", false), rclcpp::Parameter("test.max_iterations", -1), rclcpp::Parameter("test.minimum_turning_radius", 1.0), @@ -194,9 +125,7 @@ TEST(SmacTest, test_smac_se2_reconfigure) rclcpp::Parameter("test.analytic_expansion_max_length", 42.0), rclcpp::Parameter("test.max_on_approach_iterations", 42), rclcpp::Parameter("test.terminal_checking_interval", 42), - rclcpp::Parameter("test.motion_model_for_search", std::string("REEDS_SHEPP")), - rclcpp::Parameter("test.goal_heading_mode", std::string("BIDIRECTIONAL")), - rclcpp::Parameter("test.coarse_search_resolution", -1)}); + rclcpp::Parameter("test.motion_model_for_search", std::string("REEDS_SHEPP"))}); rclcpp::spin_until_future_complete( nodeSE2->get_node_base_interface(), @@ -204,7 +133,7 @@ TEST(SmacTest, test_smac_se2_reconfigure) EXPECT_EQ(nodeSE2->get_parameter("test.downsample_costmap").as_bool(), true); EXPECT_EQ(nodeSE2->get_parameter("test.downsampling_factor").as_int(), 2); - EXPECT_EQ(nodeSE2->get_parameter("test.angle_quantization_bins").as_int(), 72); + EXPECT_EQ(nodeSE2->get_parameter("test.angle_quantization_bins").as_int(), 100); EXPECT_EQ(nodeSE2->get_parameter("test.allow_unknown").as_bool(), false); EXPECT_EQ(nodeSE2->get_parameter("test.max_iterations").as_int(), -1); EXPECT_EQ(nodeSE2->get_parameter("test.minimum_turning_radius").as_double(), 1.0); @@ -225,9 +154,6 @@ TEST(SmacTest, test_smac_se2_reconfigure) EXPECT_EQ( nodeSE2->get_parameter("test.motion_model_for_search").as_string(), std::string("REEDS_SHEPP")); - EXPECT_EQ( - nodeSE2->get_parameter("test.goal_heading_mode").as_string(), - std::string("BIDIRECTIONAL")); auto results2 = rec_param->set_parameters_atomically( {rclcpp::Parameter("resolution", 0.2)}); @@ -235,30 +161,6 @@ TEST(SmacTest, test_smac_se2_reconfigure) nodeSE2->get_node_base_interface(), results2); EXPECT_EQ(nodeSE2->get_parameter("resolution").as_double(), 0.2); - EXPECT_EQ(nodeSE2->get_parameter("test.coarse_search_resolution").as_int(), -1); - EXPECT_EQ(planner->getCoarseSearchResolution(), 1); - - // test coarse resolution edge cases. Consider when coarse resolution - // is not multiple of angle bin quantization(72) - std::vector parameters; - parameters.push_back(rclcpp::Parameter("test.coarse_search_resolution", 7)); - EXPECT_NO_THROW(planner->callDynamicParams(parameters)); - EXPECT_EQ(planner->getCoarseSearchResolution(), 1); - - // same test as before but the error comes from the angular bin - parameters.clear(); - parameters.push_back(rclcpp::Parameter("test.coarse_search_resolution", 4)); - parameters.push_back(rclcpp::Parameter("test.angle_quantization_bins", 87)); - EXPECT_NO_THROW(planner->callDynamicParams(parameters)); - EXPECT_EQ(planner->getCoarseSearchResolution(), 1); - - // test invalid goal heading mode does not modify current - // goal heading mode - parameters.clear(); - parameters.push_back(rclcpp::Parameter("test.goal_heading_mode", std::string("BIDIRECTIONAL"))); - parameters.push_back(rclcpp::Parameter("test.goal_heading_mode", std::string("invalid"))); - EXPECT_NO_THROW(planner->callDynamicParams(parameters)); - EXPECT_EQ(planner->getGoalHeadingMode(), nav2_smac_planner::GoalHeadingMode::BIDIRECTIONAL); } int main(int argc, char ** argv) diff --git a/nav2_smac_planner/test/test_smac_lattice.cpp b/nav2_smac_planner/test/test_smac_lattice.cpp index 39758294b83..a1450c374bf 100644 --- a/nav2_smac_planner/test/test_smac_lattice.cpp +++ b/nav2_smac_planner/test/test_smac_lattice.cpp @@ -36,26 +36,6 @@ class LatticeWrap : public nav2_smac_planner::SmacPlannerLattice { dynamicParametersCallback(parameters); } - - int getCoarseSearchResolution() - { - return _coarse_search_resolution; - } - - int getMaxIterations() - { - return _max_iterations; - } - - int getMaxOnApproachIterations() - { - return _max_on_approach_iterations; - } - - nav2_smac_planner::GoalHeadingMode getGoalHeadingMode() - { - return _goal_heading_mode; - } }; // SMAC smoke tests for plugin-level issues rather than algorithms @@ -83,38 +63,10 @@ TEST(SmacTest, test_smac_lattice) goal.pose.position.x = 1.0; goal.pose.position.y = 1.0; goal.pose.orientation.w = 1.0; - auto planner = std::make_unique(); + auto planner = std::make_unique(); try { - // invalid goal heading mode - nodeLattice->declare_parameter("test.goal_heading_mode", std::string("UNKNOWN")); - nodeLattice->set_parameter(rclcpp::Parameter("test.goal_heading_mode", std::string("UNKNOWN"))); - EXPECT_THROW(planner->configure(nodeLattice, "test", nullptr, costmap_ros), std::runtime_error); - nodeLattice->set_parameter(rclcpp::Parameter("test.goal_heading_mode", std::string("DEFAULT"))); - - // invalid Configuration resolution - nodeLattice->set_parameter(rclcpp::Parameter("test.coarse_search_resolution", -1)); - nodeLattice->set_parameter(rclcpp::Parameter("test.max_iterations", -1)); - nodeLattice->set_parameter(rclcpp::Parameter("test.max_on_approach_iterations", -1)); - - EXPECT_NO_THROW(planner->configure(nodeLattice, "test", nullptr, costmap_ros)); - EXPECT_EQ(planner->getCoarseSearchResolution(), 1); - EXPECT_EQ(planner->getMaxIterations(), std::numeric_limits::max()); - EXPECT_EQ(planner->getMaxOnApproachIterations(), std::numeric_limits::max()); - - - // Valid configuration - nodeLattice->set_parameter(rclcpp::Parameter("test.max_iterations", 1000000)); - nodeLattice->set_parameter(rclcpp::Parameter("test.max_on_approach_iterations", 1000)); - - // Coarse search resolution will throw, not multiple of number of heading(16 default) - nodeLattice->set_parameter(rclcpp::Parameter("test.coarse_search_resolution", 3)); - EXPECT_THROW(planner->configure(nodeLattice, "test", nullptr, costmap_ros), std::runtime_error); - - // Valid configuration - nodeLattice->set_parameter(rclcpp::Parameter("test.coarse_search_resolution", 4)); // Expect to throw due to invalid prims file in param planner->configure(nodeLattice, "test", nullptr, costmap_ros); - EXPECT_EQ(planner->getCoarseSearchResolution(), 4); } catch (...) { } planner->activate(); @@ -191,41 +143,9 @@ TEST(SmacTest, test_smac_lattice_reconfigure) results); } catch (...) { } - // test edge cases Goal heading mode, make sure we don't reset the goal when invalid - std::vector parameters; - parameters.push_back(rclcpp::Parameter("test.goal_heading_mode", std::string("BIDIRECTIONAL"))); - parameters.push_back(rclcpp::Parameter("test.goal_heading_mode", std::string("invalid"))); - EXPECT_NO_THROW(planner->callDynamicParams(parameters)); - EXPECT_EQ(planner->getGoalHeadingMode(), nav2_smac_planner::GoalHeadingMode::BIDIRECTIONAL); - - // test coarse resolution edge cases. - // Negative coarse search resolution - parameters.clear(); - parameters.push_back(rclcpp::Parameter("test.coarse_search_resolution", -1)); - EXPECT_NO_THROW(planner->callDynamicParams(parameters)); - EXPECT_EQ(planner->getCoarseSearchResolution(), 1); - - // test value when coarse resolution - // is not multiple number_of_headings - parameters.clear(); - parameters.push_back(rclcpp::Parameter("test.coarse_search_resolution", 5)); - EXPECT_NO_THROW(planner->callDynamicParams(parameters)); - EXPECT_EQ(planner->getCoarseSearchResolution(), 1); - - // Similar modulous test but when the issue is from the number - // of heading, test output includes number of heading 15 - parameters.clear(); - - parameters.push_back(rclcpp::Parameter("test.coarse_search_resolution", 4)); - parameters.push_back(rclcpp::Parameter("test.lattice_filepath", - ament_index_cpp::get_package_share_directory("nav2_smac_planner") + - "/sample_primitives/test/output.json")); - EXPECT_NO_THROW(planner->callDynamicParams(parameters)); - EXPECT_EQ(planner->getCoarseSearchResolution(), 1); - // So instead, let's call manually on a change - parameters.clear(); + std::vector parameters; parameters.push_back(rclcpp::Parameter("test.lattice_filepath", std::string("HI"))); EXPECT_THROW(planner->callDynamicParams(parameters), std::runtime_error); } From 2fecf062211359d14a4de4cf59d092e218a3d0c9 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 2 Jun 2025 12:26:08 -0700 Subject: [PATCH 31/46] Disable costmap filter zones from tb3 bringup (only Tb4 enabled) (#5223) * Update tb3_loopback_simulation_launch.py Signed-off-by: Steve Macenski * Update tb3_simulation_launch.py Signed-off-by: Steve Macenski --------- Signed-off-by: Steve Macenski --- nav2_bringup/launch/tb3_loopback_simulation_launch.py | 2 ++ nav2_bringup/launch/tb3_simulation_launch.py | 2 ++ 2 files changed, 4 insertions(+) diff --git a/nav2_bringup/launch/tb3_loopback_simulation_launch.py b/nav2_bringup/launch/tb3_loopback_simulation_launch.py index 26c22a31d50..9f51e66c04b 100644 --- a/nav2_bringup/launch/tb3_loopback_simulation_launch.py +++ b/nav2_bringup/launch/tb3_loopback_simulation_launch.py @@ -144,6 +144,8 @@ def generate_launch_description() -> LaunchDescription: 'use_composition': use_composition, 'use_respawn': use_respawn, 'use_localization': 'False', # Don't use SLAM, AMCL + 'use_keepout_zones': 'False', + 'use_speed_zones': 'False', }.items(), ) diff --git a/nav2_bringup/launch/tb3_simulation_launch.py b/nav2_bringup/launch/tb3_simulation_launch.py index cc40ed90c56..e0a5f217646 100644 --- a/nav2_bringup/launch/tb3_simulation_launch.py +++ b/nav2_bringup/launch/tb3_simulation_launch.py @@ -195,6 +195,8 @@ def generate_launch_description() -> LaunchDescription: 'autostart': autostart, 'use_composition': use_composition, 'use_respawn': use_respawn, + 'use_keepout_zones': 'False', + 'use_speed_zones': 'False', }.items(), ) # The SDF file for the world is a xacro file because we wanted to From bb66f658c8f3a0920e01a51e45347c662d2dcb32 Mon Sep 17 00:00:00 2001 From: Nils-Christian Iseke <48475933+Nils-ChristianIseke@users.noreply.github.com> Date: Thu, 5 Jun 2025 19:34:41 +0200 Subject: [PATCH 32/46] Revert "Fix Ci from key signing (#5220)" (#5237) * Revert "Fix Ci from key signing (#5220)" This reverts the changes to the Dockerfile done in 1345c226bcf73068127b2e2efaee91a637d2f351. Signed-off-by: Nils-Christian Iseke * Update Cache Version Signed-off-by: Nils-Christian Iseke --------- Signed-off-by: Nils-Christian Iseke From 8febffe4384a9ccf333ad3e3ce335912a9634a9f Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 9 Jun 2025 12:59:53 -0700 Subject: [PATCH 33/46] Updating readme table for kilted release (#5249) * updating readme table for kilted release Signed-off-by: Steve Macenski * Updating table lint Signed-off-by: Steve Macenski --------- Signed-off-by: Steve Macenski --- README.md | 73 ++++++++++++++++++------------------ tools/update_readme_table.py | 8 ++-- 2 files changed, 42 insertions(+), 39 deletions(-) diff --git a/README.md b/README.md index 64125080c7f..d7e58d43dc2 100644 --- a/README.md +++ b/README.md @@ -113,39 +113,40 @@ If you use our work on VSLAM and formal comparisons for service robot needs, ple ## Build Status -| Package | humble Source | humble Debian | jazzy Source | jazzy Debian | -| :---: | :---: | :---: | :---: | :---: | -| navigation2 | [![Build Status](https://build.ros2.org/job/Hsrc_uj__navigation2__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__navigation2__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__navigation2__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__navigation2__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__navigation2__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__navigation2__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__navigation2__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__navigation2__ubuntu_noble_amd64__binary/) | -| nav2_amcl | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_amcl__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_amcl__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_amcl__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_amcl__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_amcl__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_amcl__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_amcl__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_amcl__ubuntu_noble_amd64__binary/) | -| nav2_behavior_tree | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_behavior_tree__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_behavior_tree__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_behavior_tree__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_behavior_tree__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_behavior_tree__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_behavior_tree__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_behavior_tree__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_behavior_tree__ubuntu_noble_amd64__binary/) | -| nav2_behaviors | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_behaviors__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_behaviors__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_behaviors__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_behaviors__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_behaviors__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_behaviors__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_behaviors__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_behaviors__ubuntu_noble_amd64__binary/) | -| nav2_bringup | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_bringup__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_bringup__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_bringup__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_bringup__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_bringup__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_bringup__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_bringup__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_bringup__ubuntu_noble_amd64__binary/) | -| nav2_bt_navigator | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_bt_navigator__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_bt_navigator__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_bt_navigator__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_bt_navigator__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_bt_navigator__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_bt_navigator__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_bt_navigator__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_bt_navigator__ubuntu_noble_amd64__binary/) | -| nav2_collision_monitor | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_collision_monitor__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_collision_monitor__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_collision_monitor__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_collision_monitor__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_collision_monitor__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_collision_monitor__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_collision_monitor__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_collision_monitor__ubuntu_noble_amd64__binary/) | -| nav2_common | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_common__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_common__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_common__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_common__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_common__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_common__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_common__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_common__ubuntu_noble_amd64__binary/) | -| nav2_constrained_smoother | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_constrained_smoother__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_constrained_smoother__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_constrained_smoother__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_constrained_smoother__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_constrained_smoother__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_constrained_smoother__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_constrained_smoother__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_constrained_smoother__ubuntu_noble_amd64__binary/) | -| nav2_controller | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_controller__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_controller__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_controller__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_controller__ubuntu_noble_amd64__binary/) | -| nav2_core | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_core__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_core__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_core__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_core__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_core__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_core__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_core__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_core__ubuntu_noble_amd64__binary/) | -| nav2_costmap_2d | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_costmap_2d__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_costmap_2d__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_costmap_2d__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_costmap_2d__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_costmap_2d__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_costmap_2d__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_costmap_2d__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_costmap_2d__ubuntu_noble_amd64__binary/) | -| nav2_docking | [![Build Status](https://build.ros2.org/job/Hsrc_uj__opennav_docking__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__opennav_docking__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__opennav_docking__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__opennav_docking__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__opennav_docking__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__opennav_docking__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__opennav_docking__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__opennav_docking__ubuntu_noble_amd64__binary/) | -| nav2_dwb_controller | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_dwb_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_dwb_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_dwb_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_dwb_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_dwb_controller__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_dwb_controller__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_dwb_controller__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_dwb_controller__ubuntu_noble_amd64__binary/) | -| nav2_graceful_controller | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_graceful_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_graceful_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_graceful_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_graceful_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_graceful_controller__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_graceful_controller__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_graceful_controller__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_graceful_controller__ubuntu_noble_amd64__binary/) | -| nav2_lifecycle_manager | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_lifecycle_manager__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_lifecycle_manager__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_lifecycle_manager__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_lifecycle_manager__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_lifecycle_manager__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_lifecycle_manager__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_lifecycle_manager__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_lifecycle_manager__ubuntu_noble_amd64__binary/) | -| nav2_loopback_sim | N/A | N/A | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_loopback_sim__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_loopback_sim__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_loopback_sim__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_loopback_sim__ubuntu_noble_amd64__binary/) | -| nav2_map_server | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_map_server__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_map_server__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_map_server__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_map_server__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_map_server__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_map_server__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_map_server__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_map_server__ubuntu_noble_amd64__binary/) | -| nav2_mppi_controller | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_mppi_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_mppi_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_mppi_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_mppi_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_mppi_controller__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_mppi_controller__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_mppi_controller__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_mppi_controller__ubuntu_noble_amd64__binary/) | -| nav2_msgs | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_msgs__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_msgs__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_msgs__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_msgs__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_msgs__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_msgs__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_msgs__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_msgs__ubuntu_noble_amd64__binary/) | -| nav2_navfn_planner | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_navfn_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_navfn_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_navfn_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_navfn_planner__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_navfn_planner__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_navfn_planner__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_navfn_planner__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_navfn_planner__ubuntu_noble_amd64__binary/) | -| nav2_planner | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_planner__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_planner__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_planner__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_planner__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_planner__ubuntu_noble_amd64__binary/) | -| nav2_regulated_pure_pursuit | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_regulated_pure_pursuit_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_regulated_pure_pursuit_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_regulated_pure_pursuit_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_regulated_pure_pursuit_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_regulated_pure_pursuit_controller__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_regulated_pure_pursuit_controller__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_regulated_pure_pursuit_controller__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_regulated_pure_pursuit_controller__ubuntu_noble_amd64__binary/) | -| nav2_rotation_shim_controller | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_rotation_shim_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_rotation_shim_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_rotation_shim_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_rotation_shim_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_rotation_shim_controller__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_rotation_shim_controller__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_rotation_shim_controller__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_rotation_shim_controller__ubuntu_noble_amd64__binary/) | -| nav2_rviz_plugins | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_rviz_plugins__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_rviz_plugins__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_rviz_plugins__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_rviz_plugins__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_rviz_plugins__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_rviz_plugins__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_rviz_plugins__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_rviz_plugins__ubuntu_noble_amd64__binary/) | -| nav2_simple_commander | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_simple_commander__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_simple_commander__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_simple_commander__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_simple_commander__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_simple_commander__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_simple_commander__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_simple_commander__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_simple_commander__ubuntu_noble_amd64__binary/) | -| nav2_smac_planner | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_smac_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_smac_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_smac_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_smac_planner__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_smac_planner__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_smac_planner__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_smac_planner__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_smac_planner__ubuntu_noble_amd64__binary/) | -| nav2_smoother | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_smoother__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_smoother__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_smoother__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_smoother__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_smoother__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_smoother__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_smoother__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_smoother__ubuntu_noble_amd64__binary/) | -| nav2_system_tests | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_system_tests__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_system_tests__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_system_tests__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_system_tests__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_system_tests__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_system_tests__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_system_tests__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_system_tests__ubuntu_noble_amd64__binary/) | -| nav2_theta_star_planner | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_theta_star_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_theta_star_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_theta_star_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_theta_star_planner__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_theta_star_planner__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_theta_star_planner__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_theta_star_planner__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_theta_star_planner__ubuntu_noble_amd64__binary/) | -| nav2_util | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_util__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_util__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_util__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_util__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_util__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_util__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_util__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_util__ubuntu_noble_amd64__binary/) | -| nav2_velocity_smoother | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_velocity_smoother__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_velocity_smoother__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_velocity_smoother__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_velocity_smoother__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_velocity_smoother__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_velocity_smoother__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_velocity_smoother__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_velocity_smoother__ubuntu_noble_amd64__binary/) | -| nav2_voxel_grid | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_voxel_grid__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_voxel_grid__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_voxel_grid__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_voxel_grid__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_voxel_grid__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_voxel_grid__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_voxel_grid__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_voxel_grid__ubuntu_noble_amd64__binary/) | -| nav2_waypoint_follower | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_waypoint_follower__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_waypoint_follower__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_waypoint_follower__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_waypoint_follower__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_waypoint_follower__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_waypoint_follower__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_waypoint_follower__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_waypoint_follower__ubuntu_noble_amd64__binary/) | +| Package | humble Source | humble Debian | jazzy Source | jazzy Debian | kilted Source | kilted Debian | +| :---: | :---: | :---: | :---: | :---: | :---: | :---: | +| navigation2 | [![Build Status](https://build.ros2.org/job/Hsrc_uj__navigation2__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__navigation2__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__navigation2__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__navigation2__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__navigation2__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__navigation2__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__navigation2__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__navigation2__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__navigation2__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__navigation2__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__navigation2__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_un64__navigation2__ubuntu_noble_amd64__binary/) | +| nav2_amcl | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_amcl__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_amcl__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_amcl__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_amcl__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_amcl__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_amcl__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_amcl__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_amcl__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_amcl__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_amcl__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_amcl__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_un64__nav2_amcl__ubuntu_noble_amd64__binary/) | +| nav2_behavior_tree | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_behavior_tree__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_behavior_tree__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_behavior_tree__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_behavior_tree__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_behavior_tree__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_behavior_tree__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_behavior_tree__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_behavior_tree__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_behavior_tree__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_behavior_tree__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_behavior_tree__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_un64__nav2_behavior_tree__ubuntu_noble_amd64__binary/) | +| nav2_behaviors | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_behaviors__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_behaviors__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_behaviors__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_behaviors__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_behaviors__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_behaviors__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_behaviors__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_behaviors__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_behaviors__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_behaviors__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_behaviors__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_un64__nav2_behaviors__ubuntu_noble_amd64__binary/) | +| nav2_bringup | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_bringup__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_bringup__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_bringup__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_bringup__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_bringup__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_bringup__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_bringup__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_bringup__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_bringup__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_bringup__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_bringup__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_un64__nav2_bringup__ubuntu_noble_amd64__binary/) | +| nav2_bt_navigator | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_bt_navigator__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_bt_navigator__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_bt_navigator__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_bt_navigator__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_bt_navigator__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_bt_navigator__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_bt_navigator__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_bt_navigator__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_bt_navigator__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_bt_navigator__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_bt_navigator__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_un64__nav2_bt_navigator__ubuntu_noble_amd64__binary/) | +| nav2_collision_monitor | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_collision_monitor__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_collision_monitor__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_collision_monitor__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_collision_monitor__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_collision_monitor__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_collision_monitor__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_collision_monitor__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_collision_monitor__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_collision_monitor__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_collision_monitor__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_collision_monitor__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_un64__nav2_collision_monitor__ubuntu_noble_amd64__binary/) | +| nav2_common | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_common__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_common__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_common__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_common__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_common__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_common__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_common__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_common__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_common__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_common__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_common__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_un64__nav2_common__ubuntu_noble_amd64__binary/) | +| nav2_constrained_smoother | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_constrained_smoother__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_constrained_smoother__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_constrained_smoother__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_constrained_smoother__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_constrained_smoother__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_constrained_smoother__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_constrained_smoother__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_constrained_smoother__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_constrained_smoother__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_constrained_smoother__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_constrained_smoother__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_un64__nav2_constrained_smoother__ubuntu_noble_amd64__binary/) | +| nav2_controller | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_controller__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_controller__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_controller__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_controller__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_controller__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_controller__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_controller__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_un64__nav2_controller__ubuntu_noble_amd64__binary/) | +| nav2_core | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_core__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_core__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_core__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_core__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_core__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_core__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_core__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_core__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_core__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_core__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_core__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_un64__nav2_core__ubuntu_noble_amd64__binary/) | +| nav2_costmap_2d | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_costmap_2d__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_costmap_2d__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_costmap_2d__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_costmap_2d__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_costmap_2d__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_costmap_2d__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_costmap_2d__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_costmap_2d__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_costmap_2d__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_costmap_2d__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_costmap_2d__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_un64__nav2_costmap_2d__ubuntu_noble_amd64__binary/) | +| nav2_docking | [![Build Status](https://build.ros2.org/job/Hsrc_uj__opennav_docking__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__opennav_docking__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__opennav_docking__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__opennav_docking__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__opennav_docking__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__opennav_docking__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__opennav_docking__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__opennav_docking__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__opennav_docking__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__opennav_docking__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__opennav_docking__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_un64__opennav_docking__ubuntu_noble_amd64__binary/) | +| nav2_dwb_controller | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_dwb_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_dwb_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_dwb_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_dwb_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_dwb_controller__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_dwb_controller__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_dwb_controller__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_dwb_controller__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_dwb_controller__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_dwb_controller__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_dwb_controller__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_un64__nav2_dwb_controller__ubuntu_noble_amd64__binary/) | +| nav2_graceful_controller | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_graceful_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_graceful_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_graceful_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_graceful_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_graceful_controller__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_graceful_controller__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_graceful_controller__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_graceful_controller__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_graceful_controller__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_graceful_controller__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_graceful_controller__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_un64__nav2_graceful_controller__ubuntu_noble_amd64__binary/) | +| nav2_lifecycle_manager | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_lifecycle_manager__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_lifecycle_manager__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_lifecycle_manager__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_lifecycle_manager__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_lifecycle_manager__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_lifecycle_manager__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_lifecycle_manager__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_lifecycle_manager__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_lifecycle_manager__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_lifecycle_manager__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_lifecycle_manager__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_un64__nav2_lifecycle_manager__ubuntu_noble_amd64__binary/) | +| nav2_loopback_sim | N/A | N/A | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_loopback_sim__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_loopback_sim__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_loopback_sim__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_loopback_sim__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_loopback_sim__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_loopback_sim__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_loopback_sim__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_un64__nav2_loopback_sim__ubuntu_noble_amd64__binary/) | +| nav2_map_server | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_map_server__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_map_server__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_map_server__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_map_server__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_map_server__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_map_server__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_map_server__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_map_server__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_map_server__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_map_server__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_map_server__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_un64__nav2_map_server__ubuntu_noble_amd64__binary/) | +| nav2_mppi_controller | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_mppi_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_mppi_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_mppi_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_mppi_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_mppi_controller__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_mppi_controller__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_mppi_controller__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_mppi_controller__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_mppi_controller__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_mppi_controller__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_mppi_controller__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_un64__nav2_mppi_controller__ubuntu_noble_amd64__binary/) | +| nav2_msgs | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_msgs__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_msgs__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_msgs__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_msgs__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_msgs__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_msgs__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_msgs__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_msgs__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_msgs__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_msgs__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_msgs__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_un64__nav2_msgs__ubuntu_noble_amd64__binary/) | +| nav2_navfn_planner | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_navfn_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_navfn_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_navfn_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_navfn_planner__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_navfn_planner__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_navfn_planner__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_navfn_planner__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_navfn_planner__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_navfn_planner__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_navfn_planner__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_navfn_planner__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_un64__nav2_navfn_planner__ubuntu_noble_amd64__binary/) | +| nav2_planner | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_planner__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_planner__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_planner__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_planner__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_planner__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_planner__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_planner__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_planner__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_un64__nav2_planner__ubuntu_noble_amd64__binary/) | +| nav2_regulated_pure_pursuit | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_regulated_pure_pursuit_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_regulated_pure_pursuit_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_regulated_pure_pursuit_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_regulated_pure_pursuit_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_regulated_pure_pursuit_controller__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_regulated_pure_pursuit_controller__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_regulated_pure_pursuit_controller__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_regulated_pure_pursuit_controller__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_regulated_pure_pursuit_controller__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_regulated_pure_pursuit_controller__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_regulated_pure_pursuit_controller__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_un64__nav2_regulated_pure_pursuit_controller__ubuntu_noble_amd64__binary/) | +| nav2_rotation_shim_controller | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_rotation_shim_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_rotation_shim_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_rotation_shim_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_rotation_shim_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_rotation_shim_controller__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_rotation_shim_controller__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_rotation_shim_controller__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_rotation_shim_controller__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_rotation_shim_controller__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_rotation_shim_controller__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_rotation_shim_controller__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_un64__nav2_rotation_shim_controller__ubuntu_noble_amd64__binary/) | +| nav2_route | N/A | N/A | N/A | N/A | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_route__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_route__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_route__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_un64__nav2_route__ubuntu_noble_amd64__binary/) | +| nav2_rviz_plugins | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_rviz_plugins__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_rviz_plugins__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_rviz_plugins__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_rviz_plugins__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_rviz_plugins__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_rviz_plugins__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_rviz_plugins__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_rviz_plugins__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_rviz_plugins__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_rviz_plugins__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_rviz_plugins__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_un64__nav2_rviz_plugins__ubuntu_noble_amd64__binary/) | +| nav2_simple_commander | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_simple_commander__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_simple_commander__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_simple_commander__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_simple_commander__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_simple_commander__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_simple_commander__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_simple_commander__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_simple_commander__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_simple_commander__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_simple_commander__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_simple_commander__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_un64__nav2_simple_commander__ubuntu_noble_amd64__binary/) | +| nav2_smac_planner | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_smac_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_smac_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_smac_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_smac_planner__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_smac_planner__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_smac_planner__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_smac_planner__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_smac_planner__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_smac_planner__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_smac_planner__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_smac_planner__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_un64__nav2_smac_planner__ubuntu_noble_amd64__binary/) | +| nav2_smoother | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_smoother__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_smoother__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_smoother__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_smoother__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_smoother__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_smoother__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_smoother__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_smoother__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_smoother__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_smoother__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_smoother__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_un64__nav2_smoother__ubuntu_noble_amd64__binary/) | +| nav2_system_tests | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_system_tests__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_system_tests__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_system_tests__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_system_tests__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_system_tests__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_system_tests__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_system_tests__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_system_tests__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_system_tests__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_system_tests__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_system_tests__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_un64__nav2_system_tests__ubuntu_noble_amd64__binary/) | +| nav2_theta_star_planner | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_theta_star_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_theta_star_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_theta_star_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_theta_star_planner__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_theta_star_planner__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_theta_star_planner__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_theta_star_planner__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_theta_star_planner__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_theta_star_planner__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_theta_star_planner__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_theta_star_planner__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_un64__nav2_theta_star_planner__ubuntu_noble_amd64__binary/) | +| nav2_util | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_util__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_util__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_util__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_util__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_util__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_util__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_util__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_util__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_util__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_util__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_util__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_un64__nav2_util__ubuntu_noble_amd64__binary/) | +| nav2_velocity_smoother | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_velocity_smoother__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_velocity_smoother__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_velocity_smoother__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_velocity_smoother__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_velocity_smoother__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_velocity_smoother__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_velocity_smoother__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_velocity_smoother__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_velocity_smoother__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_velocity_smoother__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_velocity_smoother__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_un64__nav2_velocity_smoother__ubuntu_noble_amd64__binary/) | +| nav2_voxel_grid | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_voxel_grid__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_voxel_grid__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_voxel_grid__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_voxel_grid__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_voxel_grid__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_voxel_grid__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_voxel_grid__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_voxel_grid__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_voxel_grid__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_voxel_grid__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_voxel_grid__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_un64__nav2_voxel_grid__ubuntu_noble_amd64__binary/) | +| nav2_waypoint_follower | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_waypoint_follower__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_waypoint_follower__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_waypoint_follower__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_waypoint_follower__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_waypoint_follower__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_waypoint_follower__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_waypoint_follower__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_waypoint_follower__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_waypoint_follower__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_waypoint_follower__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_waypoint_follower__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_un64__nav2_waypoint_follower__ubuntu_noble_amd64__binary/) | diff --git a/tools/update_readme_table.py b/tools/update_readme_table.py index d36b65eac72..5ca0a2d1d84 100755 --- a/tools/update_readme_table.py +++ b/tools/update_readme_table.py @@ -18,8 +18,8 @@ import requests # Global information about current distributions, shouldn't need to update -OSs = {'humble': 'jammy', 'jazzy': 'noble'} -Prefixs = {'humble': 'H', 'jazzy': 'J'} +OSs = {'humble': 'jammy', 'jazzy': 'noble', 'kilted': 'noble'} +Prefixs = {'humble': 'H', 'jazzy': 'J', 'kilted': 'K'} # Set your packages here Packages = [ @@ -47,6 +47,7 @@ 'nav2_planner', 'nav2_regulated_pure_pursuit_controller', 'nav2_rotation_shim_controller', + 'nav2_route', 'nav2_rviz_plugins', 'nav2_simple_commander', 'nav2_smac_planner', @@ -60,7 +61,7 @@ ] # Set which distributions you care about -Distros = ['humble', 'jazzy'] +Distros = ['humble', 'jazzy', 'kilted'] def getSrcPath(package: str, prefix: str, OS: str) -> str: @@ -78,6 +79,7 @@ def createPreamble(Distros: list[str]) -> str: table = '| Package | ' for distro in Distros: table += distro + ' Source | ' + distro + ' Debian | ' + table = table[:-1] # Remove the last space table += '\n' table += '| :---: |' for distro in Distros: From 303fdc72b2a2b1b691d03fbc0d4bde7155e53189 Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Tue, 10 Jun 2025 20:49:55 +0100 Subject: [PATCH 34/46] Add min_distance_to_obstacle parameter to RPP (#4543) * min_distance_to_obstacle Signed-off-by: Guillaume Doisy * suggestion to time base and combine Signed-off-by: Guillaume Doisy * typo Signed-off-by: Guillaume Doisy * use min_approach_linear_velocity Signed-off-by: Guillaume Doisy --------- Signed-off-by: Guillaume Doisy Co-authored-by: Guillaume Doisy --- nav2_regulated_pure_pursuit_controller/README.md | 1 + .../parameter_handler.hpp | 1 + .../src/collision_checker.cpp | 10 +++++++++- .../src/parameter_handler.cpp | 8 ++++++++ .../test/test_regulated_pp.cpp | 2 ++ 5 files changed, 21 insertions(+), 1 deletion(-) diff --git a/nav2_regulated_pure_pursuit_controller/README.md b/nav2_regulated_pure_pursuit_controller/README.md index f49f9b31420..2caec97ff32 100644 --- a/nav2_regulated_pure_pursuit_controller/README.md +++ b/nav2_regulated_pure_pursuit_controller/README.md @@ -91,6 +91,7 @@ Note: The maximum allowed time to collision is thresholded by the lookahead poin | `max_angular_accel` | Maximum allowable angular acceleration while rotating to heading, if enabled | | `max_robot_pose_search_dist` | Maximum integrated distance along the path to bound the search for the closest pose to the robot. This is set by default to the maximum costmap extent, so it shouldn't be set manually unless there are loops within the local costmap. | | `interpolate_curvature_after_goal` | Needs use_fixed_curvature_lookahead to be true. Interpolate a carrot after the goal dedicated to the curvature calculation (to avoid oscillations at the end of the path) | +| `min_distance_to_obstacle` | The shortest distance at which the robot is allowed to be from an obstacle along its trajectory. Set <= 0.0 to disable. It is limited to maximum distance of lookahead distance selected. | Example fully-described XML with default parameter values: diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp index da5950d9736..e3090df2cef 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp @@ -42,6 +42,7 @@ struct Parameters double min_approach_linear_velocity; double approach_velocity_scaling_dist; double max_allowed_time_to_collision_up_to_carrot; + double min_distance_to_obstacle; bool use_regulated_linear_velocity_scaling; bool use_cost_regulated_linear_velocity_scaling; double cost_scaling_dist; diff --git a/nav2_regulated_pure_pursuit_controller/src/collision_checker.cpp b/nav2_regulated_pure_pursuit_controller/src/collision_checker.cpp index ac654fcab76..89a5a2a1bff 100644 --- a/nav2_regulated_pure_pursuit_controller/src/collision_checker.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/collision_checker.cpp @@ -92,8 +92,16 @@ bool CollisionChecker::isCollisionImminent( curr_pose.theta = tf2::getYaw(robot_pose.pose.orientation); // only forward simulate within time requested + double max_allowed_time_to_collision_check = params_->max_allowed_time_to_collision_up_to_carrot; + if (params_->min_distance_to_obstacle > 0.0) { + max_allowed_time_to_collision_check = std::max( + params_->max_allowed_time_to_collision_up_to_carrot, + params_->min_distance_to_obstacle / std::max(std::abs(linear_vel), + params_->min_approach_linear_velocity) + ); + } int i = 1; - while (i * projection_time < params_->max_allowed_time_to_collision_up_to_carrot) { + while (i * projection_time < max_allowed_time_to_collision_check) { i++; // apply velocity at curr_pose over distance diff --git a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp index 293a6dace2e..5a346c23c1d 100644 --- a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp @@ -61,6 +61,9 @@ ParameterHandler::ParameterHandler( declare_parameter_if_not_declared( node, plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot", rclcpp::ParameterValue(1.0)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".min_distance_to_obstacle", + rclcpp::ParameterValue(-1.0)); declare_parameter_if_not_declared( node, plugin_name_ + ".use_regulated_linear_velocity_scaling", rclcpp::ParameterValue(true)); declare_parameter_if_not_declared( @@ -131,6 +134,9 @@ ParameterHandler::ParameterHandler( node->get_parameter( plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot", params_.max_allowed_time_to_collision_up_to_carrot); + node->get_parameter( + plugin_name_ + ".min_distance_to_obstacle", + params_.min_distance_to_obstacle); node->get_parameter( plugin_name_ + ".use_regulated_linear_velocity_scaling", params_.use_regulated_linear_velocity_scaling); @@ -278,6 +284,8 @@ ParameterHandler::updateParametersCallback( params_.curvature_lookahead_dist = parameter.as_double(); } else if (param_name == plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot") { params_.max_allowed_time_to_collision_up_to_carrot = parameter.as_double(); + } else if (param_name == plugin_name_ + ".min_distance_to_obstacle") { + params_.min_distance_to_obstacle = parameter.as_double(); } else if (param_name == plugin_name_ + ".cost_scaling_dist") { params_.cost_scaling_dist = parameter.as_double(); } else if (param_name == plugin_name_ + ".cost_scaling_gain") { diff --git a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp index 0ca4527c2ee..c6c0f32ca40 100644 --- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp +++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp @@ -720,6 +720,7 @@ TEST(RegulatedPurePursuitTest, testDynamicParameter) rclcpp::Parameter("test.rotate_to_heading_angular_vel", 18.0), rclcpp::Parameter("test.min_approach_linear_velocity", 1.0), rclcpp::Parameter("test.max_allowed_time_to_collision_up_to_carrot", 2.0), + rclcpp::Parameter("test.min_distance_to_obstacle", 2.0), rclcpp::Parameter("test.cost_scaling_dist", 2.0), rclcpp::Parameter("test.cost_scaling_gain", 4.0), rclcpp::Parameter("test.regulated_linear_scaling_min_radius", 10.0), @@ -749,6 +750,7 @@ TEST(RegulatedPurePursuitTest, testDynamicParameter) EXPECT_EQ( node->get_parameter( "test.max_allowed_time_to_collision_up_to_carrot").as_double(), 2.0); + EXPECT_EQ(node->get_parameter("test.min_distance_to_obstacle").as_double(), 2.0); EXPECT_EQ(node->get_parameter("test.cost_scaling_dist").as_double(), 2.0); EXPECT_EQ(node->get_parameter("test.cost_scaling_gain").as_double(), 4.0); EXPECT_EQ(node->get_parameter("test.regulated_linear_scaling_min_radius").as_double(), 10.0); From b1359aa1574c74904b2bb6fbe5a80ee80eb4ecea Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 10 Jun 2025 20:42:16 -0700 Subject: [PATCH 35/46] Fixing builds for message filters API change while retaining Jazzy, Kilted, and Rolling support (#5251) * Update amcl_node.hpp Signed-off-by: Steve Macenski * Update amcl_node.cpp Signed-off-by: Steve Macenski * Working for Kilted, Jazzy Signed-off-by: Steve Macenski * Update amcl_node.cpp Signed-off-by: Steve Macenski * Update amcl_node.cpp Signed-off-by: Steve Macenski * Update amcl_node.cpp Signed-off-by: Steve Macenski --------- Signed-off-by: Steve Macenski --- nav2_amcl/include/nav2_amcl/amcl_node.hpp | 7 +++++ nav2_amcl/src/amcl_node.cpp | 6 ++++ .../nav2_costmap_2d/obstacle_layer.hpp | 6 ++++ nav2_costmap_2d/plugins/obstacle_layer.cpp | 28 ++++++++++++++++--- 4 files changed, 43 insertions(+), 4 deletions(-) diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp index c7341a5985a..58b48246a9d 100644 --- a/nav2_amcl/include/nav2_amcl/amcl_node.hpp +++ b/nav2_amcl/include/nav2_amcl/amcl_node.hpp @@ -29,6 +29,7 @@ #include #include "message_filters/subscriber.hpp" +#include "rclcpp/version.h" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_util/lifecycle_node.hpp" @@ -171,8 +172,14 @@ class AmclNode : public nav2_util::LifecycleNode * @brief Initialize incoming data message subscribers and filters */ void initMessageFilters(); + + // To Support Kilted and Older from Message Filters API change + #if RCLCPP_VERSION_GTE(29, 6, 0) + std::unique_ptr> laser_scan_sub_; + #else std::unique_ptr> laser_scan_sub_; + #endif std::unique_ptr> laser_scan_filter_; message_filters::Connection laser_scan_connection_; diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 71b50bdcc79..953e5e05509 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -1523,9 +1523,15 @@ AmclNode::initMessageFilters() { auto sub_opt = rclcpp::SubscriptionOptions(); sub_opt.callback_group = callback_group_; + + #if RCLCPP_VERSION_GTE(29, 6, 0) + laser_scan_sub_ = std::make_unique>( + shared_from_this(), scan_topic_, rclcpp::SensorDataQoS(), sub_opt); + #else laser_scan_sub_ = std::make_unique>( shared_from_this(), scan_topic_, rmw_qos_profile_sensor_data, sub_opt); + #endif laser_scan_filter_ = std::make_unique>( *laser_scan_sub_, *tf_buffer_, odom_frame_id_, 10, diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp index 1cdec808da7..16113d61b16 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp @@ -43,6 +43,7 @@ #include #include "rclcpp/rclcpp.hpp" +#include "rclcpp/version.h" #include "laser_geometry/laser_geometry.hpp" #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wreorder" @@ -234,8 +235,13 @@ class ObstacleLayer : public CostmapLayer /// @brief Used to project laser scans into point clouds laser_geometry::LaserProjection projector_; /// @brief Used for the observation message filters + #if RCLCPP_VERSION_GTE(29, 6, 0) + std::vector> + observation_subscribers_; + #else std::vector>> observation_subscribers_; + #endif /// @brief Used to make sure that transforms are available for each sensor std::vector> observation_notifiers_; /// @brief Used to store observations from various sensors diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp index fcfa9e180cc..60847e3df1a 100644 --- a/nav2_costmap_2d/plugins/obstacle_layer.cpp +++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp @@ -231,13 +231,23 @@ void ObstacleLayer::onInitialize() // create a callback for the topic if (data_type == "LaserScan") { + // For Kilted and Older Support from Message Filters API change + #if RCLCPP_VERSION_GTE(29, 6, 0) + std::shared_ptr> sub; + #else std::shared_ptr> sub; + #endif - // For Jazzy compatibility - #if RCLCPP_VERSION_GTE(29, 0, 0) + // For Kilted compatibility in Message Filters API change + #if RCLCPP_VERSION_GTE(29, 6, 0) + sub = std::make_shared>( + node, topic, custom_qos_profile, sub_opt); + // For Jazzy compatibility in Message Filters API change + #elif RCLCPP_VERSION_GTE(29, 0, 0) sub = std::make_shared>(node, topic, custom_qos_profile, sub_opt); + // For Humble and Older compatibility in Message Filters API change #else sub = std::make_shared>( @@ -271,13 +281,23 @@ void ObstacleLayer::onInitialize() observation_notifiers_.back()->setTolerance(rclcpp::Duration::from_seconds(0.05)); } else { + // For Kilted and Older Support from Message Filters API change + #if RCLCPP_VERSION_GTE(29, 6, 0) + std::shared_ptr> sub; + #else std::shared_ptr> sub; + #endif - // For Jazzy compatibility - #if RCLCPP_VERSION_GTE(29, 0, 0) + // For Kilted compatibility in Message Filters API change + #if RCLCPP_VERSION_GTE(29, 6, 0) + sub = std::make_shared>( + node, topic, custom_qos_profile, sub_opt); + // For Jazzy compatibility in Message Filters API change + #elif RCLCPP_VERSION_GTE(29, 0, 0) sub = std::make_shared>(node, topic, custom_qos_profile, sub_opt); + // For Humble and Older compatibility in Message Filters API change #else sub = std::make_shared>( From aade6e54b003386c9f40ca43e7a496b1590a0b43 Mon Sep 17 00:00:00 2001 From: alexanderjyuen <103065090+alexanderjyuen@users.noreply.github.com> Date: Wed, 11 Jun 2025 19:53:43 -0700 Subject: [PATCH 36/46] Route server corner smoothing (#5226) * added edge length method Signed-off-by: Alexander Yuen * Added corner arc class Signed-off-by: Alexander Yuen * replaced double vectors with Coordinates, added methods to return start and end coordinates Signed-off-by: Alexander Yuen * using Coordinates, fixed direction of tangents Signed-off-by: Alexander Yuen * added corner arc in header, added logger in protected variable Signed-off-by: Alexander Yuen * first pass of corner smoothing algorithm Signed-off-by: Alexander Yuen * reassigning next edge to have a different start, if a corner occurs before it Signed-off-by: Alexander Yuen * using unique pointer instead of raw pointers for new edges and nodes Signed-off-by: Alexander Yuen * added smoothing parameter Signed-off-by: Alexander Yuen * made angle of interpolation a parameter Signed-off-by: Alexander Yuen * const for return methods, added flag for smoothing corners Signed-off-by: Alexander Yuen * moved getEdgeLength() into the Directional Edge struct Signed-off-by: Alexander Yuen * using float instead of double Signed-off-by: Alexander Yuen * smoothing radius is float, couple methods moved to protected Signed-off-by: Alexander Yuen * removed signed_angle_ as a member variable Signed-off-by: Alexander Yuen * removed unnecessary member variables Signed-off-by: Alexander Yuen * removed angle of interpolation and inferring it from path density and radius instead Signed-off-by: Alexander Yuen * consolidated corner arc into one header function Signed-off-by: Alexander Yuen * readded newline Signed-off-by: Alexander Yuen * changed corner arc to corner smoothing Signed-off-by: Alexander Yuen * replaced the use of edges with coordinates to generate smoothing arc, removed storage of nodes and edges Signed-off-by: Alexander Yuen * linting Signed-off-by: Alexander Yuen * fixing cpplint Signed-off-by: Alexander Yuen * linting for headers Signed-off-by: Alexander Yuen * cpplinting Signed-off-by: Alexander Yuen * Update nav2_route/src/path_converter.cpp Signed-off-by: Steve Macenski * Update nav2_route/src/path_converter.cpp Signed-off-by: Steve Macenski * Update nav2_route/src/path_converter.cpp Signed-off-by: Steve Macenski * Update nav2_route/src/path_converter.cpp Signed-off-by: Steve Macenski * Update nav2_route/include/nav2_route/corner_smoothing.hpp Signed-off-by: Steve Macenski * fixed divide by zeros and accessing empty route.edges Signed-off-by: Alexander Yuen * uncrustify linting Signed-off-by: Alexander Yuen * cpp linting Signed-off-by: Alexander Yuen * path converter linting Signed-off-by: Alexander Yuen * changed all doubles to floats Signed-off-by: Alexander Yuen * added check for edges that are colinear to avoid divide by 0, fixed final edge interpolation Signed-off-by: Alexander Yuen * linting Signed-off-by: Alexander Yuen * Update nav2_route/include/nav2_route/corner_smoothing.hpp Signed-off-by: Steve Macenski * added doxygen for corner arc class Signed-off-by: Alexander Yuen * added warning message if corner can't be smoothed Signed-off-by: Alexander Yuen * added smooth_corners to the nav2 params file Signed-off-by: Alexander Yuen * added smoothing flag and radius parameter to README.md' Signed-off-by: Alexander Yuen * typo in README Signed-off-by: Alexander Yuen * added testing for corner smoothing Signed-off-by: Alexander Yuen * Update nav2_route/include/nav2_route/corner_smoothing.hpp Signed-off-by: Steve Macenski --------- Signed-off-by: Alexander Yuen Signed-off-by: Steve Macenski Co-authored-by: Steve Macenski --- nav2_bringup/params/nav2_params.yaml | 1 + nav2_route/README.md | 2 + .../include/nav2_route/corner_smoothing.hpp | 176 ++++++++++++++++++ .../include/nav2_route/path_converter.hpp | 4 + nav2_route/include/nav2_route/types.hpp | 9 + nav2_route/src/path_converter.cpp | 52 +++++- nav2_route/test/CMakeLists.txt | 8 + nav2_route/test/test_corner_smoothing.cpp | 115 ++++++++++++ 8 files changed, 361 insertions(+), 6 deletions(-) create mode 100644 nav2_route/include/nav2_route/corner_smoothing.hpp create mode 100644 nav2_route/test/test_corner_smoothing.cpp diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index f1d7fb2169b..cfbcdc858bf 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -447,6 +447,7 @@ route_server: # graph_filepath: $(find-pkg-share nav2_route)/graphs/aws_graph.geojson boundary_radius_to_achieve_node: 1.0 radius_to_achieve_node: 2.0 + smooth_corners: true operations: ["AdjustSpeedLimit", "ReroutingService", "CollisionMonitor"] ReroutingService: plugin: "nav2_route::ReroutingService" diff --git a/nav2_route/README.md b/nav2_route/README.md index a434276bff8..c0eb510b1db 100644 --- a/nav2_route/README.md +++ b/nav2_route/README.md @@ -114,6 +114,8 @@ route_server: path_density: 0.05 # Density of points for generating the dense nav_msgs/Path from route (m) max_iterations: 0 # Maximum number of search iterations, if 0, uses maximum possible max_planning_time: 2.0 # Maximum planning time (seconds) + smooth_corners: true # Whether to smooth corners formed by adjacent edges or not + smoothing_radius: 1.0 # Radius of corner to fit into the corner costmap_topic: 'global_costmap/costmap_raw' # Costmap topic when enable_nn_search is enabled. May also be used by the collision monitor operation and/or the costmap edge scorer if using the same topic to share resources. graph_file_loader: "GeoJsonGraphFileLoader" # Name of default file loader diff --git a/nav2_route/include/nav2_route/corner_smoothing.hpp b/nav2_route/include/nav2_route/corner_smoothing.hpp new file mode 100644 index 00000000000..f14d2f43a41 --- /dev/null +++ b/nav2_route/include/nav2_route/corner_smoothing.hpp @@ -0,0 +1,176 @@ +// Copyright (c) 2025, Polymath Robotics +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_ROUTE__CORNER_SMOOTHING_HPP_ +#define NAV2_ROUTE__CORNER_SMOOTHING_HPP_ + +#include +#include +#include + +#include "nav2_route/types.hpp" +#include "nav2_route/utils.hpp" + +namespace nav2_route +{ +/** + * @class nav2_route::CornerArc + * @brief A class used to smooth corners defined by the edges and nodes + * of the route graph. Used with path converter to output a corner smoothed plan + */ +class CornerArc +{ +public: + /** + * @brief A constructor for nav2_route::CornerArc + * @param start start coordinate of corner to be smoothed + * @param corner corner coordinate of corner to be smoothed + * @param end end coordinate of corner to be smoothed + * @param minimum_radius smoothing radius to fit to the corner + */ + CornerArc( + const Coordinates & start, const Coordinates & corner, const Coordinates & end, + float minimum_radius) + { + start_edge_length_ = hypotf(corner.x - start.x, corner.y - start.y); + end_edge_length_ = hypotf(end.x - corner.x, end.y - corner.y); + + // invalid scenario would cause equations to blow up + if (start_edge_length_ == 0.0 || end_edge_length_ == 0.0) {return;} + + float angle = getAngleBetweenEdges(start, corner, end); + + // cannot smooth a 0 (back and forth) or 180 (straight line) angle + if (std::abs(angle) < 1E-6 || std::abs(angle - M_PI) < 1E-6) {return;} + + float tangent_length = minimum_radius / (std::tan(std::fabs(angle) / 2)); + + if (tangent_length < start_edge_length_ && tangent_length < end_edge_length_) { + std::vector start_edge_unit_tangent = + {(start.x - corner.x) / start_edge_length_, (start.y - corner.y) / start_edge_length_}; + std::vector end_edge_unit_tangent = + {(end.x - corner.x) / end_edge_length_, (end.y - corner.y) / end_edge_length_}; + + float bisector_x = start_edge_unit_tangent[0] + end_edge_unit_tangent[0]; + float bisector_y = start_edge_unit_tangent[1] + end_edge_unit_tangent[1]; + float bisector_magnitude = std::sqrt(bisector_x * bisector_x + bisector_y * bisector_y); + + std::vector unit_bisector = + {bisector_x / bisector_magnitude, bisector_y / bisector_magnitude}; + + start_coordinate_.x = corner.x + start_edge_unit_tangent[0] * tangent_length; + start_coordinate_.y = corner.y + start_edge_unit_tangent[1] * tangent_length; + + end_coordinate_.x = corner.x + end_edge_unit_tangent[0] * tangent_length; + end_coordinate_.y = corner.y + end_edge_unit_tangent[1] * tangent_length; + + float bisector_length = minimum_radius / std::sin(angle / 2); + + circle_center_coordinate_.x = corner.x + unit_bisector[0] * bisector_length; + circle_center_coordinate_.y = corner.y + unit_bisector[1] * bisector_length; + + valid_corner_ = true; + } + } + + /** + * @brief A destructor for nav2_route::CornerArc + */ + ~CornerArc() = default; + + /** + * @brief interpolates the arc for a path of certain density + * @param max_angle_resolution Resolution to interpolate path to + * @param poses Pose output + */ + void interpolateArc( + const float & max_angle_resolution, + std::vector & poses) + { + std::vector r_start{start_coordinate_.x - circle_center_coordinate_.x, + start_coordinate_.y - circle_center_coordinate_.y}; + std::vector r_end{end_coordinate_.x - circle_center_coordinate_.x, + end_coordinate_.y - circle_center_coordinate_.y}; + float cross = r_start[0] * r_end[1] - r_start[1] * r_end[0]; + float dot = r_start[0] * r_end[0] + r_start[1] * r_end[1]; + float signed_angle = std::atan2(cross, dot); + // lower limit for N is 1 to protect against divide by 0 + int N = std::max(1, static_cast(std::ceil(std::abs(signed_angle) / max_angle_resolution))); + float angle_resolution = signed_angle / N; + + float x, y; + for (int i = 0; i <= N; i++) { + float angle = i * angle_resolution; + x = circle_center_coordinate_.x + + (r_start[0] * std::cos(angle) - r_start[1] * std::sin(angle)); + y = circle_center_coordinate_.y + + (r_start[0] * std::sin(angle) + r_start[1] * std::cos(angle)); + poses.push_back(utils::toMsg(x, y)); + } + } + + /** + * @brief return if a valid corner arc (one that doesn't overrun the edge lengths) is generated + * @return if the a corner was able to be generated + */ + bool isCornerValid() const {return valid_corner_;} + + /** + * @brief return the start coordinate of the corner arc + * @return start coordinate of the arc + */ + Coordinates getCornerStart() const {return start_coordinate_;} + + /** + * @brief return the end coordinate of the corner arc + * @return end coordinate of the arc + */ + Coordinates getCornerEnd() const {return end_coordinate_;} + +protected: + /** + * @brief find the signed angle between a corner generated by 3 points + * @param start start coordinate of corner to be smoothed + * @param corner corner coordinate of corner to be smoothed + * @param end end coordinate of corner to be smoothed + * @return signed angle of the corner + */ + float getAngleBetweenEdges( + const Coordinates & start, const Coordinates & corner, + const Coordinates & end) + { + float start_dx = start.x - corner.x; + float start_dy = start.y - corner.y; + + float end_dx = end.x - corner.x; + float end_dy = end.y - corner.y; + + float angle = + acos((start_dx * end_dx + start_dy * end_dy) / (start_edge_length_ * end_edge_length_)); + + return angle; + } + +private: + bool valid_corner_{false}; + float start_edge_length_; + float end_edge_length_; + Coordinates start_coordinate_; + Coordinates end_coordinate_; + Coordinates circle_center_coordinate_; +}; + +} // namespace nav2_route + +#endif // NAV2_ROUTE__CORNER_SMOOTHING_HPP_ diff --git a/nav2_route/include/nav2_route/path_converter.hpp b/nav2_route/include/nav2_route/path_converter.hpp index 90b61898160..c4454f768ba 100644 --- a/nav2_route/include/nav2_route/path_converter.hpp +++ b/nav2_route/include/nav2_route/path_converter.hpp @@ -26,6 +26,7 @@ #include "nav2_util/node_utils.hpp" #include "nav2_route/types.hpp" #include "nav2_route/utils.hpp" +#include "nav2_route/corner_smoothing.hpp" namespace nav2_route { @@ -80,7 +81,10 @@ class PathConverter protected: rclcpp_lifecycle::LifecyclePublisher::SharedPtr path_pub_; + rclcpp::Logger logger_{rclcpp::get_logger("PathConverter")}; float density_; + float smoothing_radius_; + bool smooth_corners_; }; } // namespace nav2_route diff --git a/nav2_route/include/nav2_route/types.hpp b/nav2_route/include/nav2_route/types.hpp index dc31c5e01da..0f021a0e23f 100644 --- a/nav2_route/include/nav2_route/types.hpp +++ b/nav2_route/include/nav2_route/types.hpp @@ -138,6 +138,8 @@ struct DirectionalEdge EdgeCost edge_cost; // Cost information associated with edge Metadata metadata; // Any metadata stored in the graph file of interest Operations operations; // Operations to perform related to the edge + + float getEdgeLength(); }; typedef DirectionalEdge * EdgePtr; @@ -194,6 +196,13 @@ struct Node } }; +inline float DirectionalEdge::getEdgeLength() +{ + return hypotf( + end->coords.x - start->coords.x, + end->coords.y - start->coords.y); +} + /** * @struct nav2_route::Route * @brief An ordered set of nodes and edges corresponding to the planned route diff --git a/nav2_route/src/path_converter.cpp b/nav2_route/src/path_converter.cpp index 38adea20c25..85607a5491b 100644 --- a/nav2_route/src/path_converter.cpp +++ b/nav2_route/src/path_converter.cpp @@ -30,9 +30,16 @@ void PathConverter::configure(nav2_util::LifecycleNode::SharedPtr node) nav2_util::declare_parameter_if_not_declared( node, "path_density", rclcpp::ParameterValue(0.05)); density_ = static_cast(node->get_parameter("path_density").as_double()); + nav2_util::declare_parameter_if_not_declared( + node, "smoothing_radius", rclcpp::ParameterValue(1.0)); + smoothing_radius_ = static_cast(node->get_parameter("smoothing_radius").as_double()); + nav2_util::declare_parameter_if_not_declared( + node, "smooth_corners", rclcpp::ParameterValue(false)); + smooth_corners_ = node->get_parameter("smooth_corners").as_bool(); path_pub_ = node->create_publisher("plan", 1); path_pub_->on_activate(); + logger_ = node->get_logger(); } nav_msgs::msg::Path PathConverter::densify( @@ -54,17 +61,50 @@ nav_msgs::msg::Path PathConverter::densify( interpolateEdge(start.x, start.y, end.x, end.y, path.poses); } - // Fill in path via route edges - for (unsigned int i = 0; i != route.edges.size(); i++) { - const EdgePtr edge = route.edges[i]; - const Coordinates & start = edge->start->coords; - const Coordinates & end = edge->end->coords; - interpolateEdge(start.x, start.y, end.x, end.y, path.poses); + Coordinates start; + Coordinates end; + + if (!route.edges.empty()) { + start = route.edges[0]->start->coords; + + // Fill in path via route edges + for (unsigned int i = 0; i < route.edges.size() - 1; i++) { + const EdgePtr edge = route.edges[i]; + const EdgePtr & next_edge = route.edges[i + 1]; + end = edge->end->coords; + + CornerArc corner_arc(start, end, next_edge->end->coords, smoothing_radius_); + if (corner_arc.isCornerValid() && smooth_corners_) { + // if an arc exists, end of the first edge is the start of the arc + end = corner_arc.getCornerStart(); + + // interpolate to start of arc + interpolateEdge(start.x, start.y, end.x, end.y, path.poses); + + // interpolate arc + corner_arc.interpolateArc(density_ / smoothing_radius_, path.poses); + + // new start of next edge is end of smoothing arc + start = corner_arc.getCornerEnd(); + } else { + if (smooth_corners_) { + RCLCPP_WARN( + logger_, "Unable to smooth corner between edge %i and edge %i", edge->edgeid, + next_edge->edgeid); + } + interpolateEdge(start.x, start.y, end.x, end.y, path.poses); + start = end; + } + } } if (route.edges.empty()) { path.poses.push_back(utils::toMsg(route.start_node->coords.x, route.start_node->coords.y)); } else { + interpolateEdge( + start.x, start.y, route.edges.back()->end->coords.x, + route.edges.back()->end->coords.y, path.poses); + path.poses.push_back( utils::toMsg(route.edges.back()->end->coords.x, route.edges.back()->end->coords.y)); } diff --git a/nav2_route/test/CMakeLists.txt b/nav2_route/test/CMakeLists.txt index 186c93dbfca..6ff732b35e4 100644 --- a/nav2_route/test/CMakeLists.txt +++ b/nav2_route/test/CMakeLists.txt @@ -127,3 +127,11 @@ ament_add_gtest(test_goal_intent_search target_link_libraries(test_goal_intent_search ${library_name} ) + +# Test corner smoothing +ament_add_gtest(test_corner_smoothing + test_corner_smoothing.cpp +) +target_link_libraries(test_corner_smoothing + ${library_name} +) diff --git a/nav2_route/test/test_corner_smoothing.cpp b/nav2_route/test/test_corner_smoothing.cpp new file mode 100644 index 00000000000..928ab07f3cd --- /dev/null +++ b/nav2_route/test/test_corner_smoothing.cpp @@ -0,0 +1,115 @@ +// Copyright (c) 2025, Polymath Robotics +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#include +#include +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_route/corner_smoothing.hpp" +// #include "nav2_route/types.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +using namespace nav2_route; // NOLINT + +TEST(CornerSmoothingTest, test_corner_smoothing) +{ + Node test_node1, test_node2, test_node3; + test_node1.nodeid = 0; + test_node1.coords.x = 0.0; + test_node1.coords.y = 0.0; + test_node2.nodeid = 1; + test_node2.coords.x = 10.0; + test_node2.coords.y = 0.0; + test_node3.nodeid = 2; + test_node3.coords.x = 10.0; + test_node3.coords.y = 10.0; + + double smoothing_radius = 2.0; + + CornerArc corner_arc(test_node1.coords, test_node2.coords, test_node3.coords, smoothing_radius); + + Coordinates start = corner_arc.getCornerStart(); + Coordinates end = corner_arc.getCornerEnd(); + + EXPECT_TRUE(corner_arc.isCornerValid()); + EXPECT_EQ(start.x, 8.0); + EXPECT_EQ(start.y, 0.0); + EXPECT_EQ(end.x, 10.0); + EXPECT_EQ(end.y, 2.0); +} + +TEST(LargeRadiusTest, test_large_radius_smoothing) +{ + Node test_node1, test_node2, test_node3; + test_node1.nodeid = 0; + test_node1.coords.x = 0.0; + test_node1.coords.y = 0.0; + test_node2.nodeid = 1; + test_node2.coords.x = 10.0; + test_node2.coords.y = 0.0; + test_node3.nodeid = 2; + test_node3.coords.x = 10.0; + test_node3.coords.y = 10.0; + + double smoothing_radius = 20.0; + + CornerArc corner_arc(test_node1.coords, test_node2.coords, test_node3.coords, smoothing_radius); + + EXPECT_FALSE(corner_arc.isCornerValid()); +} + +TEST(ColinearSmoothingTest, test_colinear_smoothing) +{ + Node test_node1, test_node2, test_node3; + test_node1.nodeid = 0; + test_node1.coords.x = 0.0; + test_node1.coords.y = 0.0; + test_node2.nodeid = 1; + test_node2.coords.x = 10.0; + test_node2.coords.y = 0.0; + test_node3.nodeid = 2; + test_node3.coords.x = 20.0; + test_node3.coords.y = 0.0; + + double smoothing_radius = 2.0; + + CornerArc corner_arc(test_node1.coords, test_node2.coords, test_node3.coords, smoothing_radius); + + EXPECT_FALSE(corner_arc.isCornerValid()); +} + +TEST(DegeneratePointsTest, test_degenerate_points_smoothing) +{ + Node test_node1; + test_node1.nodeid = 0; + test_node1.coords.x = 0.0; + test_node1.coords.y = 0.0; + + double smoothing_radius = 2.0; + + CornerArc corner_arc(test_node1.coords, test_node1.coords, test_node1.coords, smoothing_radius); + + EXPECT_FALSE(corner_arc.isCornerValid()); +} From 30971bc28cb56cd10fae48a9bc2c16e8e348e661 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 12 Jun 2025 15:31:57 +0200 Subject: [PATCH 37/46] Conserve curvature with LIMIT action (#5255) * Conserve curvature with LIMIT action Signed-off-by: Tony Najjar * fix format Signed-off-by: Tony Najjar * fix test Signed-off-by: Tony Najjar --------- Signed-off-by: Tony Najjar --- .../src/collision_monitor_node.cpp | 14 +++++++++----- .../test/collision_monitor_node_test.cpp | 12 ++++++++---- 2 files changed, 17 insertions(+), 9 deletions(-) diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 472a7d255e3..420d0268bbf 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -548,13 +548,17 @@ bool CollisionMonitor::processStopSlowdownLimit( const double linear_vel = std::hypot(velocity.x, velocity.y); // absolute Velocity safe_vel; double ratio = 1.0; + + // Calculate the most restrictive ratio to preserve curvature if (linear_vel != 0.0) { - ratio = std::clamp(polygon->getLinearLimit() / linear_vel, 0.0, 1.0); + ratio = std::min(ratio, polygon->getLinearLimit() / linear_vel); + } + if (velocity.tw != 0.0) { + ratio = std::min(ratio, polygon->getAngularLimit() / std::abs(velocity.tw)); } - safe_vel.x = velocity.x * ratio; - safe_vel.y = velocity.y * ratio; - safe_vel.tw = std::clamp( - velocity.tw, -polygon->getAngularLimit(), polygon->getAngularLimit()); + ratio = std::clamp(ratio, 0.0, 1.0); + // Apply the same ratio to all components to preserve curvature + safe_vel = velocity * ratio; // Check that currently calculated velocity is safer than // chosen for previous shapes one if (safe_vel < robot_action.req_vel) { diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp index 0de57bd895e..2fda8571995 100644 --- a/nav2_collision_monitor/test/collision_monitor_node_test.cpp +++ b/nav2_collision_monitor/test/collision_monitor_node_test.cpp @@ -821,10 +821,12 @@ TEST_F(Tester, testProcessStopSlowdownLimit) publishCmdVel(0.5, 0.2, 0.1); ASSERT_TRUE(waitCmdVel(500ms)); const double speed = std::sqrt(0.5 * 0.5 + 0.2 * 0.2); - const double ratio = LINEAR_LIMIT / speed; + const double linear_ratio = LINEAR_LIMIT / speed; + const double angular_ratio = ANGULAR_LIMIT / 0.1; + const double ratio = std::min(linear_ratio, angular_ratio); ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5 * ratio, EPSILON); ASSERT_NEAR(cmd_vel_out_->linear.y, 0.2 * ratio, EPSILON); - ASSERT_NEAR(cmd_vel_out_->angular.z, 0.09, EPSILON); + ASSERT_NEAR(cmd_vel_out_->angular.z, 0.1 * ratio, EPSILON); ASSERT_TRUE(waitActionState(500ms)); ASSERT_EQ(action_state_->action_type, LIMIT); ASSERT_EQ(action_state_->polygon_name, "Limit"); @@ -906,10 +908,12 @@ TEST_F(Tester, testPolygonSource) publishCmdVel(0.5, 0.2, 0.1); EXPECT_TRUE(waitCmdVel(500ms)); const double speed = std::sqrt(0.5 * 0.5 + 0.2 * 0.2); - const double ratio = LINEAR_LIMIT / speed; + const double linear_ratio = LINEAR_LIMIT / speed; + const double angular_ratio = ANGULAR_LIMIT / 0.1; + const double ratio = std::min(linear_ratio, angular_ratio); EXPECT_NEAR(cmd_vel_out_->linear.x, 0.5 * ratio, EPSILON); EXPECT_NEAR(cmd_vel_out_->linear.y, 0.2 * ratio, EPSILON); - EXPECT_NEAR(cmd_vel_out_->angular.z, 0.09, EPSILON); + EXPECT_NEAR(cmd_vel_out_->angular.z, 0.1 * ratio, EPSILON); EXPECT_TRUE(waitActionState(500ms)); EXPECT_EQ(action_state_->action_type, LIMIT); EXPECT_EQ(action_state_->polygon_name, "Limit"); From 4c7f8dc61fde8fdc7de6531627207b6ff5530dc2 Mon Sep 17 00:00:00 2001 From: Marco Bassa <101661130+MarcoMatteoBassa@users.noreply.github.com> Date: Thu, 12 Jun 2025 16:33:42 +0200 Subject: [PATCH 38/46] Parametrizing obstacle layer tf filter tolerance (#5261) Signed-off-by: Marco Bassa --- nav2_costmap_2d/plugins/obstacle_layer.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp index 60847e3df1a..e3181acdf78 100644 --- a/nav2_costmap_2d/plugins/obstacle_layer.cpp +++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp @@ -46,6 +46,7 @@ #include "pluginlib/class_list_macros.hpp" #include "sensor_msgs/point_cloud2_iterator.hpp" #include "nav2_costmap_2d/costmap_math.hpp" +#include "nav2_util/node_utils.hpp" #include "rclcpp/version.h" PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::ObstacleLayer, nav2_costmap_2d::Layer) @@ -100,6 +101,8 @@ void ObstacleLayer::onInitialize() node->get_parameter("track_unknown_space", track_unknown_space); node->get_parameter("transform_tolerance", transform_tolerance); node->get_parameter(name_ + "." + "observation_sources", topics_string); + double tf_filter_tolerance = nav2_util::declare_or_get_parameter(node, name_ + "." + + "tf_filter_tolerance", 0.05); int combination_method_param{}; node->get_parameter(name_ + "." + "combination_method", combination_method_param); @@ -278,7 +281,8 @@ void ObstacleLayer::onInitialize() observation_subscribers_.push_back(sub); observation_notifiers_.push_back(filter); - observation_notifiers_.back()->setTolerance(rclcpp::Duration::from_seconds(0.05)); + observation_notifiers_.back()->setTolerance(rclcpp::Duration::from_seconds( + tf_filter_tolerance)); } else { // For Kilted and Older Support from Message Filters API change From 2966e34638cf9d051109e2dff533a20dfbd2bc5a Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Fri, 13 Jun 2025 23:36:58 +0800 Subject: [PATCH 39/46] Add namespace support for rviz costmap cost tool (#5268) Signed-off-by: Maurice-1235 --- nav2_rviz_plugins/src/costmap_cost_tool.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_rviz_plugins/src/costmap_cost_tool.cpp b/nav2_rviz_plugins/src/costmap_cost_tool.cpp index 47539607ebb..1f83425b064 100644 --- a/nav2_rviz_plugins/src/costmap_cost_tool.cpp +++ b/nav2_rviz_plugins/src/costmap_cost_tool.cpp @@ -54,12 +54,12 @@ void CostmapCostTool::onInitialize() rclcpp::Node::SharedPtr node = node_ptr_->get_raw_node(); local_cost_client_ = std::make_shared>( - "/local_costmap/get_cost_local_costmap", + "local_costmap/get_cost_local_costmap", node, false /* Does not create and spin an internal executor*/); global_cost_client_ = std::make_shared>( - "/global_costmap/get_cost_global_costmap", + "global_costmap/get_cost_global_costmap", node, false /* Does not create and spin an internal executor*/); } From ec95308fc05d0f5513b07a6e94f3bd662a5ea69a Mon Sep 17 00:00:00 2001 From: Stevedan Ogochukwu Omodolor <61468301+stevedanomodolor@users.noreply.github.com> Date: Sun, 15 Jun 2025 23:54:34 +0200 Subject: [PATCH 40/46] Fix/smac planner orientation goals (#5235) * cherry pick Signed-off-by: Stevedan Omodolor * cherry pick 6a74ba61ca0ea117b2995dd0c8e266a5ec9741ba Signed-off-by: Stevedan Omodolor * cherrpy pick Signed-off-by: Stevedan Omodolor * include x11 forwarding Signed-off-by: Stevedan Omodolor * kind of working version Signed-off-by: Stevedan Omodolor * cleanup Signed-off-by: Stevedan Omodolor * formatting Signed-off-by: Stevedan Omodolor * minor format change Signed-off-by: Stevedan Omodolor * change naming Signed-off-by: Stevedan Omodolor * minor changes Signed-off-by: Stevedan Omodolor * working with new changes Signed-off-by: Stevedan Omodolor * Revert "Fix Ci from key signing (#5220)" (#5237) * Revert "Fix Ci from key signing (#5220)" This reverts the changes to the Dockerfile done in 1345c226bcf73068127b2e2efaee91a637d2f351. Signed-off-by: Nils-Christian Iseke * Update Cache Version Signed-off-by: Nils-Christian Iseke --------- Signed-off-by: Nils-Christian Iseke Signed-off-by: Stevedan Omodolor * Revert back Signed-off-by: Stevedan Omodolor * enable_groot_monitoring_ false (#5246) Signed-off-by: Guillaume Doisy Co-authored-by: Guillaume Doisy Signed-off-by: Stevedan Omodolor * Updating readme table for kilted release (#5249) * updating readme table for kilted release Signed-off-by: Steve Macenski * Updating table lint Signed-off-by: Steve Macenski --------- Signed-off-by: Steve Macenski Signed-off-by: Stevedan Omodolor * Add min_distance_to_obstacle parameter to RPP (#4543) * min_distance_to_obstacle Signed-off-by: Guillaume Doisy * suggestion to time base and combine Signed-off-by: Guillaume Doisy * typo Signed-off-by: Guillaume Doisy * use min_approach_linear_velocity Signed-off-by: Guillaume Doisy --------- Signed-off-by: Guillaume Doisy Co-authored-by: Guillaume Doisy Signed-off-by: Stevedan Omodolor * Fixing builds for message filters API change while retaining Jazzy, Kilted, and Rolling support (#5251) * Update amcl_node.hpp Signed-off-by: Steve Macenski * Update amcl_node.cpp Signed-off-by: Steve Macenski * Working for Kilted, Jazzy Signed-off-by: Steve Macenski * Update amcl_node.cpp Signed-off-by: Steve Macenski * Update amcl_node.cpp Signed-off-by: Steve Macenski * Update amcl_node.cpp Signed-off-by: Steve Macenski --------- Signed-off-by: Steve Macenski Signed-off-by: Stevedan Omodolor * Change max_cost default to 254 (#5256) Signed-off-by: Tony Najjar Signed-off-by: Stevedan Omodolor * linter Signed-off-by: Stevedan Omodolor * remove const Signed-off-by: Stevedan Omodolor * pass const pointer by value Signed-off-by: Stevedan Omodolor * pass const pointer by value Signed-off-by: Stevedan Omodolor * remove unused param Signed-off-by: Stevedan Omodolor --------- Signed-off-by: Stevedan Omodolor Signed-off-by: Nils-Christian Iseke Signed-off-by: Guillaume Doisy Signed-off-by: Steve Macenski Signed-off-by: Tony Najjar Co-authored-by: Steve Macenski Co-authored-by: Nils-Christian Iseke <48475933+Nils-ChristianIseke@users.noreply.github.com> Co-authored-by: Guillaume Doisy Co-authored-by: Guillaume Doisy Co-authored-by: Tony Najjar --- .devcontainer/devcontainer.json | 4 + .../include/nav2_smac_planner/a_star.hpp | 46 +- .../nav2_smac_planner/analytic_expansion.hpp | 73 +- .../include/nav2_smac_planner/constants.hpp | 35 + .../nav2_smac_planner/goal_manager.hpp | 218 + .../include/nav2_smac_planner/node_2d.hpp | 22 +- .../include/nav2_smac_planner/node_hybrid.hpp | 6 +- .../nav2_smac_planner/node_lattice.hpp | 6 +- .../nav2_smac_planner/smac_planner_hybrid.hpp | 2 + .../smac_planner_lattice.hpp | 2 + .../include/nav2_smac_planner/types.hpp | 11 + .../sample_primitives/test/output.json | 3931 +++++++++++++++++ nav2_smac_planner/src/a_star.cpp | 143 +- nav2_smac_planner/src/analytic_expansion.cpp | 277 +- nav2_smac_planner/src/node_2d.cpp | 6 +- nav2_smac_planner/src/node_hybrid.cpp | 14 +- nav2_smac_planner/src/node_lattice.cpp | 15 +- nav2_smac_planner/src/smac_planner_hybrid.cpp | 76 +- .../src/smac_planner_lattice.cpp | 73 +- nav2_smac_planner/test/CMakeLists.txt | 10 + nav2_smac_planner/test/test_a_star.cpp | 147 +- nav2_smac_planner/test/test_goal_manager.cpp | 186 + nav2_smac_planner/test/test_node2d.cpp | 4 +- nav2_smac_planner/test/test_smac_hybrid.cpp | 108 +- nav2_smac_planner/test/test_smac_lattice.cpp | 84 +- 25 files changed, 5329 insertions(+), 170 deletions(-) create mode 100644 nav2_smac_planner/include/nav2_smac_planner/goal_manager.hpp create mode 100644 nav2_smac_planner/lattice_primitives/sample_primitives/test/output.json create mode 100644 nav2_smac_planner/test/test_goal_manager.cpp diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 01731dda48c..0a64abd703a 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -14,6 +14,8 @@ // "--pid=host", // DDS discovery with host, without --network=host // "--privileged", // device access to host peripherals, e.g. USB // "--security-opt=seccomp=unconfined", // enable debugging, e.g. gdb + // "--volume=/tmp/.X11-unix:/tmp/.X11-unix", // X11 socket for GUI applications + // "--gpus=all" // access to all GPUs, e.g. for GPU-accelerated applications ], "workspaceFolder": "/opt/overlay_ws/src/navigation2", "workspaceMount": "source=${localWorkspaceFolder},target=${containerWorkspaceFolder},type=bind", @@ -23,6 +25,8 @@ "remoteEnv": { "OVERLAY_MIXINS": "release ccache lld", "CCACHE_DIR": "/tmp/.ccache" + // "QT_X11_NO_MITSHM": "1", // disable MIT-SHM for X11 forwarding + // "DISPLAY": "${localEnv:DISPLAY}", // X11 forwarding }, "mounts": [ { diff --git a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp index 8b127960cc3..a4720b3c5b2 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp @@ -32,6 +32,7 @@ #include "nav2_smac_planner/node_hybrid.hpp" #include "nav2_smac_planner/node_lattice.hpp" #include "nav2_smac_planner/node_basic.hpp" +#include "nav2_smac_planner/goal_manager.hpp" #include "nav2_smac_planner/types.hpp" #include "nav2_smac_planner/constants.hpp" @@ -54,6 +55,8 @@ class AStarAlgorithm typedef typename NodeT::CoordinateVector CoordinateVector; typedef typename NodeVector::iterator NeighborIterator; typedef std::function NodeGetter; + typedef GoalManager GoalManagerT; + /** * @struct nav2_smac_planner::NodeComparator @@ -90,6 +93,8 @@ class AStarAlgorithm * or planning time exceeded * @param max_planning_time Maximum time (in seconds) to wait for a plan, createPath returns * false after this timeout + * @param lookup_table_size Size of the lookup table to store heuristic values + * @param dim_3_size Number of quantization bins */ void initialize( const bool & allow_unknown, @@ -125,11 +130,15 @@ class AStarAlgorithm * @param mx The node X index of the goal * @param my The node Y index of the goal * @param dim_3 The node dim_3 index of the goal + * @param goal_heading_mode The goal heading mode to use + * @param coarse_search_resolution The resolution to search for goal heading */ void setGoal( const float & mx, const float & my, - const unsigned int & dim_3); + const unsigned int & dim_3, + const GoalHeadingMode & goal_heading_mode = GoalHeadingMode::DEFAULT, + const int & coarse_search_resolution = 1); /** * @brief Set the starting pose for planning, as a node index @@ -154,12 +163,6 @@ class AStarAlgorithm */ NodePtr & getStart(); - /** - * @brief Get pointer reference to goal node - * @return Node pointer reference to goal node - */ - NodePtr & getGoal(); - /** * @brief Get maximum number of on-approach iterations after within threshold * @return Reference to Maximum on-approach iterations parameter @@ -190,6 +193,18 @@ class AStarAlgorithm */ unsigned int & getSizeDim3(); + /** + * @brief Get the resolution of the coarse search + * @return Size of the goals to expand + */ + unsigned int getCoarseSearchResolution(); + + /** + * @brief Get the goals manager class + * @return Goal manager class + */ + GoalManagerT getGoalManager(); + protected: /** * @brief Get pointer to next goal in open set @@ -210,13 +225,6 @@ class AStarAlgorithm */ inline NodePtr addToGraph(const uint64_t & index); - /** - * @brief Check if this node is the goal node - * @param node Node pointer to check if its the goal node - * @return if node is goal - */ - inline bool isGoal(NodePtr & node); - /** * @brief Get cost of heuristic of node * @param node Node pointer to get heuristic for @@ -240,6 +248,11 @@ class AStarAlgorithm */ inline void clearGraph(); + /** + * @brief Check if node has been visited + * @param current_node Node to check if visited + * @return if node has been visited + */ inline bool onVisitationCheckNode(const NodePtr & node); /** @@ -260,12 +273,11 @@ class AStarAlgorithm unsigned int _x_size; unsigned int _y_size; unsigned int _dim3_size; + unsigned int _coarse_search_resolution; SearchInfo _search_info; - Coordinates _goal_coordinates; NodePtr _start; - NodePtr _goal; - + GoalManagerT _goal_manager; Graph _graph; NodeQueue _queue; diff --git a/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp index 6be84d1c109..0c5cdede0bd 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp @@ -15,6 +15,10 @@ #ifndef NAV2_SMAC_PLANNER__ANALYTIC_EXPANSION_HPP_ #define NAV2_SMAC_PLANNER__ANALYTIC_EXPANSION_HPP_ +#include +#include +#include + #include #include #include @@ -35,8 +39,10 @@ class AnalyticExpansion { public: typedef NodeT * NodePtr; + typedef std::vector NodeVector; typedef typename NodeT::Coordinates Coordinates; typedef std::function NodeGetter; + typedef typename NodeT::CoordinateVector CoordinateVector; /** * @struct nav2_smac_planner::AnalyticExpansion::AnalyticExpansionNodes @@ -59,7 +65,33 @@ class AnalyticExpansion Coordinates proposed_coords; }; - typedef std::vector AnalyticExpansionNodes; + /** + * @struct AnalyticExpansionNodes + * @brief Analytic expansion nodes and associated metadata + * + * This structure holds a collection of analytic expansion nodes and the number of direction + * changes encountered during the expansion. + */ + struct AnalyticExpansionNodes + { + AnalyticExpansionNodes() = default; + + void add( + NodePtr & node, + Coordinates & initial_coords, + Coordinates & proposed_coords) + { + nodes.emplace_back(node, initial_coords, proposed_coords); + } + + void setDirectionChanges(int changes) + { + direction_changes = changes; + } + + std::vector nodes; + int direction_changes{0}; + }; /** * @brief Constructor for analytic expansion object @@ -79,17 +111,22 @@ class AnalyticExpansion /** * @brief Attempt an analytic path completion * @param node The node to start the analytic path from - * @param goal The goal node to plan to + * @param coarse_check_goals Coarse list of goals nodes to plan to + * @param fine_check_goals Fine list of goals nodes to plan to + * @param goals_coords vector of goal coordinates to plan to * @param getter Gets a node at a set of coordinates * @param iterations Iterations to run over - * @param best_cost Best heuristic cost to propertionally expand more closer to the goal - * @return Node pointer reference to goal node if successful, else - * return nullptr + * @param closest_distance Closest distance to goal + * @return Node pointer reference to goal node with the best score out of the goals node if + * successful, else return nullptr */ NodePtr tryAnalyticExpansion( const NodePtr & current_node, - const NodePtr & goal_node, - const NodeGetter & getter, int & iterations, int & best_cost); + const NodeVector & coarse_check_goals, + const NodeVector & fine_check_goals, + const CoordinateVector & goals_coords, + const NodeGetter & getter, int & iterations, + int & closest_distance); /** * @brief Perform an analytic path expansion to the goal @@ -103,6 +140,21 @@ class AnalyticExpansion const NodePtr & node, const NodePtr & goal, const NodeGetter & getter, const ompl::base::StateSpacePtr & state_space); + /** + * @brief Refined analytic path from the current node to the goal + * @param node The node to start the analytic path from. Node head may + * change as a result of refinement + * @param goal_node The goal node to plan to + * @param getter The function object that gets valid nodes from the graph + * @param analytic_nodes The set of analytic nodes to refine + * @return The score of the refined path + */ + float refineAnalyticPath( + NodePtr & node, + const NodePtr & goal_node, + const NodeGetter & getter, + AnalyticExpansionNodes & analytic_nodes); + /** * @brief Takes final analytic expansion and appends to current expanded node * @param node The node to start the analytic path from @@ -114,6 +166,13 @@ class AnalyticExpansion const NodePtr & node, const NodePtr & goal, const AnalyticExpansionNodes & expanded_nodes); + /** + * @brief Counts the number of direction changes in a Reeds-Shepp path + * @param path The Reeds-Shepp path to count direction changes in + * @return The number of direction changes in the path + */ + int countDirectionChanges(const ompl::base::ReedsSheppStateSpace::ReedsSheppPath & path); + /** * @brief Takes an expanded nodes to clean up, if necessary, of any state * information that may be polluting it from a prior search iteration diff --git a/nav2_smac_planner/include/nav2_smac_planner/constants.hpp b/nav2_smac_planner/include/nav2_smac_planner/constants.hpp index af44ce53659..6344c86fb89 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/constants.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/constants.hpp @@ -28,6 +28,14 @@ enum class MotionModel STATE_LATTICE = 4, }; +enum class GoalHeadingMode +{ + UNKNOWN = 0, + DEFAULT = 1, + BIDIRECTIONAL = 2, + ALL_DIRECTION = 3, +}; + inline std::string toString(const MotionModel & n) { switch (n) { @@ -59,6 +67,33 @@ inline MotionModel fromString(const std::string & n) } } +inline std::string toString(const GoalHeadingMode & n) +{ + switch (n) { + case GoalHeadingMode::DEFAULT: + return "DEFAULT"; + case GoalHeadingMode::BIDIRECTIONAL: + return "BIDIRECTIONAL"; + case GoalHeadingMode::ALL_DIRECTION: + return "ALL_DIRECTION"; + default: + return "Unknown"; + } +} + +inline GoalHeadingMode fromStringToGH(const std::string & n) +{ + if (n == "DEFAULT") { + return GoalHeadingMode::DEFAULT; + } else if (n == "BIDIRECTIONAL") { + return GoalHeadingMode::BIDIRECTIONAL; + } else if (n == "ALL_DIRECTION") { + return GoalHeadingMode::ALL_DIRECTION; + } else { + return GoalHeadingMode::UNKNOWN; + } +} + const float UNKNOWN_COST = 255.0; const float OCCUPIED_COST = 254.0; const float INSCRIBED_COST = 253.0; diff --git a/nav2_smac_planner/include/nav2_smac_planner/goal_manager.hpp b/nav2_smac_planner/include/nav2_smac_planner/goal_manager.hpp new file mode 100644 index 00000000000..317a67de933 --- /dev/null +++ b/nav2_smac_planner/include/nav2_smac_planner/goal_manager.hpp @@ -0,0 +1,218 @@ +// Copyright (c) 2020, Samsung Research America +// Copyright (c) 2020, Applied Electric Vehicles Pty Ltd +// Copyright (c) 2024, Stevedan Ogochukwu Omodolor Omodia +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#ifndef NAV2_SMAC_PLANNER__GOAL_MANAGER_HPP_ +#define NAV2_SMAC_PLANNER__GOAL_MANAGER_HPP_ + +#include +#include +#include + +#include "nav2_smac_planner/types.hpp" +#include "nav2_smac_planner/node_2d.hpp" +#include "nav2_smac_planner/node_hybrid.hpp" +#include "nav2_smac_planner/node_lattice.hpp" +#include "nav2_smac_planner/node_basic.hpp" +#include "nav2_smac_planner/collision_checker.hpp" + + +namespace nav2_smac_planner +{ + +/** +* @class nav2_smac_planner::GoalManager +* @brief Responsible for managing multiple variables storing information on the goal +*/ +template +class GoalManager +{ +public: + typedef NodeT * NodePtr; + typedef std::vector NodeVector; + typedef std::unordered_set NodeSet; + typedef std::vector> GoalStateVector; + typedef typename NodeT::Coordinates Coordinates; + typedef typename NodeT::CoordinateVector CoordinateVector; + + /** + * @brief Constructor: Initializes empty goal state. sets and coordinate lists. + */ + GoalManager() + : _goals_set(NodeSet()), + _goals_state(GoalStateVector()), + _goals_coordinate(CoordinateVector()), + _ref_goal_coord(Coordinates()) + { + } + + /** + * @brief Destructor for the GoalManager + */ + ~GoalManager() = default; + + /** + * @brief Checks if the goals set is empty + * @return true if the goals set is empty + */ + bool goalsIsEmpty() + { + return _goals_state.empty(); + } + + /** + * @brief Adds goal to the goal vector + *@param goal Reference to the NodePtr + */ + void addGoal(NodePtr & goal) + { + _goals_state.push_back({goal, true}); + } + + /** + * @brief Clears all internal goal data, including goals, states, and coordinates. + */ + void clear() + { + _goals_set.clear(); + _goals_state.clear(); + _goals_coordinate.clear(); + } + + /** + * @brief Populates coarse and fine goal lists for analytic expansion. + * @param coarse_check_goals Output list of goals for coarse search expansion. + * @param fine_check_goals Output list of goals for fine search refinement. + * @param coarse_search_resolution Number of fine goals per coarse goal. + */ + void prepareGoalsForAnalyticExpansion( + NodeVector & coarse_check_goals, NodeVector & fine_check_goals, + int coarse_search_resolution) + { + for (unsigned int i = 0; i < _goals_state.size(); i++) { + if (_goals_state[i].is_valid) { + if (i % coarse_search_resolution == 0) { + coarse_check_goals.push_back(_goals_state[i].goal); + } else { + fine_check_goals.push_back(_goals_state[i].goal); + } + } + } + } + + /** + * @brief Filters and marks invalid goals based on collision checking and tolerance thresholds. + * + * Stores only valid (or tolerably infeasible) goals into internal goal sets and coordinates. + * + * @param tolerance Heuristic tolerance allowed for infeasible goals. + * @param collision_checker Collision checker to validate goal positions. + * @param traverse_unknown Flag whether traversal through unknown space is allowed. + */ + void removeInvalidGoals( + const float & tolerance, + GridCollisionChecker * collision_checker, + const bool & traverse_unknown) + { + // Make sure that there was a goal clear before this was run + if (!_goals_set.empty() || !_goals_coordinate.empty()) { + throw std::runtime_error("Goal set should be cleared before calling " + "removeinvalidgoals"); + } + for (unsigned int i = 0; i < _goals_state.size(); i++) { + if (_goals_state[i].goal->isNodeValid(traverse_unknown, collision_checker) || + tolerance > 0.001) + { + _goals_state[i].is_valid = true; + _goals_set.insert(_goals_state[i].goal); + _goals_coordinate.push_back(_goals_state[i].goal->pose); + } else { + _goals_state[i].is_valid = false; + } + } + } + + /** + * @brief Check if a given node is part of the goal set. + * @param node Node pointer to check. + * @return if node matches any goal in the goal set. + */ + inline bool isGoal(const NodePtr & node) + { + return _goals_set.find(node) != _goals_set.end(); + } + + /** + * @brief Get pointer reference to goals set vector + * @return unordered_set of node pointers reference to the goals nodes + */ + inline NodeSet & getGoalsSet() + { + return _goals_set; + } + + /** + * @brief Get pointer reference to goals state + * @return vector of node pointers reference to the goals state + */ + inline GoalStateVector & getGoalsState() + { + return _goals_state; + } + + /** + * @brief Get pointer reference to goals coordinates + * @return vector of goals coordinates reference to the goals coordinates + */ + inline CoordinateVector & getGoalsCoordinates() + { + return _goals_coordinate; + } + + /** + * @brief Set the Reference goal coordinate + * @param coord Coordinates to set as Reference goal + */ + inline void setRefGoalCoordinates(const Coordinates & coord) + { + _ref_goal_coord = coord; + } + + /** + * @brief Checks whether the Reference goal coordinate has changed. + * @param coord Coordinates to compare with the current Reference goal coordinate. + * @return true if the Reference goal coordinate has changed, false otherwise. + */ + inline bool hasGoalChanged(const Coordinates & coord) + { + /** + * Note: This function checks if the goal has changed. This has to be done with + * the coordinates not the Node pointer because the Node pointer + * can be reused for different goals, but the coordinates will always + * be unique for each goal. + */ + return _ref_goal_coord != coord; + } + +protected: + NodeSet _goals_set; + GoalStateVector _goals_state; + CoordinateVector _goals_coordinate; + Coordinates _ref_goal_coord; +}; + +} // namespace nav2_smac_planner + +#endif // NAV2_SMAC_PLANNER__GOAL_MANAGER_HPP_ diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp index 1bd0993a8eb..6d54e1696ab 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp @@ -50,6 +50,16 @@ class Node2D : x(x_in), y(y_in) {} + inline bool operator==(const Coordinates & rhs) const + { + return this->x == rhs.x && this->y == rhs.y; + } + + inline bool operator!=(const Coordinates & rhs) const + { + return !(*this == rhs); + } + float x, y; }; typedef std::vector CoordinateVector; @@ -75,6 +85,15 @@ class Node2D return this->_index == rhs._index; } + /** + * @brief setting continuous coordinate search poses (in partial-cells) + * @param Pose pose + */ + inline void setPose(const Coordinates & pose_in) + { + pose = pose_in; + } + /** * @brief Reset method for new search */ @@ -224,7 +243,7 @@ class Node2D */ static float getHeuristicCost( const Coordinates & node_coords, - const Coordinates & goal_coordinates); + const CoordinateVector & goals_coords); /** * @brief Initialize the neighborhood to be used in A* @@ -264,6 +283,7 @@ class Node2D bool backtracePath(CoordinateVector & path); Node2D * parent; + Coordinates pose; static float cost_travel_multiplier; static std::vector _neighbors_grid_offsets; diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp index ee3a4bf231c..cc3650563a8 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp @@ -167,12 +167,12 @@ class NodeHybrid : x(x_in), y(y_in), theta(theta_in) {} - inline bool operator==(const Coordinates & rhs) + inline bool operator==(const Coordinates & rhs) const { return this->x == rhs.x && this->y == rhs.y && this->theta == rhs.theta; } - inline bool operator!=(const Coordinates & rhs) + inline bool operator!=(const Coordinates & rhs) const { return !(*this == rhs); } @@ -374,7 +374,7 @@ class NodeHybrid */ static float getHeuristicCost( const Coordinates & node_coords, - const Coordinates & goal_coordinates); + const CoordinateVector & goals_coords); /** * @brief Initialize motion models diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp index 824b435447d..5b07e5453bf 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp @@ -314,7 +314,7 @@ class NodeLattice */ static float getHeuristicCost( const Coordinates & node_coords, - const Coordinates & goal_coordinates); + const CoordinateVector & goals_coords); /** * @brief Initialize motion models @@ -408,8 +408,8 @@ class NodeLattice bool backtracePath(CoordinateVector & path); /** - * \brief add node to the path - * \param current_node + * @brief add node to the path + * @param current_node */ void addNodeToPath(NodePtr current_node, CoordinateVector & path); diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp index ca88725cc23..0b50c0e15fd 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp @@ -123,6 +123,8 @@ class SmacPlannerHybrid : public nav2_core::GlobalPlanner bool _debug_visualizations; std::string _motion_model_for_search; MotionModel _motion_model; + GoalHeadingMode _goal_heading_mode; + int _coarse_search_resolution; rclcpp_lifecycle::LifecyclePublisher::SharedPtr _raw_plan_publisher; rclcpp_lifecycle::LifecyclePublisher::SharedPtr _planned_footprints_publisher; diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp index 42b30066b8a..8cead0a0145 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp @@ -122,6 +122,8 @@ class SmacPlannerLattice : public nav2_core::GlobalPlanner _smoothed_footprints_publisher; rclcpp_lifecycle::LifecyclePublisher::SharedPtr _expansions_publisher; + GoalHeadingMode _goal_heading_mode; + int _coarse_search_resolution; std::mutex _mutex; rclcpp_lifecycle::LifecycleNode::WeakPtr _node; diff --git a/nav2_smac_planner/include/nav2_smac_planner/types.hpp b/nav2_smac_planner/include/nav2_smac_planner/types.hpp index cc6c9975c5f..f71b4a932f6 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/types.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/types.hpp @@ -188,6 +188,17 @@ struct MotionPrimitive MotionPoses poses; }; +/** + * @struct nav2_smac_planner::GoalState + * @brief A struct to store the goal state + */ +template +struct GoalState +{ + NodeT * goal = nullptr; + bool is_valid = true; +}; + typedef std::vector MotionPrimitives; typedef std::vector MotionPrimitivePtrs; diff --git a/nav2_smac_planner/lattice_primitives/sample_primitives/test/output.json b/nav2_smac_planner/lattice_primitives/sample_primitives/test/output.json new file mode 100644 index 00000000000..feae5b38326 --- /dev/null +++ b/nav2_smac_planner/lattice_primitives/sample_primitives/test/output.json @@ -0,0 +1,3931 @@ +{ + "version": 1.0, + "date_generated": "2022-03-17", + "lattice_metadata": { + "motion_model": "ackermann", + "turning_radius": 0.5, + "grid_resolution": 0.05, + "stopping_threshold": 5, + "num_of_headings": 15, + "heading_angles": [ + 0.0, + 0.4636476090008061, + 0.7853981633974483, + 1.1071487177940904, + 1.5707963267948966, + 2.0344439357957027, + 2.356194490192345, + 2.677945044588987, + 3.141592653589793, + 3.6052402625905993, + 3.9269908169872414, + 4.2487413713838835, + 4.71238898038469, + 5.176036589385496, + 5.497787143782138 + ], + "number_of_trajectories": 80 + }, + "primitives": [ + { + "trajectory_id": 0, + "start_angle_index": 0, + "end_angle_index": 13, + "left_turn": false, + "trajectory_radius": 0.52586, + "trajectory_length": 0.64852, + "arc_length": 0.58221, + "straight_length": 0.06631, + "poses": [ + [ + 0.04981, + -0.00236, + 6.18831988221979 + ], + [ + 0.09917, + -0.00944, + 6.093454457259993 + ], + [ + 0.14765, + -0.02115, + 5.998589032300196 + ], + [ + 0.19479, + -0.03741, + 5.903723607340399 + ], + [ + 0.24018, + -0.05805, + 5.808858182380602 + ], + [ + 0.28341, + -0.08291, + 5.713992757420805 + ], + [ + 0.3241, + -0.11175, + 5.619127332461009 + ], + [ + 0.36187, + -0.14431, + 5.524261907501212 + ], + [ + 0.39638, + -0.1803, + 5.429396482541415 + ], + [ + 0.42733, + -0.2194, + 5.334531057581619 + ], + [ + 0.45444, + -0.26126, + 5.239665632621822 + ], + [ + 0.47769, + -0.30538, + 5.176036589385496 + ], + [ + 0.5, + -0.35, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 1, + "start_angle_index": 0, + "end_angle_index": 15, + "left_turn": false, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + 0.05254, + -0.00218, + 6.200401159939406 + ], + [ + 0.10472, + -0.00869, + 6.117617012699226 + ], + [ + 0.15619, + -0.0195, + 6.034832865459045 + ], + [ + 0.20658, + -0.03452, + 5.952048718218864 + ], + [ + 0.25556, + -0.05366, + 5.869264570978684 + ], + [ + 0.30295, + -0.07648, + 5.81953769817878 + ], + [ + 0.35, + -0.1, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 2, + "start_angle_index": 0, + "end_angle_index": 0, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.15, + "arc_length": 0.0, + "straight_length": 0.15, + "poses": [ + [ + 0.05, + 0.0, + 0.0 + ], + [ + 0.1, + 0.0, + 0.0 + ], + [ + 0.15, + 0.0, + 0.0 + ] + ] + }, + { + "trajectory_id": 3, + "start_angle_index": 0, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + 0.05254, + 0.00218, + 0.0827841472401804 + ], + [ + 0.10472, + 0.00869, + 0.1655682944803608 + ], + [ + 0.15619, + 0.0195, + 0.2483524417205412 + ], + [ + 0.20658, + 0.03452, + 0.3311365889607216 + ], + [ + 0.25556, + 0.05366, + 0.413920736200902 + ], + [ + 0.30295, + 0.07648, + 0.4636476090008061 + ], + [ + 0.35, + 0.1, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 4, + "start_angle_index": 0, + "end_angle_index": 3, + "left_turn": true, + "trajectory_radius": 0.52586, + "trajectory_length": 0.64852, + "arc_length": 0.58221, + "straight_length": 0.06631, + "poses": [ + [ + 0.04981, + 0.00236, + 0.0948654249597968 + ], + [ + 0.09917, + 0.00944, + 0.1897308499195936 + ], + [ + 0.14765, + 0.02115, + 0.2845962748793904 + ], + [ + 0.19479, + 0.03741, + 0.3794616998391872 + ], + [ + 0.24018, + 0.05805, + 0.4743271247989839 + ], + [ + 0.28341, + 0.08291, + 0.5691925497587808 + ], + [ + 0.3241, + 0.11175, + 0.6640579747185776 + ], + [ + 0.36187, + 0.14431, + 0.7589233996783744 + ], + [ + 0.39638, + 0.1803, + 0.8537888246381711 + ], + [ + 0.42733, + 0.2194, + 0.9486542495979677 + ], + [ + 0.45444, + 0.26126, + 1.0435196745577644 + ], + [ + 0.47769, + 0.30538, + 1.1071487177940904 + ], + [ + 0.5, + 0.35, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 5, + "start_angle_index": 1, + "end_angle_index": 15, + "left_turn": false, + "trajectory_radius": 0.50312, + "trajectory_length": 0.46654, + "arc_length": 0.46654, + "straight_length": 0.0, + "poses": [ + [ + 0.04747, + 0.02076, + 0.360614807000627 + ], + [ + 0.09683, + 0.03652, + 0.2575820050004478 + ], + [ + 0.14755, + 0.04712, + 0.15454920300026875 + ], + [ + 0.19909, + 0.05245, + 0.051516401000089584 + ], + [ + 0.25091, + 0.05245, + 6.231668906179497 + ], + [ + 0.30245, + 0.04712, + 6.128636104179318 + ], + [ + 0.35317, + 0.03652, + 6.025603302179138 + ], + [ + 0.40253, + 0.02076, + 5.922570500178959 + ], + [ + 0.45, + 0.0, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 6, + "start_angle_index": 1, + "end_angle_index": 15, + "left_turn": false, + "trajectory_radius": 0.61492, + "trajectory_length": 0.57021, + "arc_length": 0.57021, + "straight_length": 0.0, + "poses": [ + [ + 0.04729, + 0.0212, + 0.37934804372793224 + ], + [ + 0.09619, + 0.03835, + 0.29504847845505844 + ], + [ + 0.14636, + 0.05131, + 0.21074891318218458 + ], + [ + 0.19745, + 0.06001, + 0.12644934790931073 + ], + [ + 0.24909, + 0.06437, + 0.04214978263643687 + ], + [ + 0.30091, + 0.06437, + 6.2410355245431495 + ], + [ + 0.35255, + 0.06001, + 6.156735959270275 + ], + [ + 0.40364, + 0.05131, + 6.0724363939974015 + ], + [ + 0.45381, + 0.03835, + 5.988136828724528 + ], + [ + 0.50271, + 0.0212, + 5.903837263451654 + ], + [ + 0.55, + 0.0, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 7, + "start_angle_index": 1, + "end_angle_index": 0, + "left_turn": false, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + 0.04705, + 0.02352, + 0.4636476090008061 + ], + [ + 0.09444, + 0.04634, + 0.413920736200902 + ], + [ + 0.14342, + 0.06548, + 0.3311365889607216 + ], + [ + 0.19381, + 0.0805, + 0.24835244172054122 + ], + [ + 0.24528, + 0.09131, + 0.16556829448036087 + ], + [ + 0.29746, + 0.09782, + 0.08278414724018052 + ], + [ + 0.35, + 0.1, + 0.0 + ] + ] + }, + { + "trajectory_id": 8, + "start_angle_index": 1, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.22361, + "arc_length": 0.0, + "straight_length": 0.22361, + "poses": [ + [ + 0.05, + 0.025, + 0.4636476090008061 + ], + [ + 0.1, + 0.05, + 0.4636476090008061 + ], + [ + 0.15, + 0.075, + 0.4636476090008061 + ], + [ + 0.2, + 0.1, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 9, + "start_angle_index": 1, + "end_angle_index": 2, + "left_turn": true, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + 0.04409, + 0.0241, + 0.536596705651906 + ], + [ + 0.08631, + 0.05134, + 0.6095458023030058 + ], + [ + 0.12643, + 0.08159, + 0.6824948989541056 + ], + [ + 0.16424, + 0.11469, + 0.7554439956052055 + ], + [ + 0.2, + 0.15, + 0.7853981633974483 + ] + ] + }, + { + "trajectory_id": 10, + "start_angle_index": 2, + "end_angle_index": 0, + "left_turn": false, + "trajectory_radius": 0.51213, + "trajectory_length": 0.4401, + "arc_length": 0.40223, + "straight_length": 0.03787, + "poses": [ + [ + 0.03617, + 0.03288, + 0.6899154709776123 + ], + [ + 0.07532, + 0.06215, + 0.5944327785577764 + ], + [ + 0.11707, + 0.08756, + 0.4989500861379405 + ], + [ + 0.16106, + 0.10888, + 0.4034673937181046 + ], + [ + 0.20688, + 0.1259, + 0.30798470129826866 + ], + [ + 0.25412, + 0.13848, + 0.21250200887843262 + ], + [ + 0.30234, + 0.1465, + 0.11701931645859664 + ], + [ + 0.3511, + 0.14988, + 0.021536624038760666 + ], + [ + 0.4, + 0.15, + 0.0 + ] + ] + }, + { + "trajectory_id": 11, + "start_angle_index": 2, + "end_angle_index": 1, + "left_turn": false, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + 0.03576, + 0.03531, + 0.7554439956052055 + ], + [ + 0.07357, + 0.06841, + 0.6824948989541056 + ], + [ + 0.11369, + 0.09866, + 0.6095458023030058 + ], + [ + 0.15591, + 0.1259, + 0.5365967056519059 + ], + [ + 0.2, + 0.15, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 12, + "start_angle_index": 2, + "end_angle_index": 2, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.21213, + "arc_length": 0.0, + "straight_length": 0.21213, + "poses": [ + [ + 0.0375, + 0.0375, + 0.7853981633974483 + ], + [ + 0.075, + 0.075, + 0.7853981633974483 + ], + [ + 0.1125, + 0.1125, + 0.7853981633974483 + ], + [ + 0.15, + 0.15, + 0.7853981633974483 + ] + ] + }, + { + "trajectory_id": 13, + "start_angle_index": 2, + "end_angle_index": 3, + "left_turn": true, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + 0.03531, + 0.03576, + 0.8153523311896911 + ], + [ + 0.06841, + 0.07357, + 0.8883014278407909 + ], + [ + 0.09866, + 0.11369, + 0.9612505244918907 + ], + [ + 0.1259, + 0.15591, + 1.0341996211429907 + ], + [ + 0.15, + 0.2, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 14, + "start_angle_index": 2, + "end_angle_index": 4, + "left_turn": true, + "trajectory_radius": 0.51213, + "trajectory_length": 0.4401, + "arc_length": 0.40223, + "straight_length": 0.03787, + "poses": [ + [ + 0.03288, + 0.03617, + 0.8808808558172843 + ], + [ + 0.06215, + 0.07532, + 0.9763635482371201 + ], + [ + 0.08756, + 0.11707, + 1.071846240656956 + ], + [ + 0.10888, + 0.16106, + 1.167328933076792 + ], + [ + 0.1259, + 0.20688, + 1.262811625496628 + ], + [ + 0.13848, + 0.25412, + 1.358294317916464 + ], + [ + 0.1465, + 0.30234, + 1.4537770103363 + ], + [ + 0.14988, + 0.3511, + 1.549259702756136 + ], + [ + 0.15, + 0.4, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 15, + "start_angle_index": 3, + "end_angle_index": 2, + "left_turn": false, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + 0.0241, + 0.04409, + 1.0341996211429905 + ], + [ + 0.05134, + 0.08631, + 0.9612505244918907 + ], + [ + 0.08159, + 0.12643, + 0.8883014278407909 + ], + [ + 0.11469, + 0.16424, + 0.8153523311896911 + ], + [ + 0.15, + 0.2, + 0.7853981633974483 + ] + ] + }, + { + "trajectory_id": 16, + "start_angle_index": 3, + "end_angle_index": 3, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.22361, + "arc_length": 0.0, + "straight_length": 0.22361, + "poses": [ + [ + 0.025, + 0.05, + 1.1071487177940904 + ], + [ + 0.05, + 0.1, + 1.1071487177940904 + ], + [ + 0.075, + 0.15, + 1.1071487177940904 + ], + [ + 0.1, + 0.2, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 17, + "start_angle_index": 3, + "end_angle_index": 4, + "left_turn": true, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + 0.02352, + 0.04705, + 1.1071487177940904 + ], + [ + 0.04634, + 0.09444, + 1.1568755905939945 + ], + [ + 0.06548, + 0.14342, + 1.239659737834175 + ], + [ + 0.0805, + 0.19381, + 1.3224438850743554 + ], + [ + 0.09131, + 0.24528, + 1.4052280323145356 + ], + [ + 0.09782, + 0.29746, + 1.488012179554716 + ], + [ + 0.1, + 0.35, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 18, + "start_angle_index": 3, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 0.50312, + "trajectory_length": 0.46654, + "arc_length": 0.46654, + "straight_length": 0.0, + "poses": [ + [ + 0.02076, + 0.04747, + 1.2101815197942696 + ], + [ + 0.03652, + 0.09683, + 1.3132143217944487 + ], + [ + 0.04712, + 0.14755, + 1.4162471237946277 + ], + [ + 0.05245, + 0.19909, + 1.519279925794807 + ], + [ + 0.05245, + 0.25091, + 1.622312727794986 + ], + [ + 0.04712, + 0.30245, + 1.7253455297951654 + ], + [ + 0.03652, + 0.35317, + 1.8283783317953444 + ], + [ + 0.02076, + 0.40253, + 1.9314111337955238 + ], + [ + 0.0, + 0.45, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 19, + "start_angle_index": 3, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 0.61492, + "trajectory_length": 0.57021, + "arc_length": 0.57021, + "straight_length": 0.0, + "poses": [ + [ + 0.0212, + 0.04729, + 1.1914482830669642 + ], + [ + 0.03835, + 0.09619, + 1.2757478483398381 + ], + [ + 0.05131, + 0.14636, + 1.3600474136127119 + ], + [ + 0.06001, + 0.19745, + 1.4443469788855858 + ], + [ + 0.06437, + 0.24909, + 1.5286465441584596 + ], + [ + 0.06437, + 0.30091, + 1.6129461094313333 + ], + [ + 0.06001, + 0.35255, + 1.6972456747042073 + ], + [ + 0.05131, + 0.40364, + 1.7815452399770813 + ], + [ + 0.03835, + 0.45381, + 1.8658448052499552 + ], + [ + 0.0212, + 0.50271, + 1.9501443705228287 + ], + [ + 0.0, + 0.55, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 20, + "start_angle_index": 4, + "end_angle_index": 1, + "left_turn": false, + "trajectory_radius": 0.52586, + "trajectory_length": 0.64852, + "arc_length": 0.58221, + "straight_length": 0.06631, + "poses": [ + [ + 0.00236, + 0.04981, + 1.4759309018350997 + ], + [ + 0.00944, + 0.09917, + 1.381065476875303 + ], + [ + 0.02115, + 0.14765, + 1.2862000519155061 + ], + [ + 0.03741, + 0.19479, + 1.1913346269557095 + ], + [ + 0.05805, + 0.24018, + 1.0964692019959126 + ], + [ + 0.08291, + 0.28341, + 1.0016037770361157 + ], + [ + 0.11175, + 0.3241, + 0.9067383520763189 + ], + [ + 0.14431, + 0.36187, + 0.8118729271165221 + ], + [ + 0.1803, + 0.39638, + 0.7170075021567255 + ], + [ + 0.2194, + 0.42733, + 0.6221420771969288 + ], + [ + 0.26126, + 0.45444, + 0.5272766522371322 + ], + [ + 0.30538, + 0.47769, + 0.4636476090008061 + ], + [ + 0.35, + 0.5, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 21, + "start_angle_index": 4, + "end_angle_index": 3, + "left_turn": false, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + 0.00218, + 0.05254, + 1.488012179554716 + ], + [ + 0.00869, + 0.10472, + 1.4052280323145356 + ], + [ + 0.0195, + 0.15619, + 1.3224438850743554 + ], + [ + 0.03452, + 0.20658, + 1.239659737834175 + ], + [ + 0.05366, + 0.25556, + 1.1568755905939945 + ], + [ + 0.07648, + 0.30295, + 1.1071487177940904 + ], + [ + 0.1, + 0.35, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 22, + "start_angle_index": 4, + "end_angle_index": 4, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.15, + "arc_length": 0.0, + "straight_length": 0.15, + "poses": [ + [ + 0.0, + 0.05, + 1.5707963267948966 + ], + [ + 0.0, + 0.1, + 1.5707963267948966 + ], + [ + 0.0, + 0.15, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 23, + "start_angle_index": 4, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + -0.00218, + 0.05254, + 1.653580474035077 + ], + [ + -0.00869, + 0.10472, + 1.7363646212752575 + ], + [ + -0.0195, + 0.15619, + 1.8191487685154377 + ], + [ + -0.03452, + 0.20658, + 1.9019329157556182 + ], + [ + -0.05366, + 0.25556, + 1.9847170629957986 + ], + [ + -0.07648, + 0.30295, + 2.0344439357957027 + ], + [ + -0.1, + 0.35, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 24, + "start_angle_index": 4, + "end_angle_index": 7, + "left_turn": true, + "trajectory_radius": 0.52586, + "trajectory_length": 0.64852, + "arc_length": 0.58221, + "straight_length": 0.06631, + "poses": [ + [ + -0.00236, + 0.04981, + 1.6656617517546934 + ], + [ + -0.00944, + 0.09917, + 1.76052717671449 + ], + [ + -0.02115, + 0.14765, + 1.855392601674287 + ], + [ + -0.03741, + 0.19479, + 1.9502580266340837 + ], + [ + -0.05805, + 0.24018, + 2.0451234515938803 + ], + [ + -0.08291, + 0.28341, + 2.1399888765536774 + ], + [ + -0.11175, + 0.3241, + 2.234854301513474 + ], + [ + -0.14431, + 0.36187, + 2.3297197264732707 + ], + [ + -0.1803, + 0.39638, + 2.4245851514330674 + ], + [ + -0.2194, + 0.42733, + 2.519450576392864 + ], + [ + -0.26126, + 0.45444, + 2.6143160013526607 + ], + [ + -0.30538, + 0.47769, + 2.677945044588987 + ], + [ + -0.35, + 0.5, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 25, + "start_angle_index": 5, + "end_angle_index": 3, + "left_turn": false, + "trajectory_radius": 0.50312, + "trajectory_length": 0.46654, + "arc_length": 0.46654, + "straight_length": 0.0, + "poses": [ + [ + -0.02076, + 0.04747, + 1.9314111337955235 + ], + [ + -0.03652, + 0.09683, + 1.8283783317953444 + ], + [ + -0.04712, + 0.14755, + 1.7253455297951654 + ], + [ + -0.05245, + 0.19909, + 1.622312727794986 + ], + [ + -0.05245, + 0.25091, + 1.519279925794807 + ], + [ + -0.04712, + 0.30245, + 1.4162471237946277 + ], + [ + -0.03652, + 0.35317, + 1.3132143217944487 + ], + [ + -0.02076, + 0.40253, + 1.2101815197942694 + ], + [ + 0.0, + 0.45, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 26, + "start_angle_index": 5, + "end_angle_index": 3, + "left_turn": false, + "trajectory_radius": 0.61492, + "trajectory_length": 0.57021, + "arc_length": 0.57021, + "straight_length": 0.0, + "poses": [ + [ + -0.0212, + 0.04729, + 1.950144370522829 + ], + [ + -0.03835, + 0.09619, + 1.865844805249955 + ], + [ + -0.05131, + 0.14636, + 1.7815452399770813 + ], + [ + -0.06001, + 0.19745, + 1.6972456747042073 + ], + [ + -0.06437, + 0.24909, + 1.6129461094313335 + ], + [ + -0.06437, + 0.30091, + 1.5286465441584598 + ], + [ + -0.06001, + 0.35255, + 1.4443469788855858 + ], + [ + -0.05131, + 0.40364, + 1.3600474136127119 + ], + [ + -0.03835, + 0.45381, + 1.275747848339838 + ], + [ + -0.0212, + 0.50271, + 1.1914482830669644 + ], + [ + 0.0, + 0.55, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 27, + "start_angle_index": 5, + "end_angle_index": 4, + "left_turn": false, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + -0.02352, + 0.04705, + 2.0344439357957027 + ], + [ + -0.04634, + 0.09444, + 1.9847170629957986 + ], + [ + -0.06548, + 0.14342, + 1.9019329157556182 + ], + [ + -0.0805, + 0.19381, + 1.8191487685154377 + ], + [ + -0.09131, + 0.24528, + 1.7363646212752575 + ], + [ + -0.09782, + 0.29746, + 1.653580474035077 + ], + [ + -0.1, + 0.35, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 28, + "start_angle_index": 5, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.22361, + "arc_length": 0.0, + "straight_length": 0.22361, + "poses": [ + [ + -0.025, + 0.05, + 2.0344439357957027 + ], + [ + -0.05, + 0.1, + 2.0344439357957027 + ], + [ + -0.075, + 0.15, + 2.0344439357957027 + ], + [ + -0.1, + 0.2, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 29, + "start_angle_index": 5, + "end_angle_index": 6, + "left_turn": true, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + -0.0241, + 0.04409, + 2.1073930324468026 + ], + [ + -0.05134, + 0.08631, + 2.1803421290979026 + ], + [ + -0.08159, + 0.12643, + 2.253291225749002 + ], + [ + -0.11469, + 0.16424, + 2.326240322400102 + ], + [ + -0.15, + 0.2, + 2.356194490192345 + ] + ] + }, + { + "trajectory_id": 30, + "start_angle_index": 6, + "end_angle_index": 4, + "left_turn": false, + "trajectory_radius": 0.51213, + "trajectory_length": 0.4401, + "arc_length": 0.40223, + "straight_length": 0.03787, + "poses": [ + [ + -0.03288, + 0.03617, + 2.260711797772509 + ], + [ + -0.06215, + 0.07532, + 2.165229105352673 + ], + [ + -0.08756, + 0.11707, + 2.069746412932837 + ], + [ + -0.10888, + 0.16106, + 1.9742637205130011 + ], + [ + -0.1259, + 0.20688, + 1.8787810280931652 + ], + [ + -0.13848, + 0.25412, + 1.7832983356733292 + ], + [ + -0.1465, + 0.30234, + 1.6878156432534932 + ], + [ + -0.14988, + 0.3511, + 1.5923329508336572 + ], + [ + -0.15, + 0.4, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 31, + "start_angle_index": 6, + "end_angle_index": 5, + "left_turn": false, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + -0.03531, + 0.03576, + 2.326240322400102 + ], + [ + -0.06841, + 0.07357, + 2.253291225749002 + ], + [ + -0.09866, + 0.11369, + 2.1803421290979026 + ], + [ + -0.1259, + 0.15591, + 2.1073930324468026 + ], + [ + -0.15, + 0.2, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 32, + "start_angle_index": 6, + "end_angle_index": 6, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.21213, + "arc_length": 0.0, + "straight_length": 0.21213, + "poses": [ + [ + -0.0375, + 0.0375, + 2.356194490192345 + ], + [ + -0.075, + 0.075, + 2.356194490192345 + ], + [ + -0.1125, + 0.1125, + 2.356194490192345 + ], + [ + -0.15, + 0.15, + 2.356194490192345 + ] + ] + }, + { + "trajectory_id": 33, + "start_angle_index": 6, + "end_angle_index": 7, + "left_turn": true, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + -0.03576, + 0.03531, + 2.3861486579845876 + ], + [ + -0.07357, + 0.06841, + 2.4590977546356876 + ], + [ + -0.11369, + 0.09866, + 2.532046851286787 + ], + [ + -0.15591, + 0.1259, + 2.604995947937887 + ], + [ + -0.2, + 0.15, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 34, + "start_angle_index": 6, + "end_angle_index": 8, + "left_turn": true, + "trajectory_radius": 0.51213, + "trajectory_length": 0.4401, + "arc_length": 0.40223, + "straight_length": 0.03787, + "poses": [ + [ + -0.03617, + 0.03288, + 2.4516771826121806 + ], + [ + -0.07532, + 0.06215, + 2.547159875032017 + ], + [ + -0.11707, + 0.08756, + 2.6426425674518526 + ], + [ + -0.16106, + 0.10888, + 2.7381252598716888 + ], + [ + -0.20688, + 0.1259, + 2.8336079522915245 + ], + [ + -0.25412, + 0.13848, + 2.9290906447113603 + ], + [ + -0.30234, + 0.1465, + 3.0245733371311965 + ], + [ + -0.3511, + 0.14988, + 3.1200560295510327 + ], + [ + -0.4, + 0.15, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 35, + "start_angle_index": 7, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 0.50312, + "trajectory_length": 0.46654, + "arc_length": 0.46654, + "straight_length": 0.0, + "poses": [ + [ + -0.04747, + 0.02076, + 2.780977846589166 + ], + [ + -0.09683, + 0.03652, + 2.8840106485893453 + ], + [ + -0.14755, + 0.04712, + 2.9870434505895243 + ], + [ + -0.19909, + 0.05245, + 3.0900762525897036 + ], + [ + -0.25091, + 0.05245, + 3.1931090545898826 + ], + [ + -0.30245, + 0.04712, + 3.296141856590062 + ], + [ + -0.35317, + 0.03652, + 3.399174658590241 + ], + [ + -0.40253, + 0.02076, + 3.5022074605904203 + ], + [ + -0.45, + 0.0, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 36, + "start_angle_index": 7, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 0.61492, + "trajectory_length": 0.57021, + "arc_length": 0.57021, + "straight_length": 0.0, + "poses": [ + [ + -0.04729, + 0.0212, + 2.762244609861861 + ], + [ + -0.09619, + 0.03835, + 2.8465441751347345 + ], + [ + -0.14636, + 0.05131, + 2.9308437404076084 + ], + [ + -0.19745, + 0.06001, + 3.0151433056804824 + ], + [ + -0.24909, + 0.06437, + 3.0994428709533564 + ], + [ + -0.30091, + 0.06437, + 3.18374243622623 + ], + [ + -0.35255, + 0.06001, + 3.268042001499104 + ], + [ + -0.40364, + 0.05131, + 3.352341566771978 + ], + [ + -0.45381, + 0.03835, + 3.4366411320448518 + ], + [ + -0.50271, + 0.0212, + 3.5209406973177253 + ], + [ + -0.55, + 0.0, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 37, + "start_angle_index": 7, + "end_angle_index": 6, + "left_turn": false, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + -0.04409, + 0.0241, + 2.604995947937887 + ], + [ + -0.08631, + 0.05134, + 2.532046851286787 + ], + [ + -0.12643, + 0.08159, + 2.4590977546356876 + ], + [ + -0.16424, + 0.11469, + 2.3861486579845876 + ], + [ + -0.2, + 0.15, + 2.356194490192345 + ] + ] + }, + { + "trajectory_id": 38, + "start_angle_index": 7, + "end_angle_index": 7, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.22361, + "arc_length": 0.0, + "straight_length": 0.22361, + "poses": [ + [ + -0.05, + 0.025, + 2.677945044588987 + ], + [ + -0.1, + 0.05, + 2.677945044588987 + ], + [ + -0.15, + 0.075, + 2.677945044588987 + ], + [ + -0.2, + 0.1, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 39, + "start_angle_index": 7, + "end_angle_index": 8, + "left_turn": true, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + -0.04705, + 0.02352, + 2.677945044588987 + ], + [ + -0.09444, + 0.04634, + 2.727671917388891 + ], + [ + -0.14342, + 0.06548, + 2.8104560646290713 + ], + [ + -0.19381, + 0.0805, + 2.893240211869252 + ], + [ + -0.24528, + 0.09131, + 2.976024359109432 + ], + [ + -0.29746, + 0.09782, + 3.058808506349613 + ], + [ + -0.35, + 0.1, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 40, + "start_angle_index": 8, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + -0.05254, + -0.00218, + 3.2243768008299734 + ], + [ + -0.10472, + -0.00869, + 3.307160948070154 + ], + [ + -0.15619, + -0.0195, + 3.3899450953103343 + ], + [ + -0.20658, + -0.03452, + 3.472729242550515 + ], + [ + -0.25556, + -0.05366, + 3.555513389790695 + ], + [ + -0.30295, + -0.07648, + 3.6052402625905993 + ], + [ + -0.35, + -0.1, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 41, + "start_angle_index": 8, + "end_angle_index": 11, + "left_turn": true, + "trajectory_radius": 0.52586, + "trajectory_length": 0.64852, + "arc_length": 0.58221, + "straight_length": 0.06631, + "poses": [ + [ + -0.04981, + -0.00236, + 3.23645807854959 + ], + [ + -0.09917, + -0.00944, + 3.331323503509387 + ], + [ + -0.14765, + -0.02115, + 3.4261889284691835 + ], + [ + -0.19479, + -0.03741, + 3.52105435342898 + ], + [ + -0.24018, + -0.05805, + 3.615919778388777 + ], + [ + -0.28341, + -0.08291, + 3.710785203348574 + ], + [ + -0.3241, + -0.11175, + 3.8056506283083706 + ], + [ + -0.36187, + -0.14431, + 3.9005160532681673 + ], + [ + -0.39638, + -0.1803, + 3.995381478227964 + ], + [ + -0.42733, + -0.2194, + 4.090246903187761 + ], + [ + -0.45444, + -0.26126, + 4.185112328147557 + ], + [ + -0.47769, + -0.30538, + 4.2487413713838835 + ], + [ + -0.5, + -0.35, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 42, + "start_angle_index": 8, + "end_angle_index": 5, + "left_turn": false, + "trajectory_radius": 0.52586, + "trajectory_length": 0.64852, + "arc_length": 0.58221, + "straight_length": 0.06631, + "poses": [ + [ + -0.04981, + 0.00236, + 3.0467272286299965 + ], + [ + -0.09917, + 0.00944, + 2.9518618036701993 + ], + [ + -0.14765, + 0.02115, + 2.8569963787104027 + ], + [ + -0.19479, + 0.03741, + 2.762130953750606 + ], + [ + -0.24018, + 0.05805, + 2.6672655287908094 + ], + [ + -0.28341, + 0.08291, + 2.5724001038310123 + ], + [ + -0.3241, + 0.11175, + 2.4775346788712156 + ], + [ + -0.36187, + 0.14431, + 2.382669253911419 + ], + [ + -0.39638, + 0.1803, + 2.2878038289516223 + ], + [ + -0.42733, + 0.2194, + 2.1929384039918256 + ], + [ + -0.45444, + 0.26126, + 2.098072979032029 + ], + [ + -0.47769, + 0.30538, + 2.0344439357957027 + ], + [ + -0.5, + 0.35, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 43, + "start_angle_index": 8, + "end_angle_index": 7, + "left_turn": false, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + -0.05254, + 0.00218, + 3.058808506349613 + ], + [ + -0.10472, + 0.00869, + 2.976024359109432 + ], + [ + -0.15619, + 0.0195, + 2.893240211869252 + ], + [ + -0.20658, + 0.03452, + 2.8104560646290713 + ], + [ + -0.25556, + 0.05366, + 2.727671917388891 + ], + [ + -0.30295, + 0.07648, + 2.677945044588987 + ], + [ + -0.35, + 0.1, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 44, + "start_angle_index": 8, + "end_angle_index": 8, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.15, + "arc_length": 0.0, + "straight_length": 0.15, + "poses": [ + [ + -0.05, + 0.0, + 3.141592653589793 + ], + [ + -0.1, + 0.0, + 3.141592653589793 + ], + [ + -0.15, + 0.0, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 45, + "start_angle_index": 9, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.22361, + "arc_length": 0.0, + "straight_length": 0.22361, + "poses": [ + [ + -0.05, + -0.025, + 3.6052402625905993 + ], + [ + -0.1, + -0.05, + 3.6052402625905993 + ], + [ + -0.15, + -0.075, + 3.6052402625905993 + ], + [ + -0.2, + -0.1, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 46, + "start_angle_index": 9, + "end_angle_index": 10, + "left_turn": true, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + -0.04409, + -0.0241, + 3.678189359241699 + ], + [ + -0.08631, + -0.05134, + 3.751138455892799 + ], + [ + -0.12643, + -0.08159, + 3.8240875525438986 + ], + [ + -0.16424, + -0.11469, + 3.8970366491949986 + ], + [ + -0.2, + -0.15, + 3.9269908169872414 + ] + ] + }, + { + "trajectory_id": 47, + "start_angle_index": 9, + "end_angle_index": 7, + "left_turn": false, + "trajectory_radius": 0.50312, + "trajectory_length": 0.46654, + "arc_length": 0.46654, + "straight_length": 0.0, + "poses": [ + [ + -0.04747, + -0.02076, + 3.5022074605904203 + ], + [ + -0.09683, + -0.03652, + 3.399174658590241 + ], + [ + -0.14755, + -0.04712, + 3.296141856590062 + ], + [ + -0.19909, + -0.05245, + 3.1931090545898826 + ], + [ + -0.25091, + -0.05245, + 3.0900762525897036 + ], + [ + -0.30245, + -0.04712, + 2.9870434505895243 + ], + [ + -0.35317, + -0.03652, + 2.8840106485893453 + ], + [ + -0.40253, + -0.02076, + 2.780977846589166 + ], + [ + -0.45, + 0.0, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 48, + "start_angle_index": 9, + "end_angle_index": 7, + "left_turn": false, + "trajectory_radius": 0.61492, + "trajectory_length": 0.57021, + "arc_length": 0.57021, + "straight_length": 0.0, + "poses": [ + [ + -0.04729, + -0.0212, + 3.5209406973177253 + ], + [ + -0.09619, + -0.03835, + 3.4366411320448518 + ], + [ + -0.14636, + -0.05131, + 3.352341566771978 + ], + [ + -0.19745, + -0.06001, + 3.268042001499104 + ], + [ + -0.24909, + -0.06437, + 3.18374243622623 + ], + [ + -0.30091, + -0.06437, + 3.0994428709533564 + ], + [ + -0.35255, + -0.06001, + 3.0151433056804824 + ], + [ + -0.40364, + -0.05131, + 2.9308437404076084 + ], + [ + -0.45381, + -0.03835, + 2.8465441751347345 + ], + [ + -0.50271, + -0.0212, + 2.762244609861861 + ], + [ + -0.55, + 0.0, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 49, + "start_angle_index": 9, + "end_angle_index": 8, + "left_turn": false, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + -0.04705, + -0.02352, + 3.6052402625905993 + ], + [ + -0.09444, + -0.04634, + 3.555513389790695 + ], + [ + -0.14342, + -0.06548, + 3.472729242550515 + ], + [ + -0.19381, + -0.0805, + 3.3899450953103343 + ], + [ + -0.24528, + -0.09131, + 3.307160948070154 + ], + [ + -0.29746, + -0.09782, + 3.2243768008299734 + ], + [ + -0.35, + -0.1, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 50, + "start_angle_index": 10, + "end_angle_index": 9, + "left_turn": false, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + -0.03576, + -0.03531, + 3.8970366491949986 + ], + [ + -0.07357, + -0.06841, + 3.8240875525438986 + ], + [ + -0.11369, + -0.09866, + 3.751138455892799 + ], + [ + -0.15591, + -0.1259, + 3.678189359241699 + ], + [ + -0.2, + -0.15, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 51, + "start_angle_index": 10, + "end_angle_index": 10, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.21213, + "arc_length": 0.0, + "straight_length": 0.21213, + "poses": [ + [ + -0.0375, + -0.0375, + 3.9269908169872414 + ], + [ + -0.075, + -0.075, + 3.9269908169872414 + ], + [ + -0.1125, + -0.1125, + 3.9269908169872414 + ], + [ + -0.15, + -0.15, + 3.9269908169872414 + ] + ] + }, + { + "trajectory_id": 52, + "start_angle_index": 10, + "end_angle_index": 11, + "left_turn": true, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + -0.03531, + -0.03576, + 3.956944984779484 + ], + [ + -0.06841, + -0.07357, + 4.029894081430584 + ], + [ + -0.09866, + -0.11369, + 4.102843178081684 + ], + [ + -0.1259, + -0.15591, + 4.175792274732784 + ], + [ + -0.15, + -0.2, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 53, + "start_angle_index": 10, + "end_angle_index": 12, + "left_turn": true, + "trajectory_radius": 0.51213, + "trajectory_length": 0.4401, + "arc_length": 0.40223, + "straight_length": 0.03787, + "poses": [ + [ + -0.03288, + -0.03617, + 4.022473509407077 + ], + [ + -0.06215, + -0.07532, + 4.117956201826914 + ], + [ + -0.08756, + -0.11707, + 4.2134388942467496 + ], + [ + -0.10888, + -0.16106, + 4.308921586666585 + ], + [ + -0.1259, + -0.20688, + 4.404404279086421 + ], + [ + -0.13848, + -0.25412, + 4.499886971506257 + ], + [ + -0.1465, + -0.30234, + 4.595369663926093 + ], + [ + -0.14988, + -0.3511, + 4.690852356345929 + ], + [ + -0.15, + -0.4, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 54, + "start_angle_index": 10, + "end_angle_index": 8, + "left_turn": false, + "trajectory_radius": 0.51213, + "trajectory_length": 0.4401, + "arc_length": 0.40223, + "straight_length": 0.03787, + "poses": [ + [ + -0.03617, + -0.03288, + 3.8315081245674056 + ], + [ + -0.07532, + -0.06215, + 3.7360254321475694 + ], + [ + -0.11707, + -0.08756, + 3.6405427397277337 + ], + [ + -0.16106, + -0.10888, + 3.5450600473078975 + ], + [ + -0.20688, + -0.1259, + 3.4495773548880617 + ], + [ + -0.25412, + -0.13848, + 3.354094662468226 + ], + [ + -0.30234, + -0.1465, + 3.2586119700483898 + ], + [ + -0.3511, + -0.14988, + 3.1631292776285536 + ], + [ + -0.4, + -0.15, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 55, + "start_angle_index": 11, + "end_angle_index": 10, + "left_turn": false, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + -0.0241, + -0.04409, + 4.175792274732784 + ], + [ + -0.05134, + -0.08631, + 4.102843178081684 + ], + [ + -0.08159, + -0.12643, + 4.029894081430584 + ], + [ + -0.11469, + -0.16424, + 3.956944984779484 + ], + [ + -0.15, + -0.2, + 3.9269908169872414 + ] + ] + }, + { + "trajectory_id": 56, + "start_angle_index": 11, + "end_angle_index": 11, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.22361, + "arc_length": 0.0, + "straight_length": 0.22361, + "poses": [ + [ + -0.025, + -0.05, + 4.2487413713838835 + ], + [ + -0.05, + -0.1, + 4.2487413713838835 + ], + [ + -0.075, + -0.15, + 4.2487413713838835 + ], + [ + -0.1, + -0.2, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 57, + "start_angle_index": 11, + "end_angle_index": 12, + "left_turn": true, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + -0.02352, + -0.04705, + 4.2487413713838835 + ], + [ + -0.04634, + -0.09444, + 4.298468244183788 + ], + [ + -0.06548, + -0.14342, + 4.381252391423968 + ], + [ + -0.0805, + -0.19381, + 4.464036538664148 + ], + [ + -0.09131, + -0.24528, + 4.546820685904329 + ], + [ + -0.09782, + -0.29746, + 4.629604833144509 + ], + [ + -0.1, + -0.35, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 58, + "start_angle_index": 11, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 0.50312, + "trajectory_length": 0.46654, + "arc_length": 0.46654, + "straight_length": 0.0, + "poses": [ + [ + -0.02076, + -0.04747, + 4.3517741733840625 + ], + [ + -0.03652, + -0.09683, + 4.454806975384242 + ], + [ + -0.04712, + -0.14755, + 4.557839777384421 + ], + [ + -0.05245, + -0.19909, + 4.6608725793846 + ], + [ + -0.05245, + -0.25091, + 4.763905381384779 + ], + [ + -0.04712, + -0.30245, + 4.866938183384958 + ], + [ + -0.03652, + -0.35317, + 4.969970985385137 + ], + [ + -0.02076, + -0.40253, + 5.073003787385317 + ], + [ + 0.0, + -0.45, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 59, + "start_angle_index": 11, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 0.61492, + "trajectory_length": 0.57021, + "arc_length": 0.57021, + "straight_length": 0.0, + "poses": [ + [ + -0.0212, + -0.04729, + 4.333040936656757 + ], + [ + -0.03835, + -0.09619, + 4.4173405019296315 + ], + [ + -0.05131, + -0.14636, + 4.501640067202505 + ], + [ + -0.06001, + -0.19745, + 4.5859396324753785 + ], + [ + -0.06437, + -0.24909, + 4.670239197748253 + ], + [ + -0.06437, + -0.30091, + 4.754538763021126 + ], + [ + -0.06001, + -0.35255, + 4.838838328294001 + ], + [ + -0.05131, + -0.40364, + 4.923137893566874 + ], + [ + -0.03835, + -0.45381, + 5.007437458839748 + ], + [ + -0.0212, + -0.50271, + 5.091737024112621 + ], + [ + 0.0, + -0.55, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 60, + "start_angle_index": 12, + "end_angle_index": 9, + "left_turn": false, + "trajectory_radius": 0.52586, + "trajectory_length": 0.64852, + "arc_length": 0.58221, + "straight_length": 0.06631, + "poses": [ + [ + -0.00236, + -0.04981, + 4.617523555424893 + ], + [ + -0.00944, + -0.09917, + 4.522658130465096 + ], + [ + -0.02115, + -0.14765, + 4.427792705505299 + ], + [ + -0.03741, + -0.19479, + 4.332927280545503 + ], + [ + -0.05805, + -0.24018, + 4.2380618555857055 + ], + [ + -0.08291, + -0.28341, + 4.143196430625909 + ], + [ + -0.11175, + -0.3241, + 4.048331005666112 + ], + [ + -0.14431, + -0.36187, + 3.9534655807063155 + ], + [ + -0.1803, + -0.39638, + 3.858600155746519 + ], + [ + -0.2194, + -0.42733, + 3.763734730786722 + ], + [ + -0.26126, + -0.45444, + 3.6688693058269255 + ], + [ + -0.30538, + -0.47769, + 3.6052402625905993 + ], + [ + -0.35, + -0.5, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 61, + "start_angle_index": 12, + "end_angle_index": 11, + "left_turn": false, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + -0.00218, + -0.05254, + 4.629604833144509 + ], + [ + -0.00869, + -0.10472, + 4.546820685904329 + ], + [ + -0.0195, + -0.15619, + 4.464036538664148 + ], + [ + -0.03452, + -0.20658, + 4.381252391423968 + ], + [ + -0.05366, + -0.25556, + 4.298468244183788 + ], + [ + -0.07648, + -0.30295, + 4.2487413713838835 + ], + [ + -0.1, + -0.35, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 62, + "start_angle_index": 12, + "end_angle_index": 12, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.15, + "arc_length": 0.0, + "straight_length": 0.15, + "poses": [ + [ + 0.0, + -0.05, + 4.71238898038469 + ], + [ + 0.0, + -0.1, + 4.71238898038469 + ], + [ + 0.0, + -0.15, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 63, + "start_angle_index": 12, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + 0.00218, + -0.05254, + 4.79517312762487 + ], + [ + 0.00869, + -0.10472, + 4.87795727486505 + ], + [ + 0.0195, + -0.15619, + 4.960741422105231 + ], + [ + 0.03452, + -0.20658, + 5.0435255693454115 + ], + [ + 0.05366, + -0.25556, + 5.126309716585592 + ], + [ + 0.07648, + -0.30295, + 5.176036589385496 + ], + [ + 0.1, + -0.35, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 64, + "start_angle_index": 12, + "end_angle_index": 15, + "left_turn": true, + "trajectory_radius": 0.52586, + "trajectory_length": 0.64852, + "arc_length": 0.58221, + "straight_length": 0.06631, + "poses": [ + [ + 0.00236, + -0.04981, + 4.807254405344486 + ], + [ + 0.00944, + -0.09917, + 4.902119830304283 + ], + [ + 0.02115, + -0.14765, + 4.9969852552640805 + ], + [ + 0.03741, + -0.19479, + 5.091850680223876 + ], + [ + 0.05805, + -0.24018, + 5.186716105183674 + ], + [ + 0.08291, + -0.28341, + 5.2815815301434705 + ], + [ + 0.11175, + -0.3241, + 5.376446955103267 + ], + [ + 0.14431, + -0.36187, + 5.471312380063064 + ], + [ + 0.1803, + -0.39638, + 5.5661778050228605 + ], + [ + 0.2194, + -0.42733, + 5.661043229982657 + ], + [ + 0.26126, + -0.45444, + 5.755908654942454 + ], + [ + 0.30538, + -0.47769, + 5.81953769817878 + ], + [ + 0.35, + -0.5, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 65, + "start_angle_index": 13, + "end_angle_index": 11, + "left_turn": false, + "trajectory_radius": 0.50312, + "trajectory_length": 0.46654, + "arc_length": 0.46654, + "straight_length": 0.0, + "poses": [ + [ + 0.02076, + -0.04747, + 5.073003787385317 + ], + [ + 0.03652, + -0.09683, + 4.969970985385137 + ], + [ + 0.04712, + -0.14755, + 4.866938183384958 + ], + [ + 0.05245, + -0.19909, + 4.763905381384779 + ], + [ + 0.05245, + -0.25091, + 4.6608725793846 + ], + [ + 0.04712, + -0.30245, + 4.557839777384421 + ], + [ + 0.03652, + -0.35317, + 4.454806975384242 + ], + [ + 0.02076, + -0.40253, + 4.3517741733840625 + ], + [ + 0.0, + -0.45, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 66, + "start_angle_index": 13, + "end_angle_index": 11, + "left_turn": false, + "trajectory_radius": 0.61492, + "trajectory_length": 0.57021, + "arc_length": 0.57021, + "straight_length": 0.0, + "poses": [ + [ + 0.0212, + -0.04729, + 5.091737024112622 + ], + [ + 0.03835, + -0.09619, + 5.007437458839748 + ], + [ + 0.05131, + -0.14636, + 4.923137893566874 + ], + [ + 0.06001, + -0.19745, + 4.838838328294001 + ], + [ + 0.06437, + -0.24909, + 4.754538763021126 + ], + [ + 0.06437, + -0.30091, + 4.670239197748253 + ], + [ + 0.06001, + -0.35255, + 4.5859396324753785 + ], + [ + 0.05131, + -0.40364, + 4.501640067202505 + ], + [ + 0.03835, + -0.45381, + 4.4173405019296315 + ], + [ + 0.0212, + -0.50271, + 4.333040936656758 + ], + [ + 0.0, + -0.55, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 67, + "start_angle_index": 13, + "end_angle_index": 12, + "left_turn": false, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + 0.02352, + -0.04705, + 5.176036589385496 + ], + [ + 0.04634, + -0.09444, + 5.126309716585592 + ], + [ + 0.06548, + -0.14342, + 5.0435255693454115 + ], + [ + 0.0805, + -0.19381, + 4.960741422105231 + ], + [ + 0.09131, + -0.24528, + 4.87795727486505 + ], + [ + 0.09782, + -0.29746, + 4.79517312762487 + ], + [ + 0.1, + -0.35, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 68, + "start_angle_index": 13, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.22361, + "arc_length": 0.0, + "straight_length": 0.22361, + "poses": [ + [ + 0.025, + -0.05, + 5.176036589385496 + ], + [ + 0.05, + -0.1, + 5.176036589385496 + ], + [ + 0.075, + -0.15, + 5.176036589385496 + ], + [ + 0.1, + -0.2, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 69, + "start_angle_index": 13, + "end_angle_index": 14, + "left_turn": true, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + 0.0241, + -0.04409, + 5.248985686036596 + ], + [ + 0.05134, + -0.08631, + 5.321934782687696 + ], + [ + 0.08159, + -0.12643, + 5.394883879338796 + ], + [ + 0.11469, + -0.16424, + 5.467832975989895 + ], + [ + 0.15, + -0.2, + 5.497787143782138 + ] + ] + }, + { + "trajectory_id": 70, + "start_angle_index": 14, + "end_angle_index": 12, + "left_turn": false, + "trajectory_radius": 0.51213, + "trajectory_length": 0.4401, + "arc_length": 0.40223, + "straight_length": 0.03787, + "poses": [ + [ + 0.03288, + -0.03617, + 5.402304451362302 + ], + [ + 0.06215, + -0.07532, + 5.306821758942466 + ], + [ + 0.08756, + -0.11707, + 5.21133906652263 + ], + [ + 0.10888, + -0.16106, + 5.115856374102794 + ], + [ + 0.1259, + -0.20688, + 5.020373681682958 + ], + [ + 0.13848, + -0.25412, + 4.9248909892631225 + ], + [ + 0.1465, + -0.30234, + 4.829408296843287 + ], + [ + 0.14988, + -0.3511, + 4.73392560442345 + ], + [ + 0.15, + -0.4, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 71, + "start_angle_index": 14, + "end_angle_index": 13, + "left_turn": false, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + 0.03531, + -0.03576, + 5.467832975989895 + ], + [ + 0.06841, + -0.07357, + 5.394883879338796 + ], + [ + 0.09866, + -0.11369, + 5.321934782687696 + ], + [ + 0.1259, + -0.15591, + 5.248985686036596 + ], + [ + 0.15, + -0.2, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 72, + "start_angle_index": 14, + "end_angle_index": 14, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.21213, + "arc_length": 0.0, + "straight_length": 0.21213, + "poses": [ + [ + 0.0375, + -0.0375, + 5.497787143782138 + ], + [ + 0.075, + -0.075, + 5.497787143782138 + ], + [ + 0.1125, + -0.1125, + 5.497787143782138 + ], + [ + 0.15, + -0.15, + 5.497787143782138 + ] + ] + }, + { + "trajectory_id": 73, + "start_angle_index": 14, + "end_angle_index": 15, + "left_turn": true, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + 0.03576, + -0.03531, + 5.527741311574381 + ], + [ + 0.07357, + -0.06841, + 5.60069040822548 + ], + [ + 0.11369, + -0.09866, + 5.67363950487658 + ], + [ + 0.15591, + -0.1259, + 5.74658860152768 + ], + [ + 0.2, + -0.15, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 74, + "start_angle_index": 14, + "end_angle_index": 0, + "left_turn": true, + "trajectory_radius": 0.51213, + "trajectory_length": 0.4401, + "arc_length": 0.40223, + "straight_length": 0.03787, + "poses": [ + [ + 0.03617, + -0.03288, + 5.593269836201974 + ], + [ + 0.07532, + -0.06215, + 5.6887525286218095 + ], + [ + 0.11707, + -0.08756, + 5.784235221041646 + ], + [ + 0.16106, + -0.10888, + 5.879717913461482 + ], + [ + 0.20688, + -0.1259, + 5.975200605881318 + ], + [ + 0.25412, + -0.13848, + 6.070683298301153 + ], + [ + 0.30234, + -0.1465, + 6.166165990720989 + ], + [ + 0.3511, + -0.14988, + 6.261648683140826 + ], + [ + 0.4, + -0.15, + 0.0 + ] + ] + }, + { + "trajectory_id": 75, + "start_angle_index": 15, + "end_angle_index": 14, + "left_turn": false, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + 0.04409, + -0.0241, + 5.74658860152768 + ], + [ + 0.08631, + -0.05134, + 5.67363950487658 + ], + [ + 0.12643, + -0.08159, + 5.60069040822548 + ], + [ + 0.16424, + -0.11469, + 5.527741311574381 + ], + [ + 0.2, + -0.15, + 5.497787143782138 + ] + ] + }, + { + "trajectory_id": 76, + "start_angle_index": 15, + "end_angle_index": 15, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.22361, + "arc_length": 0.0, + "straight_length": 0.22361, + "poses": [ + [ + 0.05, + -0.025, + 5.81953769817878 + ], + [ + 0.1, + -0.05, + 5.81953769817878 + ], + [ + 0.15, + -0.075, + 5.81953769817878 + ], + [ + 0.2, + -0.1, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 77, + "start_angle_index": 15, + "end_angle_index": 0, + "left_turn": true, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + 0.04705, + -0.02352, + 5.81953769817878 + ], + [ + 0.09444, + -0.04634, + 5.869264570978684 + ], + [ + 0.14342, + -0.06548, + 5.952048718218864 + ], + [ + 0.19381, + -0.0805, + 6.034832865459045 + ], + [ + 0.24528, + -0.09131, + 6.117617012699226 + ], + [ + 0.29746, + -0.09782, + 6.200401159939406 + ], + [ + 0.35, + -0.1, + 0.0 + ] + ] + }, + { + "trajectory_id": 78, + "start_angle_index": 15, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 0.50312, + "trajectory_length": 0.46654, + "arc_length": 0.46654, + "straight_length": 0.0, + "poses": [ + [ + 0.04747, + -0.02076, + 5.922570500178959 + ], + [ + 0.09683, + -0.03652, + 6.025603302179139 + ], + [ + 0.14755, + -0.04712, + 6.128636104179318 + ], + [ + 0.19909, + -0.05245, + 6.231668906179497 + ], + [ + 0.25091, + -0.05245, + 0.05151640100008953 + ], + [ + 0.30245, + -0.04712, + 0.1545492030002687 + ], + [ + 0.35317, + -0.03652, + 0.25758200500044787 + ], + [ + 0.40253, + -0.02076, + 0.36061480700062715 + ], + [ + 0.45, + 0.0, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 79, + "start_angle_index": 15, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 0.61492, + "trajectory_length": 0.57021, + "arc_length": 0.57021, + "straight_length": 0.0, + "poses": [ + [ + 0.04729, + -0.0212, + 5.903837263451654 + ], + [ + 0.09619, + -0.03835, + 5.988136828724528 + ], + [ + 0.14636, + -0.05131, + 6.0724363939974015 + ], + [ + 0.19745, + -0.06001, + 6.156735959270275 + ], + [ + 0.24909, + -0.06437, + 6.2410355245431495 + ], + [ + 0.30091, + -0.06437, + 0.04214978263643693 + ], + [ + 0.35255, + -0.06001, + 0.1264493479093109 + ], + [ + 0.40364, + -0.05131, + 0.21074891318218475 + ], + [ + 0.45381, + -0.03835, + 0.2950484784550586 + ], + [ + 0.50271, + -0.0212, + 0.37934804372793235 + ], + [ + 0.55, + 0.0, + 0.4636476090008061 + ] + ] + } + ] +} diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index 52b94db14af..98d040bd9cc 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -43,9 +43,8 @@ AStarAlgorithm::AStarAlgorithm( _x_size(0), _y_size(0), _search_info(search_info), - _goal_coordinates(Coordinates()), _start(nullptr), - _goal(nullptr), + _goal_manager(GoalManagerT()), _motion_model(motion_model) { _graph.reserve(100000); @@ -192,35 +191,43 @@ template<> void AStarAlgorithm::setGoal( const float & mx, const float & my, - const unsigned int & dim_3) + const unsigned int & dim_3, + const GoalHeadingMode & /*goal_heading_mode*/, + const int & /*coarse_search_resolution*/) { if (dim_3 != 0) { throw std::runtime_error("Node type Node2D cannot be given non-zero goal dim 3."); } - - _goal = addToGraph( + _goal_manager.clear(); + auto goal = addToGraph( Node2D::getIndex( static_cast(mx), static_cast(my), getSizeX())); - _goal_coordinates = Node2D::Coordinates(mx, my); + + goal->setPose(Node2D::Coordinates(mx, my)); + _goal_manager.addGoal(goal); + + _coarse_search_resolution = 1; } template void AStarAlgorithm::setGoal( const float & mx, const float & my, - const unsigned int & dim_3) + const unsigned int & dim_3, + const GoalHeadingMode & goal_heading_mode, + const int & coarse_search_resolution) { - _goal = addToGraph( - NodeT::getIndex( - static_cast(mx), - static_cast(my), - dim_3)); + // Default to minimal resolution unless overridden for ALL_DIRECTION + _coarse_search_resolution = 1; - typename NodeT::Coordinates goal_coords(mx, my, dim_3); + _goal_manager.clear(); + Coordinates ref_goal_coord(mx, my, static_cast(dim_3)); - if (!_search_info.cache_obstacle_heuristic || goal_coords != _goal_coordinates) { + if (!_search_info.cache_obstacle_heuristic || + _goal_manager.hasGoalChanged(ref_goal_coord)) + { if (!_start) { throw std::runtime_error("Start must be set before goal."); } @@ -229,8 +236,66 @@ void AStarAlgorithm::setGoal( _collision_checker->getCostmapROS(), _start->pose.x, _start->pose.y, mx, my); } - _goal_coordinates = goal_coords; - _goal->setPose(_goal_coordinates); + _goal_manager.setRefGoalCoordinates(ref_goal_coord); + + unsigned int num_bins = NodeT::motion_table.num_angle_quantization; + // set goal based on heading mode + switch (goal_heading_mode) { + case GoalHeadingMode::DEFAULT: { + // add a single goal node with single heading + auto goal = addToGraph( + NodeT::getIndex( + static_cast(mx), + static_cast(my), + dim_3)); + goal->setPose(typename NodeT::Coordinates(mx, my, static_cast(dim_3))); + _goal_manager.addGoal(goal); + break; + } + + case GoalHeadingMode::BIDIRECTIONAL: { + // Add two goals, one for each direction + // add goal in original direction + auto goal = addToGraph( + NodeT::getIndex( + static_cast(mx), + static_cast(my), + dim_3)); + goal->setPose(typename NodeT::Coordinates(mx, my, static_cast(dim_3))); + _goal_manager.addGoal(goal); + + // Add goal node in opposite (180°) direction + unsigned int opposite_heading = (dim_3 + (num_bins / 2)) % num_bins; + auto opposite_goal = addToGraph( + NodeT::getIndex( + static_cast(mx), + static_cast(my), + opposite_heading)); + opposite_goal->setPose( + typename NodeT::Coordinates(mx, my, static_cast(opposite_heading))); + _goal_manager.addGoal(opposite_goal); + break; + } + + case GoalHeadingMode::ALL_DIRECTION: { + // Set the coarse search resolution only for all direction + _coarse_search_resolution = coarse_search_resolution; + + // Add goal nodes for all headings + for (unsigned int i = 0; i < num_bins; ++i) { + auto goal = addToGraph( + NodeT::getIndex( + static_cast(mx), + static_cast(my), + i)); + goal->setPose(typename NodeT::Coordinates(mx, my, static_cast(i))); + _goal_manager.addGoal(goal); + } + break; + } + case GoalHeadingMode::UNKNOWN: + throw std::runtime_error("Goal heading is UNKNOWN."); + } } template @@ -242,14 +307,15 @@ bool AStarAlgorithm::areInputsValid() } // Check if points were filled in - if (!_start || !_goal) { + if (!_start || _goal_manager.goalsIsEmpty()) { throw std::runtime_error("Failed to compute path, no valid start or goal given."); } + // remove invalid goals + _goal_manager.removeInvalidGoals(getToleranceHeuristic(), _collision_checker, _traverse_unknown); + // Check if ending point is valid - if (getToleranceHeuristic() < 0.001 && - !_goal->isNodeValid(_traverse_unknown, _collision_checker)) - { + if (_goal_manager.getGoalsSet().empty()) { throw nav2_core::GoalOccupied("Goal was in lethal cost"); } @@ -273,6 +339,10 @@ bool AStarAlgorithm::createPath( return false; } + NodeVector coarse_check_goals, fine_check_goals; + _goal_manager.prepareGoalsForAnalyticExpansion(coarse_check_goals, fine_check_goals, + _coarse_search_resolution); + // 0) Add starting point to the open set addNode(0.0, getStart()); getStart()->setAccumulatedCost(0.0); @@ -339,13 +409,14 @@ bool AStarAlgorithm::createPath( // 2.1) Use an analytic expansion (if available) to generate a path expansion_result = nullptr; expansion_result = _expander->tryAnalyticExpansion( - current_node, getGoal(), neighborGetter, analytic_iterations, closest_distance); + current_node, coarse_check_goals, fine_check_goals, + _goal_manager.getGoalsCoordinates(), neighborGetter, analytic_iterations, closest_distance); if (expansion_result != nullptr) { current_node = expansion_result; } // 3) Check if we're at the goal, backtrace if required - if (isGoal(current_node)) { + if (_goal_manager.isGoal(current_node)) { return current_node->backtracePath(path); } else if (_best_heuristic_node.first < getToleranceHeuristic()) { // Optimization: Let us find when in tolerance and refine within reason @@ -386,24 +457,12 @@ bool AStarAlgorithm::createPath( return false; } -template -bool AStarAlgorithm::isGoal(NodePtr & node) -{ - return node == getGoal(); -} - template typename AStarAlgorithm::NodePtr & AStarAlgorithm::getStart() { return _start; } -template -typename AStarAlgorithm::NodePtr & AStarAlgorithm::getGoal() -{ - return _goal; -} - template typename AStarAlgorithm::NodePtr AStarAlgorithm::getNextNode() { @@ -426,9 +485,7 @@ float AStarAlgorithm::getHeuristicCost(const NodePtr & node) { const Coordinates node_coords = NodeT::getCoords(node->getIndex(), getSizeX(), getSizeDim3()); - float heuristic = NodeT::getHeuristicCost( - node_coords, _goal_coordinates); - + float heuristic = NodeT::getHeuristicCost(node_coords, _goal_manager.getGoalsCoordinates()); if (heuristic < _best_heuristic_node.first) { _best_heuristic_node = {heuristic, node->getIndex()}; } @@ -493,6 +550,18 @@ unsigned int & AStarAlgorithm::getSizeDim3() return _dim3_size; } +template +unsigned int AStarAlgorithm::getCoarseSearchResolution() +{ + return _coarse_search_resolution; +} + +template +typename AStarAlgorithm::GoalManagerT AStarAlgorithm::getGoalManager() +{ + return _goal_manager; +} + // Instantiate algorithm for the supported template types template class AStarAlgorithm; template class AStarAlgorithm; diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp index ec0ccc31f02..13df5feb6af 100644 --- a/nav2_smac_planner/src/analytic_expansion.cpp +++ b/nav2_smac_planner/src/analytic_expansion.cpp @@ -12,10 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. Reserved. -#include -#include -#include - #include #include #include @@ -48,7 +44,10 @@ void AnalyticExpansion::setCollisionChecker( template typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalyticExpansion( - const NodePtr & current_node, const NodePtr & goal_node, + const NodePtr & current_node, + const NodeVector & coarse_check_goals, + const NodeVector & fine_check_goals, + const CoordinateVector & goals_coords, const NodeGetter & getter, int & analytic_iterations, int & closest_distance) { @@ -60,10 +59,15 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic const Coordinates node_coords = NodeT::getCoords( current_node->getIndex(), _collision_checker->getCostmap()->getSizeInCellsX(), _dim_3_size); + + AnalyticExpansionNodes current_best_analytic_nodes; + NodePtr current_best_goal = nullptr; + NodePtr current_best_node = nullptr; + float current_best_score = std::numeric_limits::max(); + closest_distance = std::min( closest_distance, - static_cast(NodeT::getHeuristicCost(node_coords, goal_node->pose))); - + static_cast(NodeT::getHeuristicCost(node_coords, goals_coords))); // We want to expand at a rate of d/expansion_ratio, // but check to see if we are so close that we would be expanding every iteration // If so, limit it to the expansion ratio (rounded up) @@ -80,81 +84,57 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic if (analytic_iterations <= 0) { // Reset the counter and try the analytic path expansion analytic_iterations = desired_iterations; - AnalyticExpansionNodes analytic_nodes = - getAnalyticPath(current_node, goal_node, getter, current_node->motion_table.state_space); - if (!analytic_nodes.empty()) { - // If we have a valid path, attempt to refine it - NodePtr node = current_node; - NodePtr test_node = current_node; - AnalyticExpansionNodes refined_analytic_nodes; - for (int i = 0; i < 8; i++) { - // Attempt to create better paths in 5 node increments, need to make sure - // they exist for each in order to do so (maximum of 40 points back). - if (test_node->parent && test_node->parent->parent && test_node->parent->parent->parent && - test_node->parent->parent->parent->parent && - test_node->parent->parent->parent->parent->parent) - { - test_node = test_node->parent->parent->parent->parent->parent; - refined_analytic_nodes = - getAnalyticPath(test_node, goal_node, getter, test_node->motion_table.state_space); - if (refined_analytic_nodes.empty()) { - break; - } - analytic_nodes = refined_analytic_nodes; - node = test_node; - } else { - break; + bool found_valid_expansion = false; + + // First check the coarse search resolution goals + for (auto & current_goal_node : coarse_check_goals) { + AnalyticExpansionNodes analytic_nodes = + getAnalyticPath( + current_node, current_goal_node, getter, + current_node->motion_table.state_space); + if (!analytic_nodes.nodes.empty()) { + found_valid_expansion = true; + NodePtr node = current_node; + float score = refineAnalyticPath( + node, current_goal_node, getter, analytic_nodes); + // Update the best score if we found a better path + if (score < current_best_score) { + current_best_analytic_nodes = analytic_nodes; + current_best_goal = current_goal_node; + current_best_score = score; + current_best_node = node; } } + } - // The analytic expansion can short-cut near obstacles when closer to a goal - // So, we can attempt to refine it more by increasing the possible radius - // higher than the minimum turning radius and use the best solution based on - // a scoring function similar to that used in traversal cost estimation. - auto scoringFn = [&](const AnalyticExpansionNodes & expansion) { - if (expansion.size() < 2) { - return std::numeric_limits::max(); - } - - float score = 0.0; - float normalized_cost = 0.0; - // Analytic expansions are consistently spaced - const float distance = hypotf( - expansion[1].proposed_coords.x - expansion[0].proposed_coords.x, - expansion[1].proposed_coords.y - expansion[0].proposed_coords.y); - const float & weight = expansion[0].node->motion_table.cost_penalty; - for (auto iter = expansion.begin(); iter != expansion.end(); ++iter) { - normalized_cost = iter->node->getCost() / 252.0f; - // Search's Traversal Cost Function - score += distance * (1.0 + weight * normalized_cost); + // perform a final search if we found a goal + if (found_valid_expansion) { + for (auto & current_goal_node : fine_check_goals) { + AnalyticExpansionNodes analytic_nodes = + getAnalyticPath( + current_node, current_goal_node, getter, + current_node->motion_table.state_space); + if (!analytic_nodes.nodes.empty()) { + NodePtr node = current_node; + float score = refineAnalyticPath( + node, current_goal_node, getter, analytic_nodes); + // Update the best score if we found a better path + if (score < current_best_score) { + current_best_analytic_nodes = analytic_nodes; + current_best_goal = current_goal_node; + current_best_score = score; + current_best_node = node; } - return score; - }; - - float best_score = scoringFn(analytic_nodes); - float score = std::numeric_limits::max(); - float min_turn_rad = node->motion_table.min_turning_radius; - const float max_min_turn_rad = 4.0 * min_turn_rad; // Up to 4x the turning radius - while (min_turn_rad < max_min_turn_rad) { - min_turn_rad += 0.5; // In Grid Coords, 1/2 cell steps - ompl::base::StateSpacePtr state_space; - if (node->motion_table.motion_model == MotionModel::DUBIN) { - state_space = std::make_shared(min_turn_rad); - } else { - state_space = std::make_shared(min_turn_rad); - } - refined_analytic_nodes = getAnalyticPath(node, goal_node, getter, state_space); - score = scoringFn(refined_analytic_nodes); - if (score <= best_score) { - analytic_nodes = refined_analytic_nodes; - best_score = score; } } - - return setAnalyticPath(node, goal_node, analytic_nodes); } } + if (!current_best_analytic_nodes.nodes.empty()) { + return setAnalyticPath( + current_best_node, current_best_goal, + current_best_analytic_nodes); + } analytic_iterations--; } @@ -162,6 +142,28 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic return NodePtr(nullptr); } +template +int AnalyticExpansion::countDirectionChanges( + const ompl::base::ReedsSheppStateSpace::ReedsSheppPath & path) +{ + const double * lengths = path.length_; + int changes = 0; + int last_dir = 0; + for (int i = 0; i < 5; ++i) { + if (lengths[i] == 0.0) { + continue; + } + + int currentDirection = (lengths[i] > 0.0) ? 1 : -1; + if (last_dir != 0 && currentDirection != last_dir) { + ++changes; + } + last_dir = currentDirection; + } + + return changes; +} + template typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansion::getAnalyticPath( const NodePtr & node, @@ -179,6 +181,12 @@ typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansiondistance(from(), to()); + auto rs_state_space = dynamic_cast(state_space.get()); + int direction_changes = 0; + if (rs_state_space) { + direction_changes = countDirectionChanges(rs_state_space->reedsShepp(from.get(), to.get())); + } + // A move of sqrt(2) is guaranteed to be in a new cell static const float sqrt_2 = sqrtf(2.0f); @@ -195,7 +203,7 @@ typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansion reals; double theta; @@ -230,7 +238,7 @@ typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansionsetPose(proposed_coordinates); if (next->isNodeValid(_traverse_unknown, _collision_checker) && next != prev) { // Save the node, and its previous coordinates in case we need to abort - possible_nodes.emplace_back(next, initial_node_coords, proposed_coordinates); + possible_nodes.add(next, initial_node_coords, proposed_coordinates); node_costs.emplace_back(next->getCost()); prev = next; } else { @@ -280,7 +288,7 @@ typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansionsetPose(node_pose.initial_coords); } @@ -289,9 +297,109 @@ typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansion +float AnalyticExpansion::refineAnalyticPath( + NodePtr & node, + const NodePtr & goal_node, + const NodeGetter & getter, + AnalyticExpansionNodes & analytic_nodes) +{ + NodePtr test_node = node; + AnalyticExpansionNodes refined_analytic_nodes; + for (int i = 0; i < 8; i++) { + // Attempt to create better paths in 5 node increments, need to make sure + // they exist for each in order to do so (maximum of 40 points back). + if (test_node->parent && test_node->parent->parent && + test_node->parent->parent->parent && + test_node->parent->parent->parent->parent && + test_node->parent->parent->parent->parent->parent) + { + test_node = test_node->parent->parent->parent->parent->parent; + // print the goals pose + refined_analytic_nodes = + getAnalyticPath( + test_node, goal_node, getter, + test_node->motion_table.state_space); + if (refined_analytic_nodes.nodes.empty()) { + break; + } + if (refined_analytic_nodes.direction_changes > analytic_nodes.direction_changes) { + // If the direction changes are worse, we don't want to use this path + continue; + } + analytic_nodes = refined_analytic_nodes; + node = test_node; + } else { + break; + } + } + + // The analytic expansion can short-cut near obstacles when closer to a goal + // So, we can attempt to refine it more by increasing the possible radius + // higher than the minimum turning radius and use the best solution based on + // a scoring function similar to that used in traversal cost estimation. + auto scoringFn = [&](const AnalyticExpansionNodes & expansion) { + if (expansion.nodes.size() < 2) { + return std::numeric_limits::max(); + } + + float score = 0.0; + float normalized_cost = 0.0; + // Analytic expansions are consistently spaced + const float distance = hypotf( + expansion.nodes[1].proposed_coords.x - expansion.nodes[0].proposed_coords.x, + expansion.nodes[1].proposed_coords.y - expansion.nodes[0].proposed_coords.y); + const float & weight = expansion.nodes[0].node->motion_table.cost_penalty; + for (auto iter = expansion.nodes.begin(); iter != expansion.nodes.end(); ++iter) { + normalized_cost = iter->node->getCost() / 252.0f; + // Search's Traversal Cost Function + score += distance * (1.0 + weight * normalized_cost); + } + return score; + }; + + float original_score = scoringFn(analytic_nodes); + float best_score = original_score; + float score = std::numeric_limits::max(); + float min_turn_rad = node->motion_table.min_turning_radius; + const float max_min_turn_rad = 4.0 * min_turn_rad; // Up to 4x the turning radius + while (min_turn_rad < max_min_turn_rad) { + min_turn_rad += 0.5; // In Grid Coords, 1/2 cell steps + ompl::base::StateSpacePtr state_space; + if (node->motion_table.motion_model == MotionModel::DUBIN) { + state_space = std::make_shared(min_turn_rad); + } else { + state_space = std::make_shared(min_turn_rad); + } + refined_analytic_nodes = getAnalyticPath(node, goal_node, getter, state_space); + score = scoringFn(refined_analytic_nodes); + + // Normal scoring: prioritize lower cost as long as not more directional changes + if (score <= best_score && + refined_analytic_nodes.direction_changes <= analytic_nodes.direction_changes) + { + analytic_nodes = refined_analytic_nodes; + best_score = score; + continue; + } + + // Special case: If we have a better score than original (only) and less directional changes + // the path quality is still better than the original and is less operationally complex + if (score <= original_score && + refined_analytic_nodes.direction_changes < analytic_nodes.direction_changes) + { + analytic_nodes = refined_analytic_nodes; + best_score = score; + } + } + + return best_score; +} + template typename AnalyticExpansion::NodePtr AnalyticExpansion::setAnalyticPath( const NodePtr & node, @@ -301,7 +409,7 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::setAnalytic _detached_nodes.clear(); // Legitimate final path - set the parent relationships, states, and poses NodePtr prev = node; - for (const auto & node_pose : expanded_nodes) { + for (const auto & node_pose : expanded_nodes.nodes) { auto n = node_pose.node; cleanNode(n); if (n->getIndex() != goal_node->getIndex()) { @@ -345,6 +453,16 @@ getAnalyticPath( return AnalyticExpansionNodes(); } +template<> +float AnalyticExpansion::refineAnalyticPath( + NodePtr &, + const NodePtr &, + const NodeGetter &, + AnalyticExpansionNodes &) +{ + return std::numeric_limits::max(); +} + template<> typename AnalyticExpansion::NodePtr AnalyticExpansion::setAnalyticPath( const NodePtr &, @@ -356,7 +474,10 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::setAnalyt template<> typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalyticExpansion( - const NodePtr &, const NodePtr &, + const NodePtr &, + const NodeVector &, + const NodeVector &, + const CoordinateVector &, const NodeGetter &, int &, int &) { diff --git a/nav2_smac_planner/src/node_2d.cpp b/nav2_smac_planner/src/node_2d.cpp index bf64ae29e3e..11fdf8b3b41 100644 --- a/nav2_smac_planner/src/node_2d.cpp +++ b/nav2_smac_planner/src/node_2d.cpp @@ -85,12 +85,12 @@ float Node2D::getTraversalCost(const NodePtr & child) float Node2D::getHeuristicCost( const Coordinates & node_coords, - const Coordinates & goal_coordinates) + const CoordinateVector & goals_coords) { // Using Moore distance as it more accurately represents the distances // even a Van Neumann neighborhood robot can navigate. - auto dx = goal_coordinates.x - node_coords.x; - auto dy = goal_coordinates.y - node_coords.y; + auto dx = goals_coords[0].x - node_coords.x; + auto dy = goals_coords[0].y - node_coords.y; return std::sqrt(dx * dx + dy * dy); } diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp index 9d46ab0f0ff..bbbae87ae7c 100644 --- a/nav2_smac_planner/src/node_hybrid.cpp +++ b/nav2_smac_planner/src/node_hybrid.cpp @@ -442,12 +442,18 @@ float NodeHybrid::getTraversalCost(const NodePtr & child) float NodeHybrid::getHeuristicCost( const Coordinates & node_coords, - const Coordinates & goal_coords) + const CoordinateVector & goals_coords) { + // obstacle heuristic does not depend on goal heading const float obstacle_heuristic = - getObstacleHeuristic(node_coords, goal_coords, motion_table.cost_penalty); - const float dist_heuristic = getDistanceHeuristic(node_coords, goal_coords, obstacle_heuristic); - return std::max(obstacle_heuristic, dist_heuristic); + getObstacleHeuristic(node_coords, goals_coords[0], motion_table.cost_penalty); + float distance_heuristic = std::numeric_limits::max(); + for (unsigned int i = 0; i < goals_coords.size(); i++) { + distance_heuristic = std::min( + distance_heuristic, + getDistanceHeuristic(node_coords, goals_coords[i], obstacle_heuristic)); + } + return std::max(obstacle_heuristic, distance_heuristic); } void NodeHybrid::initMotionModel( diff --git a/nav2_smac_planner/src/node_lattice.cpp b/nav2_smac_planner/src/node_lattice.cpp index fc03969e978..d9a7c8ebfa5 100644 --- a/nav2_smac_planner/src/node_lattice.cpp +++ b/nav2_smac_planner/src/node_lattice.cpp @@ -345,13 +345,18 @@ float NodeLattice::getTraversalCost(const NodePtr & child) float NodeLattice::getHeuristicCost( const Coordinates & node_coords, - const Coordinates & goal_coords) + const CoordinateVector & goals_coords) { // get obstacle heuristic value + // obstacle heuristic does not depend on goal heading const float obstacle_heuristic = getObstacleHeuristic( - node_coords, goal_coords, motion_table.cost_penalty); - const float distance_heuristic = - getDistanceHeuristic(node_coords, goal_coords, obstacle_heuristic); + node_coords, goals_coords[0], motion_table.cost_penalty); + float distance_heuristic = std::numeric_limits::max(); + for (unsigned int i = 0; i < goals_coords.size(); i++) { + distance_heuristic = std::min( + distance_heuristic, + getDistanceHeuristic(node_coords, goals_coords[i], obstacle_heuristic)); + } return std::max(obstacle_heuristic, distance_heuristic); } @@ -467,7 +472,7 @@ void NodeLattice::precomputeDistanceHeuristic( int dim_3_size_int = static_cast(dim_3_size); // Create a lookup table of Dubin/Reeds-Shepp distances in a window around the goal - // to help drive the search towards admissible approaches. Deu to symmetries in the + // to help drive the search towards admissible approaches. Due to symmetries in the // Heuristic space, we need to only store 2 of the 4 quadrants and simply mirror // around the X axis any relative node lookup. This reduces memory overhead and increases // the size of a window a platform can store in memory. diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index f3ece68c565..d6f4ec26fb8 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -171,7 +171,24 @@ void SmacPlannerHybrid::configure( nav2_util::declare_parameter_if_not_declared( node, "service_introspection_mode", rclcpp::ParameterValue("disabled")); + std::string goal_heading_type; + nav2_util::declare_parameter_if_not_declared( + node, name + ".goal_heading_mode", rclcpp::ParameterValue("DEFAULT")); + node->get_parameter(name + ".goal_heading_mode", goal_heading_type); + _goal_heading_mode = fromStringToGH(goal_heading_type); + + nav2_util::declare_parameter_if_not_declared( + node, name + ".coarse_search_resolution", rclcpp::ParameterValue(1)); + node->get_parameter(name + ".coarse_search_resolution", _coarse_search_resolution); + + if (_goal_heading_mode == GoalHeadingMode::UNKNOWN) { + std::string error_msg = "Unable to get GoalHeader type. Given '" + goal_heading_type + "' " + "Valid options are DEFAULT, BIDIRECTIONAL, ALL_DIRECTION. "; + throw nav2_core::PlannerException(error_msg); + } + _motion_model = fromString(_motion_model_for_search); + if (_motion_model == MotionModel::UNKNOWN) { RCLCPP_WARN( _logger, @@ -194,6 +211,21 @@ void SmacPlannerHybrid::configure( _max_iterations = std::numeric_limits::max(); } + if (_coarse_search_resolution <= 0) { + RCLCPP_WARN( + _logger, "coarse iteration resolution selected as <= 0, " + "disabling coarse iteration resolution search for goal heading" + ); + + _coarse_search_resolution = 1; + } + + if (_angle_quantizations % _coarse_search_resolution != 0) { + std::string error_msg = "coarse iteration should be an increment" + " of the number of angular bins configured"; + throw nav2_core::PlannerException(error_msg); + } + if (_minimum_turning_radius_global_coords < _costmap->getResolution() * _downsampling_factor) { RCLCPP_WARN( _logger, "Min turning radius cannot be less than the search grid cell resolution!"); @@ -413,7 +445,8 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( if (orientation_bin >= static_cast(_angle_quantizations)) { orientation_bin -= static_cast(_angle_quantizations); } - _a_star->setGoal(mx_goal, my_goal, static_cast(orientation_bin)); + _a_star->setGoal(mx_goal, my_goal, static_cast(orientation_bin), + _goal_heading_mode, _coarse_search_resolution); // Setup message nav_msgs::msg::Path plan; @@ -700,6 +733,31 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para int angle_quantizations = parameter.as_int(); _angle_bin_size = 2.0 * M_PI / angle_quantizations; _angle_quantizations = static_cast(angle_quantizations); + + if (_angle_quantizations % _coarse_search_resolution != 0) { + RCLCPP_WARN( + _logger, "coarse iteration should be an increment of the " + "number of angular bins configured. Disabling course research!" + ); + _coarse_search_resolution = 1; + } + } else if (param_name == _name + ".coarse_search_resolution") { + _coarse_search_resolution = parameter.as_int(); + if (_coarse_search_resolution <= 0) { + RCLCPP_WARN( + _logger, "coarse iteration resolution selected as <= 0. " + "Disabling course research!" + ); + _coarse_search_resolution = 1; + } + if (_angle_quantizations % _coarse_search_resolution != 0) { + RCLCPP_WARN( + _logger, + "coarse iteration should be an increment of the " + "number of angular bins configured. Disabling course research!" + ); + _coarse_search_resolution = 1; + } } } else if (param_type == ParameterType::PARAMETER_STRING) { if (param_name == _name + ".motion_model_for_search") { @@ -712,6 +770,22 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para "valid options are MOORE, VON_NEUMANN, DUBIN, REEDS_SHEPP.", _motion_model_for_search.c_str()); } + } else if (param_name == _name + ".goal_heading_mode") { + std::string goal_heading_type = parameter.as_string(); + GoalHeadingMode goal_heading_mode = fromStringToGH(goal_heading_type); + RCLCPP_INFO( + _logger, + "GoalHeadingMode type set to '%s'.", + goal_heading_type.c_str()); + if (goal_heading_mode == GoalHeadingMode::UNKNOWN) { + RCLCPP_WARN( + _logger, + "Unable to get GoalHeader type. Given '%s', " + "Valid options are DEFAULT, BIDIRECTIONAL, ALL_DIRECTION. ", + goal_heading_type.c_str()); + } else { + _goal_heading_mode = goal_heading_mode; + } } } } diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index bd68ed8a3ed..4968068e534 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -140,6 +140,22 @@ void SmacPlannerLattice::configure( node, name + ".debug_visualizations", rclcpp::ParameterValue(false)); node->get_parameter(name + ".debug_visualizations", _debug_visualizations); + std::string goal_heading_type; + nav2_util::declare_parameter_if_not_declared( + node, name + ".goal_heading_mode", rclcpp::ParameterValue("DEFAULT")); + node->get_parameter(name + ".goal_heading_mode", goal_heading_type); + _goal_heading_mode = fromStringToGH(goal_heading_type); + + nav2_util::declare_parameter_if_not_declared( + node, name + ".coarse_search_resolution", rclcpp::ParameterValue(1)); + node->get_parameter(name + ".coarse_search_resolution", _coarse_search_resolution); + + if (_goal_heading_mode == GoalHeadingMode::UNKNOWN) { + std::string error_msg = "Unable to get GoalHeader type. Given '" + goal_heading_type + "' " + "Valid options are DEFAULT, BIDIRECTIONAL, ALL_DIRECTION. "; + throw nav2_core::PlannerException(error_msg); + } + _metadata = LatticeMotionTable::getLatticeMetadata(_search_info.lattice_filepath); _search_info.minimum_turning_radius = _metadata.min_turning_radius / (_costmap->getResolution()); @@ -159,6 +175,20 @@ void SmacPlannerLattice::configure( _max_iterations = std::numeric_limits::max(); } + if (_coarse_search_resolution <= 0) { + RCLCPP_WARN( + _logger, "coarse iteration resolution selected as <= 0, " + "disabling coarse iteration resolution search for goal heading" + ); + _coarse_search_resolution = 1; + } + + if (_metadata.number_of_headings % _coarse_search_resolution != 0) { + std::string error_msg = "coarse iteration should be an increment of" + " the number of angular bins configured"; + throw nav2_core::PlannerException(error_msg); + } + float lookup_table_dim = static_cast(_lookup_table_size) / static_cast(_costmap->getResolution()); @@ -325,7 +355,8 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( } _a_star->setGoal( mx_goal, my_goal, - NodeLattice::motion_table.getClosestAngularBin(tf2::getYaw(goal.pose.orientation))); + NodeLattice::motion_table.getClosestAngularBin(tf2::getYaw(goal.pose.orientation)), + _goal_heading_mode, _coarse_search_resolution); // Setup message nav_msgs::msg::Path plan; @@ -593,6 +624,23 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector par } else if (param_name == _name + ".terminal_checking_interval") { reinit_a_star = true; _terminal_checking_interval = parameter.as_int(); + } else if (param_name == _name + ".coarse_search_resolution") { + _coarse_search_resolution = parameter.as_int(); + if (_coarse_search_resolution <= 0) { + RCLCPP_WARN( + _logger, "coarse iteration resolution selected as <= 0. " + "Disabling course research!" + ); + _coarse_search_resolution = 1; + } + if (_metadata.number_of_headings % _coarse_search_resolution != 0) { + RCLCPP_WARN( + _logger, + "coarse iteration should be an increment of the number<" + " of angular bins configured. Disabling course research!" + ); + _coarse_search_resolution = 1; + } } } else if (param_type == ParameterType::PARAMETER_STRING) { if (param_name == _name + ".lattice_filepath") { @@ -604,6 +652,29 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector par _metadata = LatticeMotionTable::getLatticeMetadata(_search_info.lattice_filepath); _search_info.minimum_turning_radius = _metadata.min_turning_radius / (_costmap->getResolution()); + if (_metadata.number_of_headings % _coarse_search_resolution != 0) { + RCLCPP_WARN( + _logger, "coarse iteration should be an increment of the number " + "of angular bins configured. Disabling course research!" + ); + _coarse_search_resolution = 1; + } + } else if (param_name == _name + ".goal_heading_mode") { + std::string goal_heading_type = parameter.as_string(); + GoalHeadingMode goal_heading_mode = fromStringToGH(goal_heading_type); + RCLCPP_INFO( + _logger, + "GoalHeadingMode type set to '%s'.", + goal_heading_type.c_str()); + if (goal_heading_mode == GoalHeadingMode::UNKNOWN) { + RCLCPP_WARN( + _logger, + "Unable to get GoalHeader type. Given '%s', " + "Valid options are DEFAULT, BIDIRECTIONAL, ALL_DIRECTION. ", + goal_heading_type.c_str()); + } else { + _goal_heading_mode = goal_heading_mode; + } } } } diff --git a/nav2_smac_planner/test/CMakeLists.txt b/nav2_smac_planner/test/CMakeLists.txt index fc4abf20c75..24c86d9c24f 100644 --- a/nav2_smac_planner/test/CMakeLists.txt +++ b/nav2_smac_planner/test/CMakeLists.txt @@ -135,3 +135,13 @@ target_link_libraries(test_lattice_node rclcpp::rclcpp rclcpp_lifecycle::rclcpp_lifecycle ) + + +# Test Goal Manager +ament_add_gtest(test_goal_manager test_goal_manager.cpp) +target_link_libraries(test_goal_manager + ${library_name} + nav2_costmap_2d::nav2_costmap_2d_core + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle +) diff --git a/nav2_smac_planner/test/test_a_star.cpp b/nav2_smac_planner/test/test_a_star.cpp index df8223ef9b5..701e7ad01ac 100644 --- a/nav2_smac_planner/test/test_a_star.cpp +++ b/nav2_smac_planner/test/test_a_star.cpp @@ -123,13 +123,32 @@ TEST(AStarTest, test_a_star_2d) } EXPECT_TRUE(a_star_2.getStart() != nullptr); - EXPECT_TRUE(a_star_2.getGoal() != nullptr); + EXPECT_NE(a_star_2.getGoalManager().getGoalsState().size(), 0); EXPECT_EQ(a_star_2.getSizeX(), 100u); EXPECT_EQ(a_star_2.getSizeY(), 100u); EXPECT_EQ(a_star_2.getSizeDim3(), 1u); EXPECT_EQ(a_star_2.getToleranceHeuristic(), 20.0); EXPECT_EQ(a_star_2.getOnApproachMaxIterations(), 10); + // test unused functions + nav2_smac_planner::AnalyticExpansion expander( + nav2_smac_planner::MotionModel::TWOD, info, false, 1); + + auto analytic_expansion_nodes = + nav2_smac_planner::AnalyticExpansion::AnalyticExpansionNodes(); + EXPECT_EQ(expander.setAnalyticPath(nullptr, nullptr, analytic_expansion_nodes), nullptr); + int dummy_int1 = 0; + int dummy_int2 = 0; + EXPECT_EQ(expander.tryAnalyticExpansion(nullptr, {}, {}, {}, + nullptr, dummy_int1, dummy_int2), nullptr); + + nav2_smac_planner::Node2D * start = nullptr; + EXPECT_EQ(expander.refineAnalyticPath(start, nullptr, nullptr, + analytic_expansion_nodes), std::numeric_limits::max()); + nav2_smac_planner::AnalyticExpansion::AnalyticExpansionNodes + expected_nodes = expander.getAnalyticPath(nullptr, nullptr, nullptr, nullptr); + EXPECT_EQ(expected_nodes.nodes.size(), 0); + delete costmapA; } @@ -391,10 +410,17 @@ TEST(AStarTest, test_se2_single_pose_path) }; // functional case testing a_star.setCollisionChecker(checker.get()); + nav2_smac_planner::NodeHybrid::CoordinateVector path; + + // test with no goals set nor start + EXPECT_THROW( + a_star.createPath( + path, num_it, tolerance, + dummy_cancel_checker), std::runtime_error); + a_star.setStart(10u, 10u, 0u); // Goal is one costmap cell away a_star.setGoal(12u, 10u, 0u); - nav2_smac_planner::NodeHybrid::CoordinateVector path; EXPECT_TRUE(a_star.createPath(path, num_it, tolerance, dummy_cancel_checker)); // Check that the path is length one @@ -406,6 +432,102 @@ TEST(AStarTest, test_se2_single_pose_path) nav2_smac_planner::NodeHybrid::destroyStaticAssets(); } +TEST(AStarTest, test_goal_heading_mode) +{ + auto lnode = std::make_shared("test"); + nav2_smac_planner::SearchInfo info; + info.change_penalty = 0.1; + info.non_straight_penalty = 1.1; + info.reverse_penalty = 2.0; + info.minimum_turning_radius = 8; // in grid coordinates + info.retrospective_penalty = 0.015; + info.analytic_expansion_max_length = 20.0; // in grid coordinates + info.analytic_expansion_ratio = 3.5; + unsigned int size_theta = 72; + info.cost_penalty = 1.7; + nav2_smac_planner::AStarAlgorithm a_star( + nav2_smac_planner::MotionModel::DUBIN, info); + int max_iterations = 10000; + float tolerance = 10.0; + int it_on_approach = 10; + int terminal_checking_interval = 5000; + double max_planning_time = 120.0; + int num_it = 0; + + // BIDIRECTIONAL goal heading mode + a_star.initialize( + false, max_iterations, it_on_approach, terminal_checking_interval, + max_planning_time, 401, size_theta); + + nav2_costmap_2d::Costmap2D * costmapA = + new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0); + + // Convert raw costmap into a costmap ros object + auto costmap_ros = std::make_shared(); + costmap_ros->on_configure(rclcpp_lifecycle::State()); + auto costmap = costmap_ros->getCostmap(); + *costmap = *costmapA; + + std::unique_ptr checker = + std::make_unique(costmap_ros, size_theta, lnode); + checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); + + a_star.setCollisionChecker(checker.get()); + + EXPECT_THROW( + a_star.setGoal( + 80u, 80u, 40u, + nav2_smac_planner::GoalHeadingMode::BIDIRECTIONAL), + std::runtime_error); + a_star.setStart(10u, 10u, 0u); + a_star.setGoal(80u, 80u, 40u, nav2_smac_planner::GoalHeadingMode::BIDIRECTIONAL); + nav2_smac_planner::NodeHybrid::CoordinateVector path; + std::unique_ptr>> expansions = nullptr; + expansions = std::make_unique>>(); + + auto dummy_cancel_checker = []() { + return false; + }; + EXPECT_TRUE(a_star.createPath(path, num_it, tolerance, dummy_cancel_checker, expansions.get())); + EXPECT_EQ(a_star.getGoalManager().getGoalsState().size(), 2); + EXPECT_EQ(a_star.getGoalManager().getGoalsState().size(), + a_star.getGoalManager().getGoalsCoordinates().size()); + + + // ALL_DIRECTION goal heading mode + unsigned int coarse_search_resolution = 16; + a_star.setCollisionChecker(checker.get()); + a_star.setStart(10u, 10u, 0u); + a_star.setGoal(80u, 80u, 40u, nav2_smac_planner::GoalHeadingMode::ALL_DIRECTION, + coarse_search_resolution); + EXPECT_TRUE(a_star.getCoarseSearchResolution() == coarse_search_resolution); + + unsigned int num_bins = nav2_smac_planner::NodeHybrid::motion_table.num_angle_quantization; + + // get number of valid goal states + unsigned int num_valid_goals = 0; + auto goals_state = a_star.getGoalManager().getGoalsState(); + for (unsigned int i = 0; i < goals_state.size(); i++) { + if(goals_state[i].is_valid) { + num_valid_goals++; + } + } + EXPECT_TRUE(a_star.getGoalManager().getGoalsState().size() == num_bins); + EXPECT_TRUE(a_star.getGoalManager().getGoalsState().size() == num_valid_goals); + EXPECT_TRUE(a_star.createPath(path, num_it, tolerance, dummy_cancel_checker, expansions.get())); + EXPECT_TRUE(a_star.getGoalManager().getGoalsState().size() == + a_star.getGoalManager().getGoalsCoordinates().size()); + + // UNKNOWN goal heading mode + a_star.setCollisionChecker(checker.get()); + a_star.setStart(10u, 10u, 0u); + + EXPECT_THROW( + a_star.setGoal( + 80u, 80u, 10u, + nav2_smac_planner::GoalHeadingMode::UNKNOWN), std::runtime_error); +} + TEST(AStarTest, test_constants) { nav2_smac_planner::MotionModel mm = nav2_smac_planner::MotionModel::UNKNOWN; // unknown @@ -417,6 +539,15 @@ TEST(AStarTest, test_constants) mm = nav2_smac_planner::MotionModel::REEDS_SHEPP; // reeds-shepp EXPECT_EQ(nav2_smac_planner::toString(mm), std::string("Reeds-Shepp")); + nav2_smac_planner::GoalHeadingMode gh = nav2_smac_planner::GoalHeadingMode::UNKNOWN; + EXPECT_EQ(nav2_smac_planner::toString(gh), std::string("Unknown")); + gh = nav2_smac_planner::GoalHeadingMode::DEFAULT; // default + EXPECT_EQ(nav2_smac_planner::toString(gh), std::string("DEFAULT")); + gh = nav2_smac_planner::GoalHeadingMode::BIDIRECTIONAL; // bidirectional + EXPECT_EQ(nav2_smac_planner::toString(gh), std::string("BIDIRECTIONAL")); + gh = nav2_smac_planner::GoalHeadingMode::ALL_DIRECTION; // all_direction + EXPECT_EQ(nav2_smac_planner::toString(gh), std::string("ALL_DIRECTION")); + EXPECT_EQ( nav2_smac_planner::fromString( "2D"), nav2_smac_planner::MotionModel::TWOD); @@ -425,6 +556,18 @@ TEST(AStarTest, test_constants) nav2_smac_planner::fromString( "REEDS_SHEPP"), nav2_smac_planner::MotionModel::REEDS_SHEPP); EXPECT_EQ(nav2_smac_planner::fromString("NONE"), nav2_smac_planner::MotionModel::UNKNOWN); + + EXPECT_EQ( + nav2_smac_planner::fromStringToGH( + "DEFAULT"), nav2_smac_planner::GoalHeadingMode::DEFAULT); + EXPECT_EQ( + nav2_smac_planner::fromStringToGH( + "BIDIRECTIONAL"), nav2_smac_planner::GoalHeadingMode::BIDIRECTIONAL); + EXPECT_EQ( + nav2_smac_planner::fromStringToGH( + "ALL_DIRECTION"), nav2_smac_planner::GoalHeadingMode::ALL_DIRECTION); + EXPECT_EQ( + nav2_smac_planner::fromStringToGH("NONE"), nav2_smac_planner::GoalHeadingMode::UNKNOWN); } int main(int argc, char ** argv) diff --git a/nav2_smac_planner/test/test_goal_manager.cpp b/nav2_smac_planner/test/test_goal_manager.cpp new file mode 100644 index 00000000000..80f71b3a688 --- /dev/null +++ b/nav2_smac_planner/test/test_goal_manager.cpp @@ -0,0 +1,186 @@ +// Copyright (c) 2023 Open Navigation LLC +// Copyright (c) 2024 Stevedan Ogochukwu Omodolor Omodia +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_smac_planner/goal_manager.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_costmap_2d/costmap_subscriber.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_smac_planner/collision_checker.hpp" + +using namespace nav2_smac_planner; // NOLINT + +using NodeT = NodeHybrid; +using NodePtr = NodeT *; +using GoalManagerHybrid = GoalManager; +using NodeVector = GoalManagerHybrid::NodeVector; +using CoordinateVector = GoalManagerHybrid::CoordinateVector; + +TEST(GoalManagerTest, test_goal_manager) +{ + auto node = std::make_shared("test_node"); + + auto costmapA = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0); + + // Create an island of lethal cost in the middle + for (unsigned int i = 40; i <= 60; ++i) { + for (unsigned int j = 40; j <= 60; ++j) { + costmapA->setCost(i, j, 254); + } + } + + // Convert raw costmap into a costmap ros object + auto costmap_ros = std::make_shared(); + costmap_ros->on_configure(rclcpp_lifecycle::State()); + auto costmap = costmap_ros->getCostmap(); + *costmap = *costmapA; + + auto checker = std::make_unique( + costmap_ros, 72, node); + checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); + + GoalManagerHybrid goal_manager; + float tolerance = 20.0f; + bool allow_unknow = false; + + EXPECT_TRUE(goal_manager.goalsIsEmpty()); + + // Create two valid goals + NodePtr pose_a = new NodeHybrid(48); + NodePtr pose_b = new NodeHybrid(49); + pose_a->setPose(NodeHybrid::Coordinates(0, 0, 0)); + pose_b->setPose(NodeHybrid::Coordinates(0, 0, 10)); + + goal_manager.addGoal(pose_a); + goal_manager.addGoal(pose_b); + + EXPECT_FALSE(goal_manager.goalsIsEmpty()); + EXPECT_EQ(goal_manager.getGoalsState().size(), 2u); + EXPECT_TRUE(goal_manager.getGoalsSet().empty()); + EXPECT_TRUE(goal_manager.getGoalsCoordinates().empty()); + + goal_manager.removeInvalidGoals(tolerance, checker.get(), allow_unknow); + + EXPECT_EQ(goal_manager.getGoalsSet().size(), 2); + EXPECT_EQ(goal_manager.getGoalsCoordinates().size(), 2); + for (const auto & goal_state : goal_manager.getGoalsState()) { + EXPECT_TRUE(goal_state.is_valid); + } + + // Test is goal + EXPECT_TRUE(goal_manager.isGoal(pose_a)); + EXPECT_TRUE(goal_manager.isGoal(pose_b)); + + // Re-populate and validate reset state + goal_manager.clear(); + goal_manager.addGoal(pose_a); + goal_manager.addGoal(pose_b); + EXPECT_EQ(goal_manager.getGoalsSet().size(), 0); + EXPECT_EQ(goal_manager.getGoalsCoordinates().size(), 0); + + // Add invalid goal + NodePtr pose_c = new NodeHybrid(50); + pose_c->setPose(NodeHybrid::Coordinates(50, 50, 0)); // inside lethal zone + + goal_manager.addGoal(pose_c); + EXPECT_EQ(goal_manager.getGoalsState().size(), 3); + + // Tolerance is high, one goal is invalid + // all goals are valid + goal_manager.removeInvalidGoals(tolerance, checker.get(), allow_unknow); + EXPECT_EQ(goal_manager.getGoalsSet().size(), 3); + EXPECT_EQ(goal_manager.getGoalsCoordinates().size(), 3); + + for (const auto & goal_state : goal_manager.getGoalsState()) { + EXPECT_TRUE(goal_state.is_valid); + } + + // Test with low tolerance, expect invalid goal to be filtered out + goal_manager.clear(); + goal_manager.addGoal(pose_a); + goal_manager.addGoal(pose_b); + goal_manager.addGoal(pose_c); + + int low_tolerance = 0.0f; + goal_manager.removeInvalidGoals(low_tolerance, checker.get(), allow_unknow); + + EXPECT_EQ(goal_manager.getGoalsSet().size(), 2); + EXPECT_EQ(goal_manager.getGoalsCoordinates().size(), 2); + + // expect last goal to be invalid + for (const auto & goal_state : goal_manager.getGoalsState()) { + if (goal_state.goal == pose_c) { + EXPECT_FALSE(goal_state.is_valid); + } else { + EXPECT_TRUE(goal_state.is_valid); + } + } + + // Prepare goals for expansion + goal_manager.clear(); + unsigned int test_goal_size = 16; + + for (unsigned int i = 0; i < test_goal_size; ++i) { + NodePtr goal = new NodeHybrid(i); + goal->setPose(NodeHybrid::Coordinates(i, i, 0)); + goal_manager.addGoal(goal); + } + + goal_manager.removeInvalidGoals(tolerance, checker.get(), allow_unknow); + + NodeVector coarse_check_goals; + NodeVector fine_check_goals; + + // Resolution 1: everything coarse + goal_manager.prepareGoalsForAnalyticExpansion(coarse_check_goals, fine_check_goals, 1); + EXPECT_EQ(coarse_check_goals.size(), test_goal_size); + EXPECT_TRUE(fine_check_goals.empty()); + + // Resolution 4: every 4th coarse + coarse_check_goals.clear(); + fine_check_goals.clear(); + + goal_manager.prepareGoalsForAnalyticExpansion(coarse_check_goals, fine_check_goals, 4); + EXPECT_EQ(coarse_check_goals.size(), 4u); // indices 0, 4, 8, 12 + EXPECT_EQ(fine_check_goals.size(), 12u); + + + // expect throw if we try to call removeinvalidgoals twice + // without clearing the goal manager first + EXPECT_THROW(goal_manager.removeInvalidGoals( + tolerance, checker.get(), allow_unknow), + std::runtime_error + ); + + delete costmapA; + nav2_smac_planner::NodeHybrid::destroyStaticAssets(); +} + + +int main(int argc, char **argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + rclcpp::init(0, nullptr); + + int result = RUN_ALL_TESTS(); + + rclcpp::shutdown(); + + return result; +} diff --git a/nav2_smac_planner/test/test_node2d.cpp b/nav2_smac_planner/test/test_node2d.cpp index 816376b814b..70706558809 100644 --- a/nav2_smac_planner/test/test_node2d.cpp +++ b/nav2_smac_planner/test/test_node2d.cpp @@ -67,8 +67,10 @@ TEST(Node2DTest, test_node_2d) // check heuristic cost computation nav2_smac_planner::Node2D::Coordinates A(0.0, 0.0); + nav2_smac_planner::Node2D::CoordinateVector B_vec; nav2_smac_planner::Node2D::Coordinates B(10.0, 5.0); - EXPECT_NEAR(testB.getHeuristicCost(A, B), 11.18, 0.02); + B_vec.push_back(B); + EXPECT_NEAR(testB.getHeuristicCost(A, B_vec), 11.18, 0.02); // check operator== works on index unsigned char costC = '2'; diff --git a/nav2_smac_planner/test/test_smac_hybrid.cpp b/nav2_smac_planner/test/test_smac_hybrid.cpp index 4f521fa71b1..4870ec12ba0 100644 --- a/nav2_smac_planner/test/test_smac_hybrid.cpp +++ b/nav2_smac_planner/test/test_smac_hybrid.cpp @@ -29,6 +29,36 @@ #include "nav2_smac_planner/smac_planner_hybrid.hpp" #include "nav2_smac_planner/smac_planner_2d.hpp" +// Simple wrapper to be able to call a private member +class HybridWrap : public nav2_smac_planner::SmacPlannerHybrid +{ +public: + void callDynamicParams(std::vector parameters) + { + dynamicParametersCallback(parameters); + } + + int getCoarseSearchResolution() + { + return _coarse_search_resolution; + } + + int getMaxIterations() + { + return _max_iterations; + } + + int getMaxOnApproachIterations() + { + return _max_on_approach_iterations; + } + + nav2_smac_planner::GoalHeadingMode getGoalHeadingMode() + { + return _goal_heading_mode; + } +}; + // SMAC smoke tests for plugin-level issues rather than algorithms // (covered by more extensively testing in other files) // System tests in nav2_system_tests will actually plan with this work @@ -59,7 +89,46 @@ TEST(SmacTest, test_smac_se2) goal.pose.position.x = 1.0; goal.pose.position.y = 1.0; goal.pose.orientation.w = 1.0; - auto planner = std::make_unique(); + auto planner = std::make_unique(); + + // invalid goal heading mode + nodeSE2->declare_parameter("test.goal_heading_mode", std::string("UNKNOWN")); + nodeSE2->set_parameter(rclcpp::Parameter("test.goal_heading_mode", std::string("UNKNOWN"))); + EXPECT_THROW(planner->configure(nodeSE2, "test", nullptr, costmap_ros), std::runtime_error); + nodeSE2->set_parameter(rclcpp::Parameter("test.goal_heading_mode", std::string("DEFAULT"))); + + // Invalid motion model should not change the default value + nodeSE2->set_parameter(rclcpp::Parameter("test.motion_model_for_search", std::string("invalid"))); + EXPECT_THROW(planner->configure(nodeSE2, "test", nullptr, costmap_ros), std::runtime_error); + nodeSE2->set_parameter(rclcpp::Parameter("test.motion_model_for_search", std::string("DUBIN"))); + + // invalid coarse search resolution + nodeSE2->set_parameter(rclcpp::Parameter("test.coarse_search_resolution", -1)); + nodeSE2->set_parameter(rclcpp::Parameter("test.max_on_approach_iterations", -1)); + nodeSE2->set_parameter(rclcpp::Parameter("test.max_iterations", -1)); + EXPECT_NO_THROW(planner->configure(nodeSE2, "test", nullptr, costmap_ros)); + + EXPECT_EQ(planner->getCoarseSearchResolution(), 1); + EXPECT_EQ(planner->getMaxIterations(), std::numeric_limits::max()); + EXPECT_EQ(planner->getMaxOnApproachIterations(), std::numeric_limits::max()); + + + // valid Configuration + nodeSE2->set_parameter(rclcpp::Parameter("test.coarse_search_resolution", 1)); + nodeSE2->set_parameter(rclcpp::Parameter("test.max_on_approach_iterations", 1000)); + nodeSE2->set_parameter(rclcpp::Parameter("test.max_iterations", 1000000)); + EXPECT_NO_THROW(planner->configure(nodeSE2, "test", nullptr, costmap_ros)); + + + // angle_quantizations not multiple of coarse search resolution would trigger a throw + nodeSE2->set_parameter(rclcpp::Parameter("test.angle_quantization_bins", 72)); + nodeSE2->set_parameter(rclcpp::Parameter("test.coarse_search_resolution", 5)); + EXPECT_THROW(planner->configure(nodeSE2, "test", nullptr, costmap_ros), std::runtime_error); + + // valid configuration + nodeSE2->set_parameter(rclcpp::Parameter("test.coarse_search_resolution", 4)); + EXPECT_NO_THROW(planner->configure(nodeSE2, "test", nullptr, costmap_ros)); + planner->configure(nodeSE2, "test", nullptr, costmap_ros); planner->activate(); @@ -93,7 +162,7 @@ TEST(SmacTest, test_smac_se2_reconfigure) std::make_shared("global_costmap"); costmap_ros->on_configure(rclcpp_lifecycle::State()); - auto planner = std::make_unique(); + auto planner = std::make_unique(); planner->configure(nodeSE2, "test", nullptr, costmap_ros); planner->activate(); @@ -107,7 +176,7 @@ TEST(SmacTest, test_smac_se2_reconfigure) auto results = rec_param->set_parameters_atomically( {rclcpp::Parameter("test.downsample_costmap", true), rclcpp::Parameter("test.downsampling_factor", 2), - rclcpp::Parameter("test.angle_quantization_bins", 100), + rclcpp::Parameter("test.angle_quantization_bins", 72), rclcpp::Parameter("test.allow_unknown", false), rclcpp::Parameter("test.max_iterations", -1), rclcpp::Parameter("test.minimum_turning_radius", 1.0), @@ -125,7 +194,9 @@ TEST(SmacTest, test_smac_se2_reconfigure) rclcpp::Parameter("test.analytic_expansion_max_length", 42.0), rclcpp::Parameter("test.max_on_approach_iterations", 42), rclcpp::Parameter("test.terminal_checking_interval", 42), - rclcpp::Parameter("test.motion_model_for_search", std::string("REEDS_SHEPP"))}); + rclcpp::Parameter("test.motion_model_for_search", std::string("REEDS_SHEPP")), + rclcpp::Parameter("test.goal_heading_mode", std::string("BIDIRECTIONAL")), + rclcpp::Parameter("test.coarse_search_resolution", -1)}); rclcpp::spin_until_future_complete( nodeSE2->get_node_base_interface(), @@ -133,7 +204,7 @@ TEST(SmacTest, test_smac_se2_reconfigure) EXPECT_EQ(nodeSE2->get_parameter("test.downsample_costmap").as_bool(), true); EXPECT_EQ(nodeSE2->get_parameter("test.downsampling_factor").as_int(), 2); - EXPECT_EQ(nodeSE2->get_parameter("test.angle_quantization_bins").as_int(), 100); + EXPECT_EQ(nodeSE2->get_parameter("test.angle_quantization_bins").as_int(), 72); EXPECT_EQ(nodeSE2->get_parameter("test.allow_unknown").as_bool(), false); EXPECT_EQ(nodeSE2->get_parameter("test.max_iterations").as_int(), -1); EXPECT_EQ(nodeSE2->get_parameter("test.minimum_turning_radius").as_double(), 1.0); @@ -154,6 +225,9 @@ TEST(SmacTest, test_smac_se2_reconfigure) EXPECT_EQ( nodeSE2->get_parameter("test.motion_model_for_search").as_string(), std::string("REEDS_SHEPP")); + EXPECT_EQ( + nodeSE2->get_parameter("test.goal_heading_mode").as_string(), + std::string("BIDIRECTIONAL")); auto results2 = rec_param->set_parameters_atomically( {rclcpp::Parameter("resolution", 0.2)}); @@ -161,6 +235,30 @@ TEST(SmacTest, test_smac_se2_reconfigure) nodeSE2->get_node_base_interface(), results2); EXPECT_EQ(nodeSE2->get_parameter("resolution").as_double(), 0.2); + EXPECT_EQ(nodeSE2->get_parameter("test.coarse_search_resolution").as_int(), -1); + EXPECT_EQ(planner->getCoarseSearchResolution(), 1); + + // test coarse resolution edge cases. Consider when coarse resolution + // is not multiple of angle bin quantization(72) + std::vector parameters; + parameters.push_back(rclcpp::Parameter("test.coarse_search_resolution", 7)); + EXPECT_NO_THROW(planner->callDynamicParams(parameters)); + EXPECT_EQ(planner->getCoarseSearchResolution(), 1); + + // same test as before but the error comes from the angular bin + parameters.clear(); + parameters.push_back(rclcpp::Parameter("test.coarse_search_resolution", 4)); + parameters.push_back(rclcpp::Parameter("test.angle_quantization_bins", 87)); + EXPECT_NO_THROW(planner->callDynamicParams(parameters)); + EXPECT_EQ(planner->getCoarseSearchResolution(), 1); + + // test invalid goal heading mode does not modify current + // goal heading mode + parameters.clear(); + parameters.push_back(rclcpp::Parameter("test.goal_heading_mode", std::string("BIDIRECTIONAL"))); + parameters.push_back(rclcpp::Parameter("test.goal_heading_mode", std::string("invalid"))); + EXPECT_NO_THROW(planner->callDynamicParams(parameters)); + EXPECT_EQ(planner->getGoalHeadingMode(), nav2_smac_planner::GoalHeadingMode::BIDIRECTIONAL); } int main(int argc, char ** argv) diff --git a/nav2_smac_planner/test/test_smac_lattice.cpp b/nav2_smac_planner/test/test_smac_lattice.cpp index a1450c374bf..39758294b83 100644 --- a/nav2_smac_planner/test/test_smac_lattice.cpp +++ b/nav2_smac_planner/test/test_smac_lattice.cpp @@ -36,6 +36,26 @@ class LatticeWrap : public nav2_smac_planner::SmacPlannerLattice { dynamicParametersCallback(parameters); } + + int getCoarseSearchResolution() + { + return _coarse_search_resolution; + } + + int getMaxIterations() + { + return _max_iterations; + } + + int getMaxOnApproachIterations() + { + return _max_on_approach_iterations; + } + + nav2_smac_planner::GoalHeadingMode getGoalHeadingMode() + { + return _goal_heading_mode; + } }; // SMAC smoke tests for plugin-level issues rather than algorithms @@ -63,10 +83,38 @@ TEST(SmacTest, test_smac_lattice) goal.pose.position.x = 1.0; goal.pose.position.y = 1.0; goal.pose.orientation.w = 1.0; - auto planner = std::make_unique(); + auto planner = std::make_unique(); try { + // invalid goal heading mode + nodeLattice->declare_parameter("test.goal_heading_mode", std::string("UNKNOWN")); + nodeLattice->set_parameter(rclcpp::Parameter("test.goal_heading_mode", std::string("UNKNOWN"))); + EXPECT_THROW(planner->configure(nodeLattice, "test", nullptr, costmap_ros), std::runtime_error); + nodeLattice->set_parameter(rclcpp::Parameter("test.goal_heading_mode", std::string("DEFAULT"))); + + // invalid Configuration resolution + nodeLattice->set_parameter(rclcpp::Parameter("test.coarse_search_resolution", -1)); + nodeLattice->set_parameter(rclcpp::Parameter("test.max_iterations", -1)); + nodeLattice->set_parameter(rclcpp::Parameter("test.max_on_approach_iterations", -1)); + + EXPECT_NO_THROW(planner->configure(nodeLattice, "test", nullptr, costmap_ros)); + EXPECT_EQ(planner->getCoarseSearchResolution(), 1); + EXPECT_EQ(planner->getMaxIterations(), std::numeric_limits::max()); + EXPECT_EQ(planner->getMaxOnApproachIterations(), std::numeric_limits::max()); + + + // Valid configuration + nodeLattice->set_parameter(rclcpp::Parameter("test.max_iterations", 1000000)); + nodeLattice->set_parameter(rclcpp::Parameter("test.max_on_approach_iterations", 1000)); + + // Coarse search resolution will throw, not multiple of number of heading(16 default) + nodeLattice->set_parameter(rclcpp::Parameter("test.coarse_search_resolution", 3)); + EXPECT_THROW(planner->configure(nodeLattice, "test", nullptr, costmap_ros), std::runtime_error); + + // Valid configuration + nodeLattice->set_parameter(rclcpp::Parameter("test.coarse_search_resolution", 4)); // Expect to throw due to invalid prims file in param planner->configure(nodeLattice, "test", nullptr, costmap_ros); + EXPECT_EQ(planner->getCoarseSearchResolution(), 4); } catch (...) { } planner->activate(); @@ -143,9 +191,41 @@ TEST(SmacTest, test_smac_lattice_reconfigure) results); } catch (...) { } + // test edge cases Goal heading mode, make sure we don't reset the goal when invalid + std::vector parameters; + parameters.push_back(rclcpp::Parameter("test.goal_heading_mode", std::string("BIDIRECTIONAL"))); + parameters.push_back(rclcpp::Parameter("test.goal_heading_mode", std::string("invalid"))); + EXPECT_NO_THROW(planner->callDynamicParams(parameters)); + EXPECT_EQ(planner->getGoalHeadingMode(), nav2_smac_planner::GoalHeadingMode::BIDIRECTIONAL); + + // test coarse resolution edge cases. + // Negative coarse search resolution + parameters.clear(); + parameters.push_back(rclcpp::Parameter("test.coarse_search_resolution", -1)); + EXPECT_NO_THROW(planner->callDynamicParams(parameters)); + EXPECT_EQ(planner->getCoarseSearchResolution(), 1); + + // test value when coarse resolution + // is not multiple number_of_headings + parameters.clear(); + parameters.push_back(rclcpp::Parameter("test.coarse_search_resolution", 5)); + EXPECT_NO_THROW(planner->callDynamicParams(parameters)); + EXPECT_EQ(planner->getCoarseSearchResolution(), 1); + + // Similar modulous test but when the issue is from the number + // of heading, test output includes number of heading 15 + parameters.clear(); + + parameters.push_back(rclcpp::Parameter("test.coarse_search_resolution", 4)); + parameters.push_back(rclcpp::Parameter("test.lattice_filepath", + ament_index_cpp::get_package_share_directory("nav2_smac_planner") + + "/sample_primitives/test/output.json")); + EXPECT_NO_THROW(planner->callDynamicParams(parameters)); + EXPECT_EQ(planner->getCoarseSearchResolution(), 1); + // So instead, let's call manually on a change - std::vector parameters; + parameters.clear(); parameters.push_back(rclcpp::Parameter("test.lattice_filepath", std::string("HI"))); EXPECT_THROW(planner->callDynamicParams(parameters), std::runtime_error); } From 7369634dac4c8f8bd209a66b188c42817949baf4 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 16 Jun 2025 18:12:30 -0700 Subject: [PATCH 41/46] Increase cache Signed-off-by: Steve Macenski --- .circleci/config.yml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.circleci/config.yml b/.circleci/config.yml index c752c1fcd15..1113d469f90 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -33,12 +33,12 @@ _commands: - restore_cache: name: Restore Cache << parameters.key >> keys: - - "<< parameters.key >>-v33\ + - "<< parameters.key >>-v999\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ -{{ checksum \"<< parameters.workspace >>/lockfile.txt\" }}" - - "<< parameters.key >>-v33\ + - "<< parameters.key >>-v999\ -{{ arch }}\ -main\ -\ @@ -58,7 +58,7 @@ _commands: steps: - save_cache: name: Save Cache << parameters.key >> - key: "<< parameters.key >>-v33\ + key: "<< parameters.key >>-v999\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ From ce4cd90caf4fd2d86822d2115e10a37692ab56b2 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 16 Jun 2025 18:41:18 -0700 Subject: [PATCH 42/46] fixes for humble main compatibility Signed-off-by: Steve Macenski --- .circleci/config.yml | 6 +- nav2_util/include/nav2_util/node_utils.hpp | 139 +++++++++++++++++++-- nav2_util/test/test_node_utils.cpp | 1 + tools/underlay.repos | 2 +- 4 files changed, 132 insertions(+), 16 deletions(-) diff --git a/.circleci/config.yml b/.circleci/config.yml index 1113d469f90..c752c1fcd15 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -33,12 +33,12 @@ _commands: - restore_cache: name: Restore Cache << parameters.key >> keys: - - "<< parameters.key >>-v999\ + - "<< parameters.key >>-v33\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ -{{ checksum \"<< parameters.workspace >>/lockfile.txt\" }}" - - "<< parameters.key >>-v999\ + - "<< parameters.key >>-v33\ -{{ arch }}\ -main\ -\ @@ -58,7 +58,7 @@ _commands: steps: - save_cache: name: Save Cache << parameters.key >> - key: "<< parameters.key >>-v999\ + key: "<< parameters.key >>-v33\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ diff --git a/nav2_util/include/nav2_util/node_utils.hpp b/nav2_util/include/nav2_util/node_utils.hpp index 74f0df2dc60..13bdf907739 100644 --- a/nav2_util/include/nav2_util/node_utils.hpp +++ b/nav2_util/include/nav2_util/node_utils.hpp @@ -81,25 +81,26 @@ rclcpp::Node::SharedPtr generate_internal_node(const std::string & prefix = ""); */ std::string time_to_string(size_t len); +using ParameterDescriptor = rcl_interfaces::msg::ParameterDescriptor; + /// Declares static ROS2 parameter and sets it to a given value if it was not already declared /* Declares static ROS2 parameter and sets it to a given value * if it was not already declared. * * \param[in] node A node in which given parameter to be declared - * \param[in] param_name The name of parameter + * \param[in] parameter_name The name of parameter * \param[in] default_value Parameter value to initialize with * \param[in] parameter_descriptor Parameter descriptor (optional) */ template void declare_parameter_if_not_declared( NodeT node, - const std::string & param_name, + const std::string & parameter_name, const rclcpp::ParameterValue & default_value, - const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor = - rcl_interfaces::msg::ParameterDescriptor()) + const ParameterDescriptor & parameter_descriptor = ParameterDescriptor()) { - if (!node->has_parameter(param_name)) { - node->declare_parameter(param_name, default_value, parameter_descriptor); + if (!node->has_parameter(parameter_name)) { + node->declare_parameter(parameter_name, default_value, parameter_descriptor); } } @@ -107,21 +108,135 @@ void declare_parameter_if_not_declared( /* Declares static ROS2 parameter with given type if it was not already declared. * * \param[in] node A node in which given parameter to be declared + * \param[in] parameter_name Name of the parameter * \param[in] param_type The type of parameter - * \param[in] default_value Parameter value to initialize with * \param[in] parameter_descriptor Parameter descriptor (optional) */ template void declare_parameter_if_not_declared( NodeT node, - const std::string & param_name, + const std::string & parameter_name, const rclcpp::ParameterType & param_type, - const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor = - rcl_interfaces::msg::ParameterDescriptor()) + const ParameterDescriptor & parameter_descriptor = ParameterDescriptor()) +{ + if (!node->has_parameter(parameter_name)) { + node->declare_parameter(parameter_name, param_type, parameter_descriptor); + } +} + +/// Declares a parameter with the specified type if it was not already declared +/// and returns its value if available. +/* Declares a parameter with the specified type if it was not already declared. + * If the parameter was overridden, its value is returned, otherwise an + * rclcpp::exceptions::InvalidParameterValueException is thrown + * + * \param[in] node A node in which given parameter to be declared + * \param[in] parameter_name Name of the parameter + * \param[in] param_type The type of parameter + * \param[in] parameter_descriptor Parameter descriptor (optional) + * \return The value of the parameter or an exception + */ +template +ParameterT declare_or_get_parameter( + NodeT node, + const std::string & parameter_name, + const rclcpp::ParameterType & param_type, + const ParameterDescriptor & parameter_descriptor = ParameterDescriptor()) +{ + if (node->has_parameter(parameter_name)) { + return node->get_parameter(parameter_name).template get_value(); + } + auto parameter = node->declare_parameter(parameter_name, param_type, parameter_descriptor); + if (parameter.get_type() == rclcpp::ParameterType::PARAMETER_NOT_SET) { + std::string description = "Parameter " + parameter_name + " not in overrides"; + throw rclcpp::exceptions::InvalidParameterValueException(description.c_str()); + } + return parameter.template get(); +} + +using NodeParamInterfacePtr = rclcpp::node_interfaces::NodeParametersInterface::SharedPtr; + +/// If the parameter is already declared, returns its value, +/// otherwise declares it and returns the default value +/** + * If the parameter is already declared, returns its value, + * otherwise declares it and returns the default value. + * The method can optionally print a warning or throw when the override is missing. + * + * \param[in] logger Node logging interface + * \param[in] param_interface Node parameter interface + * \param[in] parameter_name Name of the parameter + * \param[in] default_value Default value of the parameter + * \param[in] warn_if_no_override If true, prints a warning whenever the parameter override is missing + * \param[in] strict_param_loading If true, throws an InvalidParameterValueException if the parameter override is missing + * \param[in] parameter_descriptor Optional parameter descriptor + * \return The value of the param from the override if existent, otherwise the default value + */ +template +ParamType declare_or_get_parameter( + const rclcpp::Logger & logger, NodeParamInterfacePtr param_interface, + const std::string & parameter_name, const ParamType & default_value, + bool warn_if_no_override = false, bool strict_param_loading = false, + const ParameterDescriptor & parameter_descriptor = ParameterDescriptor()) { - if (!node->has_parameter(param_name)) { - node->declare_parameter(param_name, param_type, parameter_descriptor); + if (param_interface->has_parameter(parameter_name)) { + rclcpp::Parameter param(parameter_name, default_value); + param_interface->get_parameter(parameter_name, param); + return param.get_value(); } + + auto return_value = param_interface + ->declare_parameter(parameter_name, rclcpp::ParameterValue{default_value}, + parameter_descriptor) + .get(); + + const bool no_param_override = param_interface->get_parameter_overrides().find(parameter_name) == + param_interface->get_parameter_overrides().end(); + if (no_param_override) { + if (warn_if_no_override) { + RCLCPP_WARN_STREAM( + logger, + "Failed to get param " << parameter_name << " from overrides, using default value."); + } + if (strict_param_loading) { + std::string description = "Parameter " + parameter_name + + " not in overrides and strict_param_loading is True"; + throw rclcpp::exceptions::InvalidParameterValueException(description.c_str()); + } + } + + return return_value; +} + +/// If the parameter is already declared, returns its value, +/// otherwise declares it and returns the default value +/** + * If the parameter is already declared, returns its value, + * otherwise declares it and returns the default value. + * The method can be configured to print a warn message or throw an InvalidParameterValueException + * when the override is missing by enabling the parameters warn_on_missing_params + * or strict_param_loading respectively. + * + * \param[in] node Pointer to a node object + * \param[in] parameter_name Name of the parameter + * \param[in] default_value Default value of the parameter + * \param[in] parameter_descriptor Optional parameter descriptor + * \return The value of the param from the override if existent, otherwise the default value + */ +template +ParamType declare_or_get_parameter( + NodeT node, const std::string & parameter_name, + const ParamType & default_value, + const ParameterDescriptor & parameter_descriptor = ParameterDescriptor()) +{ + declare_parameter_if_not_declared(node, "warn_on_missing_params", rclcpp::ParameterValue(false)); + bool warn_if_no_override{false}; + node->get_parameter("warn_on_missing_params", warn_if_no_override); + declare_parameter_if_not_declared(node, "strict_param_loading", rclcpp::ParameterValue(false)); + bool strict_param_loading{false}; + node->get_parameter("strict_param_loading", strict_param_loading); + return declare_or_get_parameter(node->get_logger(), node->get_node_parameters_interface(), + parameter_name, default_value, warn_if_no_override, strict_param_loading, parameter_descriptor); } /// Gets the type of plugin for the selected node and its plugin diff --git a/nav2_util/test/test_node_utils.cpp b/nav2_util/test/test_node_utils.cpp index 17b650c10a6..c43ba85c7ee 100644 --- a/nav2_util/test/test_node_utils.cpp +++ b/nav2_util/test/test_node_utils.cpp @@ -25,6 +25,7 @@ using nav2_util::generate_internal_node; using nav2_util::add_namespaces; using nav2_util::time_to_string; using nav2_util::declare_parameter_if_not_declared; +using nav2_util::declare_or_get_parameter; using nav2_util::get_plugin_type_param; TEST(SanitizeNodeName, SanitizeNodeName) diff --git a/tools/underlay.repos b/tools/underlay.repos index 24a86813ac5..094930ac96f 100644 --- a/tools/underlay.repos +++ b/tools/underlay.repos @@ -2,7 +2,7 @@ repositories: BehaviorTree/BehaviorTree.CPP: type: git url: https://github.com/BehaviorTree/BehaviorTree.CPP.git - version: 4.6.2 + version: 4.7.0 # ros/angles: # type: git # url: https://github.com/ros/angles.git From ac2c4c488aec9a7d7fec309450258799430ed1d8 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 16 Jun 2025 18:41:26 -0700 Subject: [PATCH 43/46] Revert "Removing action server timeout duration after fixes to ROS 2, Reverts 3787 (#5183)" This reverts commit c9438b4cdad2653a32b5372f9f672b6684d40258. --- .../nav2_behavior_tree/bt_action_server_impl.hpp | 11 ++++++++++- .../include/nav2_behaviors/timed_behavior.hpp | 11 ++++++++++- nav2_bringup/params/nav2_params.yaml | 2 ++ nav2_controller/src/controller_server.cpp | 10 ++++++++-- nav2_docking/opennav_docking/src/docking_server.cpp | 11 +++++++++-- nav2_planner/src/planner_server.cpp | 10 ++++++++-- nav2_smoother/src/nav2_smoother.cpp | 11 +++++++++-- nav2_system_tests/src/system/nav2_system_params.yaml | 2 ++ nav2_waypoint_follower/src/waypoint_follower.cpp | 10 ++++++++-- 9 files changed, 66 insertions(+), 12 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index be5a5ea7912..4a6f43b925f 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -66,6 +66,9 @@ BtActionServer::BtActionServer( if (!node->has_parameter("default_server_timeout")) { node->declare_parameter("default_server_timeout", 20); } + if (!node->has_parameter("action_server_result_timeout")) { + node->declare_parameter("action_server_result_timeout", 900.0); + } if (!node->has_parameter("always_reload_bt_xml")) { node->declare_parameter("always_reload_bt_xml", false); } @@ -164,13 +167,19 @@ bool BtActionServer::on_configure() node, "transform_tolerance", rclcpp::ParameterValue(0.1)); nav2_util::copy_all_parameter_values(node, client_node_); + // set the timeout in seconds for the action server to discard goal handles if not finished + double action_server_result_timeout = + node->get_parameter("action_server_result_timeout").as_double(); + rcl_action_server_options_t server_options = rcl_action_server_get_default_options(); + server_options.result_timeout.nanoseconds = RCL_S_TO_NS(action_server_result_timeout); + action_server_ = std::make_shared( node->get_node_base_interface(), node->get_node_clock_interface(), node->get_node_logging_interface(), node->get_node_waitables_interface(), action_name_, std::bind(&BtActionServer::executeCallback, this), - nullptr, std::chrono::milliseconds(500), false); + nullptr, std::chrono::milliseconds(500), false, server_options); // Get parameters for BT timeouts int bt_loop_duration; diff --git a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp index dee4107ca45..987575354bd 100644 --- a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp +++ b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp @@ -135,10 +135,19 @@ class TimedBehavior : public nav2_core::Behavior node->get_parameter("robot_base_frame", robot_base_frame_); node->get_parameter("transform_tolerance", transform_tolerance_); + if (!node->has_parameter("action_server_result_timeout")) { + node->declare_parameter("action_server_result_timeout", 10.0); + } + + double action_server_result_timeout; + node->get_parameter("action_server_result_timeout", action_server_result_timeout); + rcl_action_server_options_t server_options = rcl_action_server_get_default_options(); + server_options.result_timeout.nanoseconds = RCL_S_TO_NS(action_server_result_timeout); + action_server_ = std::make_shared( node, behavior_name_, std::bind(&TimedBehavior::execute, this), nullptr, std::chrono::milliseconds( - 500), false); + 500), false, server_options); local_collision_checker_ = local_collision_checker; global_collision_checker_ = global_collision_checker; diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index cfbcdc858bf..768baadedb5 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -48,6 +48,7 @@ bt_navigator: filter_duration: 0.3 default_server_timeout: 20 wait_for_service_timeout: 1000 + action_server_result_timeout: 900.0 service_introspection_mode: "disabled" navigators: ["navigate_to_pose", "navigate_through_poses"] navigate_to_pose: @@ -433,6 +434,7 @@ waypoint_follower: loop_rate: 20 stop_on_failure: false service_introspection_mode: "disabled" + action_server_result_timeout: 900.0 waypoint_task_executor_plugin: "wait_at_waypoint" wait_at_waypoint: plugin: "nav2_waypoint_follower::WaitAtWaypoint" diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index f370f5437e8..9df9bc41c22 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -51,6 +51,8 @@ ControllerServer::ControllerServer(const rclcpp::NodeOptions & options) declare_parameter("controller_frequency", 20.0); + declare_parameter("action_server_result_timeout", 10.0); + declare_parameter("progress_checker_plugins", default_progress_checker_ids_); declare_parameter("goal_checker_plugins", default_goal_checker_ids_); declare_parameter("controller_plugins", default_ids_); @@ -222,6 +224,11 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) odom_sub_ = std::make_unique(node); vel_publisher_ = std::make_unique(node, "cmd_vel", 1); + double action_server_result_timeout; + get_parameter("action_server_result_timeout", action_server_result_timeout); + rcl_action_server_options_t server_options = rcl_action_server_get_default_options(); + server_options.result_timeout.nanoseconds = RCL_S_TO_NS(action_server_result_timeout); + double costmap_update_timeout_dbl; get_parameter("costmap_update_timeout", costmap_update_timeout_dbl); costmap_update_timeout_ = rclcpp::Duration::from_seconds(costmap_update_timeout_dbl); @@ -235,8 +242,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) std::bind(&ControllerServer::computeControl, this), nullptr, std::chrono::milliseconds(500), - true /*spin thread*/, rcl_action_server_get_default_options(), - use_realtime_priority_ /*soft realtime*/); + true /*spin thread*/, server_options, use_realtime_priority_ /*soft realtime*/); } catch (const std::runtime_error & e) { RCLCPP_ERROR(get_logger(), "Error creating action server! %s", e.what()); on_cleanup(state); diff --git a/nav2_docking/opennav_docking/src/docking_server.cpp b/nav2_docking/opennav_docking/src/docking_server.cpp index c14686ee1a5..888b57d08b6 100644 --- a/nav2_docking/opennav_docking/src/docking_server.cpp +++ b/nav2_docking/opennav_docking/src/docking_server.cpp @@ -85,18 +85,25 @@ DockingServer::on_configure(const rclcpp_lifecycle::State & state) get_parameter("odom_topic", odom_topic); odom_sub_ = std::make_unique(node, odom_topic); + double action_server_result_timeout; + nav2_util::declare_parameter_if_not_declared( + node, "action_server_result_timeout", rclcpp::ParameterValue(10.0)); + get_parameter("action_server_result_timeout", action_server_result_timeout); + rcl_action_server_options_t server_options = rcl_action_server_get_default_options(); + server_options.result_timeout.nanoseconds = RCL_S_TO_NS(action_server_result_timeout); + // Create the action servers for dock / undock docking_action_server_ = std::make_unique( node, "dock_robot", std::bind(&DockingServer::dockRobot, this), nullptr, std::chrono::milliseconds(500), - true); + true, server_options); undocking_action_server_ = std::make_unique( node, "undock_robot", std::bind(&DockingServer::undockRobot, this), nullptr, std::chrono::milliseconds(500), - true); + true, server_options); // Create composed utilities mutex_ = std::make_shared(); diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index ac1d1efd89d..e36602a02fd 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -52,6 +52,7 @@ PlannerServer::PlannerServer(const rclcpp::NodeOptions & options) // Declare this node's parameters declare_parameter("planner_plugins", default_ids_); declare_parameter("expected_planner_frequency", 1.0); + declare_parameter("action_server_result_timeout", 10.0); declare_parameter("costmap_update_timeout", 1.0); get_parameter("planner_plugins", planner_ids_); @@ -147,6 +148,11 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & state) // Initialize pubs & subs plan_publisher_ = create_publisher("plan", 1); + double action_server_result_timeout; + get_parameter("action_server_result_timeout", action_server_result_timeout); + rcl_action_server_options_t server_options = rcl_action_server_get_default_options(); + server_options.result_timeout.nanoseconds = RCL_S_TO_NS(action_server_result_timeout); + double costmap_update_timeout_dbl; get_parameter("costmap_update_timeout", costmap_update_timeout_dbl); costmap_update_timeout_ = rclcpp::Duration::from_seconds(costmap_update_timeout_dbl); @@ -158,7 +164,7 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & state) std::bind(&PlannerServer::computePlan, this), nullptr, std::chrono::milliseconds(500), - true); + true, server_options); action_server_poses_ = std::make_unique( shared_from_this(), @@ -166,7 +172,7 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & state) std::bind(&PlannerServer::computePlanThroughPoses, this), nullptr, std::chrono::milliseconds(500), - true); + true, server_options); return nav2_util::CallbackReturn::SUCCESS; } diff --git a/nav2_smoother/src/nav2_smoother.cpp b/nav2_smoother/src/nav2_smoother.cpp index b09b3171eb4..af3697d8856 100644 --- a/nav2_smoother/src/nav2_smoother.cpp +++ b/nav2_smoother/src/nav2_smoother.cpp @@ -53,6 +53,8 @@ SmootherServer::SmootherServer(const rclcpp::NodeOptions & options) rclcpp::ParameterValue(std::string("base_link"))); declare_parameter("transform_tolerance", rclcpp::ParameterValue(0.1)); declare_parameter("smoother_plugins", default_ids_); + + declare_parameter("action_server_result_timeout", 10.0); } SmootherServer::~SmootherServer() @@ -83,7 +85,7 @@ SmootherServer::on_configure(const rclcpp_lifecycle::State & state) transform_listener_ = std::make_shared(*tf_); std::string costmap_topic, footprint_topic, robot_base_frame; - double transform_tolerance = 0.1; + double transform_tolerance; this->get_parameter("costmap_topic", costmap_topic); this->get_parameter("footprint_topic", footprint_topic); this->get_parameter("transform_tolerance", transform_tolerance); @@ -105,6 +107,11 @@ SmootherServer::on_configure(const rclcpp_lifecycle::State & state) // Initialize pubs & subs plan_publisher_ = create_publisher("plan_smoothed", 1); + double action_server_result_timeout; + get_parameter("action_server_result_timeout", action_server_result_timeout); + rcl_action_server_options_t server_options = rcl_action_server_get_default_options(); + server_options.result_timeout.nanoseconds = RCL_S_TO_NS(action_server_result_timeout); + // Create the action server that we implement with our smoothPath method action_server_ = std::make_unique( shared_from_this(), @@ -112,7 +119,7 @@ SmootherServer::on_configure(const rclcpp_lifecycle::State & state) std::bind(&SmootherServer::smoothPlan, this), nullptr, std::chrono::milliseconds(500), - true); + true, server_options); return nav2_util::CallbackReturn::SUCCESS; } diff --git a/nav2_system_tests/src/system/nav2_system_params.yaml b/nav2_system_tests/src/system/nav2_system_params.yaml index 3ee779d90a0..311081aaa26 100644 --- a/nav2_system_tests/src/system/nav2_system_params.yaml +++ b/nav2_system_tests/src/system/nav2_system_params.yaml @@ -46,6 +46,7 @@ bt_navigator: bt_loop_duration: 10 filter_duration: 0.3 default_server_timeout: 20 + action_server_result_timeout: 900.0 navigators: ["navigate_to_pose", "navigate_through_poses"] navigate_to_pose: plugin: "nav2_bt_navigator::NavigateToPoseNavigator" @@ -290,6 +291,7 @@ waypoint_follower: ros__parameters: loop_rate: 20 stop_on_failure: false + action_server_result_timeout: 900.0 waypoint_task_executor_plugin: "wait_at_waypoint" wait_at_waypoint: plugin: "nav2_waypoint_follower::WaitAtWaypoint" diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index cd3993dc4e0..a33c7aab9e3 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -37,6 +37,8 @@ WaypointFollower::WaypointFollower(const rclcpp::NodeOptions & options) declare_parameter("stop_on_failure", true); declare_parameter("loop_rate", 20); + declare_parameter("action_server_result_timeout", 900.0); + declare_parameter("global_frame_id", "map"); nav2_util::declare_parameter_if_not_declared( @@ -76,6 +78,10 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & state) get_node_waitables_interface(), "navigate_to_pose", callback_group_); + double action_server_result_timeout = get_parameter("action_server_result_timeout").as_double(); + rcl_action_server_options_t server_options = rcl_action_server_get_default_options(); + server_options.result_timeout.nanoseconds = RCL_S_TO_NS(action_server_result_timeout); + xyz_action_server_ = std::make_unique( get_node_base_interface(), get_node_clock_interface(), @@ -84,7 +90,7 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & state) "follow_waypoints", std::bind( &WaypointFollower::followWaypointsCallback, this), nullptr, std::chrono::milliseconds( - 500), false); + 500), false, server_options); from_ll_to_map_client_ = std::make_unique< nav2_util::ServiceClient Date: Mon, 16 Jun 2025 18:53:15 -0700 Subject: [PATCH 44/46] fixing CI build Signed-off-by: Steve Macenski --- .github/workflows/update_ci_image.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/update_ci_image.yaml b/.github/workflows/update_ci_image.yaml index 5692ce8cf26..dac633728fd 100644 --- a/.github/workflows/update_ci_image.yaml +++ b/.github/workflows/update_ci_image.yaml @@ -87,7 +87,7 @@ jobs: no_cache=false if [ "${{needs.check_ci_files.outputs.no_cache}}" == 'true' ] || \ - [ "${{needs.check_ci_image.outputs.no_cache}}" == 'true' ] + [ "${{needs.check_ci_image.outputs.no_cache || 'false'}}" == 'true' ] then no_cache=true fi @@ -95,7 +95,7 @@ jobs: trigger=false if [ "${{needs.check_ci_files.outputs.trigger}}" == 'true' ] || \ - [ "${{needs.check_ci_image.outputs.trigger}}" == 'true' ] + [ "${{needs.check_ci_image.outputs.trigger || 'false'}}" == 'true' ] then trigger=true fi From a9d9ecc14626721fa242b8dea6d93365ae70528c Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 16 Jun 2025 18:57:43 -0700 Subject: [PATCH 45/46] revert Signed-off-by: Steve Macenski --- .github/workflows/update_ci_image.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/update_ci_image.yaml b/.github/workflows/update_ci_image.yaml index dac633728fd..5692ce8cf26 100644 --- a/.github/workflows/update_ci_image.yaml +++ b/.github/workflows/update_ci_image.yaml @@ -87,7 +87,7 @@ jobs: no_cache=false if [ "${{needs.check_ci_files.outputs.no_cache}}" == 'true' ] || \ - [ "${{needs.check_ci_image.outputs.no_cache || 'false'}}" == 'true' ] + [ "${{needs.check_ci_image.outputs.no_cache}}" == 'true' ] then no_cache=true fi @@ -95,7 +95,7 @@ jobs: trigger=false if [ "${{needs.check_ci_files.outputs.trigger}}" == 'true' ] || \ - [ "${{needs.check_ci_image.outputs.trigger || 'false'}}" == 'true' ] + [ "${{needs.check_ci_image.outputs.trigger}}" == 'true' ] then trigger=true fi From 34dd2bd88430a9a5c3f7bc64631836465876576f Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 16 Jun 2025 19:27:03 -0700 Subject: [PATCH 46/46] done Signed-off-by: Steve Macenski --- tools/underlay.repos | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/tools/underlay.repos b/tools/underlay.repos index 094930ac96f..fc372b54c67 100644 --- a/tools/underlay.repos +++ b/tools/underlay.repos @@ -39,7 +39,7 @@ repositories: type: git url: https://github.com/ros2/launch.git version: humble - # ros-navigation/nav2_minimal_turtlebot_simulation: - # type: git - # url: https://github.com/ros-navigation/nav2_minimal_turtlebot_simulation.git - # version: main + ros-navigation/nav2_minimal_turtlebot_simulation: + type: git + url: https://github.com/ros-navigation/nav2_minimal_turtlebot_simulation.git + version: main