diff --git a/config/nav2_params.yaml b/config/nav2_params.yaml index 1a76cb5..14fe6e4 100644 --- a/config/nav2_params.yaml +++ b/config/nav2_params.yaml @@ -26,7 +26,7 @@ amcl: recovery_alpha_fast: 0.0 recovery_alpha_slow: 0.0 resample_interval: 1 - robot_model_type: "differential" + robot_model_type: "nav2_amcl::DifferentialMotionModel" save_pose_rate: 0.5 sigma_hit: 0.2 tf_broadcast: true @@ -39,49 +39,71 @@ amcl: z_short: 0.05 scan_topic: scan -amcl_map_client: - ros__parameters: - use_sim_time: True - -amcl_rclcpp_node: - ros__parameters: - use_sim_time: True - bt_navigator: ros__parameters: use_sim_time: True global_frame: map robot_base_frame: base_link odom_topic: /odom - enable_groot_monitoring: True - groot_zmq_publisher_port: 1666 - groot_zmq_server_port: 1667 - default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml" + bt_loop_duration: 10 + default_server_timeout: 20 + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. plugin_lib_names: - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node - nav2_follow_path_action_bt_node - - nav2_back_up_action_bt_node - nav2_spin_action_bt_node - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node - nav2_clear_costmap_service_bt_node - nav2_is_stuck_condition_bt_node - nav2_goal_reached_condition_bt_node - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node - nav2_initial_pose_received_condition_bt_node - nav2_reinitialize_global_localization_service_bt_node - nav2_rate_controller_bt_node - nav2_distance_controller_bt_node - nav2_speed_controller_bt_node - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node - nav2_goal_updater_node_bt_node - nav2_recovery_node_bt_node - nav2_pipeline_sequence_bt_node - nav2_round_robin_node_bt_node - nav2_transform_available_condition_bt_node - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node -bt_navigator_rclcpp_node: +bt_navigator_navigate_through_poses_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator_navigate_to_pose_rclcpp_node: ros__parameters: use_sim_time: True @@ -92,8 +114,9 @@ controller_server: min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 progress_checker_plugin: "progress_checker" - goal_checker_plugin: "goal_checker" + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" controller_plugins: ["FollowPath"] # Progress checker parameters @@ -102,11 +125,16 @@ controller_server: required_movement_radius: 0.5 movement_time_allowance: 10.0 # Goal checker parameters - goal_checker: + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.25 + # yaw_goal_tolerance: 0.25 + # stateful: True + general_goal_checker: + stateful: True plugin: "nav2_controller::SimpleGoalChecker" xy_goal_tolerance: 0.25 yaw_goal_tolerance: 0.25 - stateful: True # DWB parameters FollowPath: plugin: "dwb_core::DWBLocalPlanner" @@ -150,10 +178,6 @@ controller_server: RotateToGoal.slowing_factor: 5.0 RotateToGoal.lookahead_time: -1.0 -controller_server_rclcpp_node: - ros__parameters: - use_sim_time: True - local_costmap: local_costmap: ros__parameters: @@ -188,15 +212,14 @@ local_costmap: clearing: True marking: True data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 static_layer: + plugin: "nav2_costmap_2d::StaticLayer" map_subscribe_transient_local: True always_send_full_costmap: True - local_costmap_client: - ros__parameters: - use_sim_time: True - local_costmap_rclcpp_node: - ros__parameters: - use_sim_time: True global_costmap: global_costmap: @@ -220,6 +243,10 @@ global_costmap: clearing: True marking: True data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 static_layer: plugin: "nav2_costmap_2d::StaticLayer" map_subscribe_transient_local: True @@ -228,25 +255,21 @@ global_costmap: cost_scaling_factor: 3.0 inflation_radius: 0.55 always_send_full_costmap: True - global_costmap_client: - ros__parameters: - use_sim_time: True - global_costmap_rclcpp_node: - ros__parameters: - use_sim_time: True map_server: ros__parameters: use_sim_time: True - yaml_filename: "turtlebot3_world.yaml" + # Overridden in launch by the "map" launch configuration or provided default value. + # To use in yaml, remove the default "map" value in the tb3_simulation_launch.py file & provide full path to map below. + yaml_filename: "" map_saver: ros__parameters: use_sim_time: True - save_map_timeout: 5000 + save_map_timeout: 5.0 free_thresh_default: 0.25 occupied_thresh_default: 0.65 - map_subscribe_transient_local: False + map_subscribe_transient_local: True planner_server: ros__parameters: @@ -259,25 +282,35 @@ planner_server: use_astar: false allow_unknown: true -planner_server_rclcpp_node: +smoother_server: ros__parameters: use_sim_time: True + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True -recoveries_server: +behavior_server: ros__parameters: costmap_topic: local_costmap/costmap_raw footprint_topic: local_costmap/published_footprint cycle_frequency: 10.0 - recovery_plugins: ["spin", "back_up", "wait"] + behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] spin: - plugin: "nav2_recoveries/Spin" - back_up: - plugin: "nav2_recoveries/BackUp" + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" wait: - plugin: "nav2_recoveries/Wait" + plugin: "nav2_behaviors/Wait" + assisted_teleop: + plugin: "nav2_behaviors/AssistedTeleop" global_frame: odom robot_base_frame: base_link - transform_timeout: 0.1 + transform_tolerance: 0.1 use_sim_time: true simulate_ahead_time: 2.0 max_rotational_vel: 1.0 @@ -286,4 +319,30 @@ recoveries_server: robot_state_publisher: ros__parameters: - use_sim_time: True \ No newline at end of file + use_sim_time: True + +waypoint_follower: + ros__parameters: + use_sim_time: True + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + use_sim_time: True + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [0.26, 0.0, 1.0] + min_velocity: [-0.26, 0.0, -1.0] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_topic: "odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 \ No newline at end of file diff --git a/launch/joy_teleop.launch.py b/launch/joy_teleop.launch.py index f5f9086..d7ac386 100644 --- a/launch/joy_teleop.launch.py +++ b/launch/joy_teleop.launch.py @@ -80,7 +80,7 @@ def generate_launch_description(): teleop_spawner = Node( package="rmp220_teleop", executable="rmp220_teleop", - remappings=[('/diffbot_base_controller/cmd_vel_unstamped','/cmd_vel_out')] + remappings=[('/diffbot_base_controller/cmd_vel_unstamped','/cmd_vel_joy')] ) cam_node = Node( diff --git a/launch/nav2.launch.py b/launch/nav2.launch.py index 487d4c9..f016df9 100644 --- a/launch/nav2.launch.py +++ b/launch/nav2.launch.py @@ -17,9 +17,12 @@ import os from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable -from launch.substitutions import LaunchConfiguration +from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration, PythonExpression +from launch_ros.actions import LoadComposableNodes from launch_ros.actions import Node +from launch_ros.descriptions import ComposableNode from nav2_common.launch import RewrittenYaml @@ -31,14 +34,19 @@ def generate_launch_description(): use_sim_time = LaunchConfiguration('use_sim_time') autostart = LaunchConfiguration('autostart') params_file = LaunchConfiguration('params_file') - default_bt_xml_filename = LaunchConfiguration('default_bt_xml_filename') - map_subscribe_transient_local = LaunchConfiguration('map_subscribe_transient_local') + use_composition = LaunchConfiguration('use_composition') + container_name = LaunchConfiguration('container_name') + container_name_full = (namespace, '/', container_name) + use_respawn = LaunchConfiguration('use_respawn') + log_level = LaunchConfiguration('log_level') lifecycle_nodes = ['controller_server', + 'smoother_server', 'planner_server', - 'recoveries_server', + 'behavior_server', 'bt_navigator', - 'waypoint_follower'] + 'waypoint_follower', + 'velocity_smoother'] # Map fully qualified names to relative ones so the node's namespace can be prepended. # In case of the transforms (tf), currently, there doesn't seem to be a better alternative @@ -52,9 +60,7 @@ def generate_launch_description(): # Create our own temporary YAML files that include substitutions param_substitutions = { 'use_sim_time': use_sim_time, - 'default_bt_xml_filename': default_bt_xml_filename, - 'autostart': autostart, - 'map_subscribe_transient_local': map_subscribe_transient_local} + 'autostart': autostart} configured_params = RewrittenYaml( source_file=params_file, @@ -62,84 +68,203 @@ def generate_launch_description(): param_rewrites=param_substitutions, convert_types=True) - return LaunchDescription([ - # Set env var to print messages to stdout immediately - SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + stdout_linebuf_envvar = SetEnvironmentVariable( + 'RCUTILS_LOGGING_BUFFERED_STREAM', '1') - DeclareLaunchArgument( - 'namespace', default_value='', - description='Top-level namespace'), + declare_namespace_cmd = DeclareLaunchArgument( + 'namespace', + default_value='', + description='Top-level namespace') - DeclareLaunchArgument( - 'use_sim_time', default_value='false', - description='Use simulation (Gazebo) clock if true'), + declare_use_sim_time_cmd = DeclareLaunchArgument( + 'use_sim_time', + default_value='false', + description='Use simulation (Gazebo) clock if true') - DeclareLaunchArgument( - 'autostart', default_value='true', - description='Automatically startup the nav2 stack'), + declare_params_file_cmd = DeclareLaunchArgument( + 'params_file', + default_value=os.path.join(bringup_dir, 'config', 'nav2_params.yaml'), + description='Full path to the ROS2 parameters file to use for all launched nodes') - DeclareLaunchArgument( - 'params_file', - default_value=os.path.join(bringup_dir, 'config', 'nav2_params.yaml'), - description='Full path to the ROS2 parameters file to use'), + declare_autostart_cmd = DeclareLaunchArgument( + 'autostart', default_value='true', + description='Automatically startup the nav2 stack') - DeclareLaunchArgument( - 'default_bt_xml_filename', - default_value=os.path.join( - get_package_share_directory('nav2_bt_navigator'), - 'behavior_trees', 'navigate_w_replanning_and_recovery.xml'), - description='Full path to the behavior tree xml file to use'), + declare_use_composition_cmd = DeclareLaunchArgument( + 'use_composition', default_value='False', + description='Use composed bringup if True') - DeclareLaunchArgument( - 'map_subscribe_transient_local', default_value='false', - description='Whether to set the map subscriber QoS to transient local'), + declare_container_name_cmd = DeclareLaunchArgument( + 'container_name', default_value='nav2_container', + description='the name of conatiner that nodes will load in if use composition') - Node( - package='nav2_controller', - executable='controller_server', - output='screen', - parameters=[configured_params], - remappings=remappings), + declare_use_respawn_cmd = DeclareLaunchArgument( + 'use_respawn', default_value='False', + description='Whether to respawn if a node crashes. Applied when composition is disabled.') - Node( - package='nav2_planner', - executable='planner_server', - name='planner_server', - output='screen', - parameters=[configured_params], - remappings=remappings), + declare_log_level_cmd = DeclareLaunchArgument( + 'log_level', default_value='info', + description='log level') - Node( - package='nav2_recoveries', - executable='recoveries_server', - name='recoveries_server', - output='screen', - parameters=[configured_params], - remappings=remappings), + load_nodes = GroupAction( + condition=IfCondition(PythonExpression(['not ', use_composition])), + actions=[ + Node( + package='nav2_controller', + executable='controller_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings + [('cmd_vel', 'cmd_vel_nav')]), + Node( + package='nav2_smoother', + executable='smoother_server', + name='smoother_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_planner', + executable='planner_server', + name='planner_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_behaviors', + executable='behavior_server', + name='behavior_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_bt_navigator', + executable='bt_navigator', + name='bt_navigator', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings), + Node( + package='nav2_waypoint_follower', + executable='waypoint_follower', + name='waypoint_follower', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings), + Node( + package='nav2_velocity_smoother', + executable='velocity_smoother', + name='velocity_smoother', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings + + [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')]), + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_navigation', + output='screen', + arguments=['--ros-args', '--log-level', log_level], + parameters=[{'use_sim_time': use_sim_time}, + {'autostart': autostart}, + {'node_names': lifecycle_nodes}]), + ] + ) - Node( - package='nav2_bt_navigator', - executable='bt_navigator', - name='bt_navigator', - output='screen', - parameters=[configured_params], - remappings=remappings), + load_composable_nodes = LoadComposableNodes( + condition=IfCondition(use_composition), + target_container=container_name_full, + composable_node_descriptions=[ + ComposableNode( + package='nav2_controller', + plugin='nav2_controller::ControllerServer', + name='controller_server', + parameters=[configured_params], + remappings=remappings + [('cmd_vel', 'cmd_vel_nav')]), + ComposableNode( + package='nav2_smoother', + plugin='nav2_smoother::SmootherServer', + name='smoother_server', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_planner', + plugin='nav2_planner::PlannerServer', + name='planner_server', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_behaviors', + plugin='behavior_server::BehaviorServer', + name='behavior_server', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_bt_navigator', + plugin='nav2_bt_navigator::BtNavigator', + name='bt_navigator', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_waypoint_follower', + plugin='nav2_waypoint_follower::WaypointFollower', + name='waypoint_follower', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_velocity_smoother', + plugin='nav2_velocity_smoother::VelocitySmoother', + name='velocity_smoother', + parameters=[configured_params], + remappings=remappings + + [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')]), + ComposableNode( + package='nav2_lifecycle_manager', + plugin='nav2_lifecycle_manager::LifecycleManager', + name='lifecycle_manager_navigation', + parameters=[{'use_sim_time': use_sim_time, + 'autostart': autostart, + 'node_names': lifecycle_nodes}]), + ], + ) - Node( - package='nav2_waypoint_follower', - executable='waypoint_follower', - name='waypoint_follower', - output='screen', - parameters=[configured_params], - remappings=remappings), + # Create the launch description and populate + ld = LaunchDescription() - Node( - package='nav2_lifecycle_manager', - executable='lifecycle_manager', - name='lifecycle_manager_navigation', - output='screen', - parameters=[{'use_sim_time': use_sim_time}, - {'autostart': autostart}, - {'node_names': lifecycle_nodes}]), + # Set environment variables + ld.add_action(stdout_linebuf_envvar) - ]) \ No newline at end of file + # Declare the launch options + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_params_file_cmd) + ld.add_action(declare_autostart_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_log_level_cmd) + # Add the actions to launch all of the navigation nodes + ld.add_action(load_nodes) + ld.add_action(load_composable_nodes) + + return ld \ No newline at end of file