diff --git a/.gitignore b/.gitignore index 76b3365..c8d7bf2 100644 --- a/.gitignore +++ b/.gitignore @@ -3,3 +3,6 @@ install/ log/ .colcon/ *.log +__pycache__/ +**/__pycache__/ +*.py[cod] diff --git a/src/planning_pkg/behavior/navigate_to_pose_no_spin.xml b/src/planning_pkg/behavior/navigate_to_pose_no_spin.xml index 3537e32..13bb784 100755 --- a/src/planning_pkg/behavior/navigate_to_pose_no_spin.xml +++ b/src/planning_pkg/behavior/navigate_to_pose_no_spin.xml @@ -4,31 +4,11 @@ --> - - - - - - - - - - - - - - - - - - - - - - - + + + + + + diff --git a/src/planning_pkg/config/planning_2d_sim_turtlebot3.yaml b/src/planning_pkg/config/planning_2d_sim_turtlebot3.yaml index fd51698..228a656 100644 --- a/src/planning_pkg/config/planning_2d_sim_turtlebot3.yaml +++ b/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 diff --git a/src/planning_pkg/launch/__pycache__/nav2_planning.launch.cpython-310.pyc b/src/planning_pkg/launch/__pycache__/nav2_planning.launch.cpython-310.pyc deleted file mode 100644 index 20e8a4c..0000000 Binary files a/src/planning_pkg/launch/__pycache__/nav2_planning.launch.cpython-310.pyc and /dev/null differ diff --git a/src/planning_pkg/launch/nav2_planning.launch.py b/src/planning_pkg/launch/nav2_planning.launch.py index 5431691..974cba6 100755 --- a/src/planning_pkg/launch/nav2_planning.launch.py +++ b/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', - 'smoother_server', - 'planner_server', + lifecycle_nodes = ['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([ - pkg_planning, - 'behavior', - 'navigate_through_poses_no_spin.xml' - ]) + # Humble starts the NavigateThroughPoses navigator as well. Reuse the + # minimal pose tree so its optional recovery actions are not required. + default_nav_through_poses_bt_xml = default_nav_to_pose_bt_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( - package='nav2_smoother', - executable='smoother_server', - name='smoother_server', - 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}, - {'autostart': True}, - {'node_names': lifecycle_nodes}] + # Let the Nav2 servers finish creating their lifecycle services first. + TimerAction( + period=2.0, + actions=[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': sim_mode == 'true', + 'autostart': True, + 'node_names': lifecycle_nodes, + }] + )], ), ], )