chore: sync NAV2 module sources

.gitignore

@@ -3,3 +3,6 @@ install/
 log/
 .colcon/
 *.log
+__pycache__/
+**/__pycache__/
+*.py[cod]

src/planning_pkg/behavior/navigate_to_pose_no_spin.xml

@@ -4,31 +4,11 @@
 -->
 <root main_tree_to_execute="MainTree">
   <BehaviorTree ID="MainTree">
-    <RecoveryNode number_of_retries="8" name="NavigateRecovery">
+    <PipelineSequence name="NavigateToPose">
-      <PipelineSequence name="NavigateWithReplanning">
       <RateController hz="1.0">
-          <RecoveryNode number_of_retries="1" name="ComputePathToPose">
         <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
-            <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
-          </RecoveryNode>
       </RateController>
-        <RecoveryNode number_of_retries="1" name="FollowPath">
       <FollowPath path="{path}" controller_id="FollowPath"/>
-          <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
-        </RecoveryNode>
     </PipelineSequence>
-      <ReactiveFallback name="RecoveryFallback">
-        <GoalUpdated/>
-        <RoundRobin name="RecoveryActions">
-          <!-- <BackUp backup_dist="1.0" backup_speed="0.20"/> -->
-          <BackUp backup_dist="0.3" backup_speed="0.20"/>
-          <!-- <Sequence name="ClearingActions">
-            <ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
-            <ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
-          </Sequence> -->
-          <Wait wait_duration="1"/>
-        </RoundRobin>
-      </ReactiveFallback>
-    </RecoveryNode>
   </BehaviorTree>
 </root>

src/planning_pkg/config/planning_2d_sim_turtlebot3.yaml

@@ -6,6 +6,11 @@ bt_navigator:
     global_frame: map
     robot_base_frame: base_footprint
     odom_topic: /odom
+    default_nav_to_pose_bt_xml: ''
+    default_nav_through_poses_bt_xml: ''
+    navigators: [navigate_to_pose]
+    navigate_to_pose:
+      plugin: nav2_bt_navigator/NavigateToPoseNavigator
     bt_loop_duration: 20
     default_server_timeout: 2000
     wait_for_service_timeout: 4000

src/planning_pkg/launch/nav2_planning.launch.py

@@ -2,7 +2,7 @@
 from ament_index_python.packages import get_package_share_directory
 
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument
+from launch.actions import DeclareLaunchArgument, TimerAction
 from launch.actions import GroupAction
 from launch.actions import OpaqueFunction
 from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
@@ -25,9 +25,7 @@ def launch_setup(context, *args, **kwargs):
     level    = LaunchConfiguration('level').perform(context)
     log_level = LaunchConfiguration('log_level').perform(context)
     
-    lifecycle_nodes = ['controller_server',
+    lifecycle_nodes = ['planner_server',
-                       'smoother_server',
-                       'planner_server',
                        'bt_navigator',
                        'waypoint_follower']
     
@@ -39,11 +37,9 @@ def launch_setup(context, *args, **kwargs):
         'behavior',
         'navigate_to_pose_no_spin.xml'
     ])
-    default_nav_through_poses_bt_xml = PathJoinSubstitution([
+    # Humble starts the NavigateThroughPoses navigator as well. Reuse the
-        pkg_planning,
+    # minimal pose tree so its optional recovery actions are not required.
-        'behavior',
+    default_nav_through_poses_bt_xml = default_nav_to_pose_bt_xml
-        'navigate_through_poses_no_spin.xml'
-    ])
     
     param_file_name = None
     
@@ -57,7 +53,7 @@ def launch_setup(context, *args, **kwargs):
         pkg_planning,
         'config', param_file_name])
     param_substitutions = {
-        'use_sim_time': 'true',
+        'use_sim_time': sim_mode,
         'default_nav_to_pose_bt_xml': default_nav_to_pose_bt_xml,
         'default_nav_through_poses_bt_xml': default_nav_through_poses_bt_xml
     }
@@ -70,16 +66,6 @@ def launch_setup(context, *args, **kwargs):
     
     load_nodes = GroupAction(
         actions=[
-            Node(
-                package='nav2_controller',
-                executable='controller_server',
-                name='controller_server',
-                output='screen',
-                respawn=False,
-                respawn_delay=2.0,
-                parameters=[configured_params],
-                arguments=['--ros-args', '--log-level', log_level],
-            ),
             Node(
                 package='nav2_planner',
                 executable='planner_server',
@@ -110,25 +96,21 @@ def launch_setup(context, *args, **kwargs):
                 parameters=[configured_params],
                 arguments=['--ros-args', '--log-level', log_level],
             ),
-            Node(
+            # Let the Nav2 servers finish creating their lifecycle services first.
-                package='nav2_smoother',
+            TimerAction(
-                executable='smoother_server',
+                period=2.0,
-                name='smoother_server',
+                actions=[Node(
-                output='screen',
-                respawn=False,
-                respawn_delay=2.0,
-                parameters=[configured_params],
-                arguments=['--ros-args', '--log-level', log_level],
-            ),
-            Node(
                     package='nav2_lifecycle_manager',
                     executable='lifecycle_manager',
                     name='lifecycle_manager_planning',
                     output='screen',
                     arguments=['--ros-args', '--log-level', log_level],
-                parameters=[{'use_sim_time': True},
+                    parameters=[{
-                            {'autostart': True},
+                        'use_sim_time': sim_mode == 'true',
-                            {'node_names': lifecycle_nodes}]
+                        'autostart': True,
+                        'node_names': lifecycle_nodes,
+                    }]
+                )],
             ),
         ],
     )