From f1aee4d445809f88aff74ac5f63e58732cf9f060 Mon Sep 17 00:00:00 2001 From: Chris Iverach-Brereton Date: Wed, 21 Aug 2024 14:28:43 -0400 Subject: [PATCH] Update nav2 launch configuration to work with the latest jazzy: - syntax update (/ -> ::) - add docking_server parameters - make sure enable_stamped_cmd_vel is true - path follower plugin dwb_core::DWBLocalPlanner -> nav2_mppi_controller::MPPIController Tested & working in simulation. Not yet tested on physical robot (waiting on firmware update) --- turtlebot4_navigation/config/nav2.yaml | 381 +++++++++++++++---------- 1 file changed, 232 insertions(+), 149 deletions(-) diff --git a/turtlebot4_navigation/config/nav2.yaml b/turtlebot4_navigation/config/nav2.yaml index aa93ca7..4ff52de 100644 --- a/turtlebot4_navigation/config/nav2.yaml +++ b/turtlebot4_navigation/config/nav2.yaml @@ -1,150 +1,140 @@ bt_navigator: ros__parameters: - use_sim_time: True + use_sim_time: true + enable_stamped_cmd_vel: true global_frame: map - robot_base_frame: base_link + robot_base_frame: base_footprint odom_topic: odom 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_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_navigate_through_poses_rclcpp_node: - ros__parameters: - use_sim_time: True - -bt_navigator_navigate_to_pose_rclcpp_node: - ros__parameters: - use_sim_time: True + wait_for_service_timeout: 1000 + action_server_result_timeout: 900.0 + navigators: ["navigate_to_pose", "navigate_through_poses"] + navigate_to_pose: + plugin: "nav2_bt_navigator::NavigateToPoseNavigator" + navigate_through_poses: + plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator" + error_code_names: + - compute_path_error_code + - follow_path_error_code controller_server: ros__parameters: - use_sim_time: True + use_sim_time: true + enable_stamped_cmd_vel: true controller_frequency: 20.0 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_plugins: ["general_goal_checker"] # "precise_goal_checker" + progress_checker_plugins: ["progress_checker"] + goal_checker_plugins: ["general_goal_checker"] controller_plugins: ["FollowPath"] - - # Progress checker parameters + use_realtime_priority: false progress_checker: plugin: "nav2_controller::SimpleProgressChecker" required_movement_radius: 0.5 movement_time_allowance: 10.0 - # Goal checker parameters - #precise_goal_checker: - # plugin: "nav2_controller::SimpleGoalChecker" - # xy_goal_tolerance: 0.25 - # yaw_goal_tolerance: 0.25 - # stateful: True general_goal_checker: - stateful: True + stateful: true plugin: "nav2_controller::SimpleGoalChecker" xy_goal_tolerance: 0.25 yaw_goal_tolerance: 0.25 - # DWB parameters FollowPath: - plugin: "dwb_core::DWBLocalPlanner" - debug_trajectory_details: True - min_vel_x: 0.0 - min_vel_y: 0.0 - max_vel_x: 0.26 - max_vel_y: 0.0 - max_vel_theta: 1.0 - min_speed_xy: 0.0 - max_speed_xy: 0.26 - min_speed_theta: 0.0 - # Add high threshold velocity for turtlebot 3 issue. - # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 - acc_lim_x: 2.5 - acc_lim_y: 0.0 - acc_lim_theta: 3.2 - decel_lim_x: -2.5 - decel_lim_y: 0.0 - decel_lim_theta: -3.2 - vx_samples: 20 - vy_samples: 5 - vtheta_samples: 20 - sim_time: 1.7 - linear_granularity: 0.05 - angular_granularity: 0.025 - transform_tolerance: 0.2 - xy_goal_tolerance: 0.25 - trans_stopped_velocity: 0.25 - short_circuit_trajectory_evaluation: True - stateful: True - critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] - BaseObstacle.scale: 0.02 - PathAlign.scale: 32.0 - PathAlign.forward_point_distance: 0.1 - GoalAlign.scale: 24.0 - GoalAlign.forward_point_distance: 0.1 - PathDist.scale: 32.0 - GoalDist.scale: 24.0 - RotateToGoal.scale: 32.0 - RotateToGoal.slowing_factor: 5.0 - RotateToGoal.lookahead_time: -1.0 + plugin: "nav2_mppi_controller::MPPIController" + time_steps: 56 + model_dt: 0.05 + batch_size: 2000 + ax_max: 3.0 + ax_min: -3.0 + ay_max: 3.0 + az_max: 3.5 + vx_std: 0.2 + vy_std: 0.2 + wz_std: 0.4 + vx_max: 0.5 + vx_min: -0.35 + vy_max: 0.5 + wz_max: 1.9 + iteration_count: 1 + prune_distance: 1.7 + transform_tolerance: 0.1 + temperature: 0.3 + gamma: 0.015 + motion_model: "DiffDrive" + visualize: true + regenerate_noises: true + TrajectoryVisualizer: + trajectory_step: 5 + time_step: 3 + AckermannConstraints: + min_turning_r: 0.2 + critics: [ + "ConstraintCritic", "CostCritic", "GoalCritic", + "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", + "PathAngleCritic", "PreferForwardCritic"] + ConstraintCritic: + enabled: true + cost_power: 1 + cost_weight: 4.0 + GoalCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 1.4 + GoalAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 3.0 + threshold_to_consider: 0.5 + PreferForwardCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 0.5 + CostCritic: + enabled: true + cost_power: 1 + cost_weight: 3.81 + critical_cost: 300.0 + consider_footprint: true + collision_cost: 1000000.0 + near_goal_distance: 1.0 + trajectory_point_step: 2 + PathAlignCritic: + enabled: true + cost_power: 1 + cost_weight: 14.0 + max_path_occupancy_ratio: 0.05 + trajectory_point_step: 4 + threshold_to_consider: 0.5 + offset_from_furthest: 20 + use_path_orientations: false + PathFollowCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + offset_from_furthest: 5 + threshold_to_consider: 1.4 + PathAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 2.0 + offset_from_furthest: 4 + threshold_to_consider: 0.5 + max_angle_to_furthest: 1.0 + mode: 0 local_costmap: local_costmap: ros__parameters: + use_sim_time: true + enable_stamped_cmd_vel: true update_frequency: 5.0 publish_frequency: 2.0 global_frame: odom - robot_base_frame: base_link - use_sim_time: True + robot_base_frame: base_footprint + use_sim_time: true rolling_window: true width: 3 height: 3 @@ -157,8 +147,8 @@ local_costmap: inflation_radius: 0.45 voxel_layer: plugin: "nav2_costmap_2d::VoxelLayer" - enabled: True - publish_voxel_map: True + enabled: true + publish_voxel_map: true origin_z: 0.0 z_resolution: 0.05 z_voxels: 16 @@ -168,8 +158,8 @@ local_costmap: scan: topic: scan max_obstacle_height: 2.0 - clearing: True - marking: True + clearing: true + marking: true data_type: "LaserScan" raytrace_max_range: 3.0 raytrace_min_range: 0.0 @@ -177,30 +167,32 @@ local_costmap: obstacle_min_range: 0.0 static_layer: plugin: "nav2_costmap_2d::StaticLayer" - map_subscribe_transient_local: True - always_send_full_costmap: True + map_subscribe_transient_local: true + always_send_full_costmap: true global_costmap: global_costmap: ros__parameters: + use_sim_time: true + enable_stamped_cmd_vel: true update_frequency: 1.0 publish_frequency: 1.0 global_frame: map - robot_base_frame: base_link - use_sim_time: True + robot_base_frame: base_footprint + use_sim_time: true robot_radius: 0.175 resolution: 0.06 track_unknown_space: true plugins: ["static_layer", "obstacle_layer", "inflation_layer"] obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" - enabled: True + enabled: true observation_sources: scan scan: topic: scan max_obstacle_height: 2.0 - clearing: True - marking: True + clearing: true + marking: true data_type: "LaserScan" raytrace_max_range: 3.0 raytrace_min_range: 0.0 @@ -208,54 +200,61 @@ global_costmap: obstacle_min_range: 0.0 static_layer: plugin: "nav2_costmap_2d::StaticLayer" - map_subscribe_transient_local: True + map_subscribe_transient_local: true inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 4.0 inflation_radius: 0.45 - always_send_full_costmap: True + always_send_full_costmap: true planner_server: ros__parameters: - expected_planner_frequency: 20.0 - use_sim_time: True + use_sim_time: true + enable_stamped_cmd_vel: true + #expected_planner_frequency: 20.0 + use_sim_time: true planner_plugins: ["GridBased"] GridBased: - plugin: "nav2_navfn_planner/NavfnPlanner" + plugin: "nav2_navfn_planner::NavfnPlanner" tolerance: 0.5 use_astar: false allow_unknown: true smoother_server: ros__parameters: - use_sim_time: True + use_sim_time: true + enable_stamped_cmd_vel: true smoother_plugins: ["simple_smoother"] simple_smoother: plugin: "nav2_smoother::SimpleSmoother" tolerance: 1.0e-10 max_its: 1000 - do_refinement: True + do_refinement: true behavior_server: ros__parameters: - costmap_topic: local_costmap/costmap_raw - footprint_topic: local_costmap/published_footprint + use_sim_time: true + enable_stamped_cmd_vel: true + local_costmap_topic: local_costmap/costmap_raw + global_costmap_topic: global_costmap/costmap_raw + local_footprint_topic: local_costmap/published_footprint + global_footprint_topic: global_costmap/published_footprint cycle_frequency: 10.0 behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] spin: - plugin: "nav2_behaviors/Spin" + plugin: "nav2_behaviors::Spin" backup: - plugin: "nav2_behaviors/BackUp" + plugin: "nav2_behaviors::BackUp" drive_on_heading: - plugin: "nav2_behaviors/DriveOnHeading" + plugin: "nav2_behaviors::DriveOnHeading" wait: - plugin: "nav2_behaviors/Wait" + plugin: "nav2_behaviors::Wait" assisted_teleop: - plugin: "nav2_behaviors/AssistedTeleop" - global_frame: odom - robot_base_frame: base_link + plugin: "nav2_behaviors::AssistedTeleop" + global_frame: map + local_frame: odom + robot_base_frame: base_footprint transform_tolerance: 0.1 - use_sim_time: true simulate_ahead_time: 2.0 max_rotational_vel: 1.0 min_rotational_vel: 0.4 @@ -263,22 +262,25 @@ behavior_server: robot_state_publisher: ros__parameters: - use_sim_time: True + use_sim_time: true waypoint_follower: ros__parameters: - use_sim_time: True + use_sim_time: true + enable_stamped_cmd_vel: true 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" - enabled: True + enabled: true waypoint_pause_duration: 200 velocity_smoother: ros__parameters: - use_sim_time: True + use_sim_time: true + enable_stamped_cmd_vel: true smoothing_frequency: 20.0 scale_velocities: False feedback: "OPEN_LOOP" @@ -289,4 +291,85 @@ velocity_smoother: 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 + velocity_timeout: 1.0 + +collision_monitor: + ros__parameters: + use_sim_time: true + enable_stamped_cmd_vel: true + base_frame_id: "base_footprint" + odom_frame_id: "odom" + cmd_vel_in_topic: "cmd_vel_smoothed" + cmd_vel_out_topic: "cmd_vel" + state_topic: "collision_monitor_state" + transform_tolerance: 0.2 + source_timeout: 1.0 + base_shift_correction: True + stop_pub_timeout: 2.0 + # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, + # and robot footprint for "approach" action type. + polygons: ["FootprintApproach"] + FootprintApproach: + type: "polygon" + action_type: "approach" + footprint_topic: "/local_costmap/published_footprint" + time_before_collision: 1.2 + simulation_time_step: 0.1 + min_points: 6 + visualize: False + enabled: True + observation_sources: ["scan"] + scan: + type: "scan" + topic: "scan" + min_height: 0.15 + max_height: 2.0 + enabled: True + +docking_server: + ros__parameters: + use_sim_time: true + enable_stamped_cmd_vel: true + controller_frequency: 50.0 + initial_perception_timeout: 5.0 + wait_charge_timeout: 5.0 + dock_approach_timeout: 30.0 + undock_linear_tolerance: 0.05 + undock_angular_tolerance: 0.1 + max_retries: 3 + base_frame: "base_footprint" + fixed_frame: "odom" + dock_backwards: false + dock_prestaging_tolerance: 0.5 + + # Types of docks + dock_plugins: ['simple_charging_dock'] + simple_charging_dock: + plugin: 'opennav_docking::SimpleChargingDock' + docking_threshold: 0.05 + staging_x_offset: -0.7 + use_external_detection_pose: true + use_battery_status: false # true + use_stall_detection: false # true + + external_detection_timeout: 1.0 + external_detection_translation_x: -0.18 + external_detection_translation_y: 0.0 + external_detection_rotation_roll: -1.57 + external_detection_rotation_pitch: -1.57 + external_detection_rotation_yaw: 0.0 + filter_coef: 0.1 + + # Dock instances + # The following example illustrates configuring dock instances. + # docks: ['home_dock'] # Input your docks here + # home_dock: + # type: 'simple_charging_dock' + # frame: map + # pose: [0.0, 0.0, 0.0] + + controller: + k_phi: 3.0 + k_delta: 2.0 + v_linear_min: 0.15 + v_linear_max: 0.15 \ No newline at end of file