search

ros-navigation / navigation2

ROS 2 Navigation Framework and System
https://nav2.org/
Other
2.64k stars 1.31k forks source link

The speed of the differentially driven AMR robot in the DWB does not decrease near the cost map #4279

Closed frkngltk closed 6 months ago

frkngltk commented 7 months ago

Required Info:

Information about the problem

Hello, I wish everyone a good day. I have an AMR(differential drive) robot that I work with in real time. In accordance with the main system <link> : https://navigation.ros.org/configuration/packages/configuring-dwb-controller.html from this site and this parameter page <link> : https://github.com/ros-planning/navigation2/blob/main/nav2_bringup/params/nav2_params.yaml I made adjustments according to my own robot. You can see my tuning file below.

amcl:
  ros__parameters:
    use_sim_time: False
    alpha1: 0.9
    alpha2: 0.1
    alpha3: 0.05
    alpha4: 0.01
    alpha5: 0.04
    base_frame_id: "base_footprint"
    beam_skip_distance: 0.5
    beam_skip_error_threshold: 0.9
    beam_skip_threshold: 0.3
    do_beamskip: false
    global_frame_id: "map"
    lambda_short: 0.1
    laser_likelihood_max_dist: 2.0
    laser_max_range: 100.0
    laser_min_range: -1.0
    laser_model_type: "likelihood_field"
    max_beams: 60
    max_particles: 2000
    min_particles: 500
    odom_frame_id: "odom"
    pf_err: 0.02
    pf_z: 0.85
    recovery_alpha_fast: 0.0
    recovery_alpha_slow: 0.0
    resample_interval: 2
    robot_model_type: "nav2_amcl::DifferentialMotionModel"
    save_pose_rate: 0.5
    sigma_hit: 0.02
    tf_broadcast: true
    transform_tolerance: 0.5
    update_min_a: 0.06
    update_min_d: 0.025
    z_hit: 0.7
    z_max: 0.001
    z_rand: 0.059
    z_short: 0.24
    scan_topic: scan_front_filtered

    set_initial_pose: True
    initial_pose.x: 0.0
    initial_pose.y: 0.0
    initial_pose.z: 0.0
    initial_pose.yaw: 0.0

amcl_map_client:
  ros__parameters:
    use_sim_time: False

amcl_rclcpp_node:
  ros__parameters:
    use_sim_time: False

bt_navigator:
  ros__parameters:
    use_sim_time: False
    global_frame: map
    robot_base_frame: base_footprint
    odom_topic: /odom
    bt_loop_duration: 10
    default_server_timeout: 20
    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_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_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_drive_on_heading_cancel_bt_node

bt_navigator_rclcpp_node:
  ros__parameters:
    use_sim_time: False

controller_server:
  ros__parameters:
    use_sim_time: False
    controller_frequency: 35.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"] 
    controller_plugins: ["FollowPath"]
    speed_limit_topic: speed_limit

    progress_checker:
      plugin: "nav2_controller::SimpleProgressChecker"
      required_movement_radius: 0.5
      movement_time_allowance: 10.0
    general_goal_checker:
      stateful: True
      plugin: "nav2_controller::SimpleGoalChecker"
      xy_goal_tolerance: 0.25
      yaw_goal_tolerance: 0.25
    FollowPath:
      plugin: "dwb_core::DWBLocalPlanner"
      debug_trajectory_details: True
      split_path: True
      min_vel_x: -1.0
      min_vel_y: 0.0
      max_vel_x: 1.0
      max_vel_y: 0.0
      max_vel_theta: 0.55
      min_speed_xy: 0.2
      max_speed_xy: 1.0
      min_speed_theta: 0.2
      acc_lim_x: 0.11
      acc_lim_y: 0.0
      acc_lim_theta: 0.75
      decel_lim_x: -0.4
      decel_lim_y: 0.0
      decel_lim_theta: -1.75
      vx_samples: 32
      vy_samples: 0
      vtheta_samples: 32
      sim_time: 1.7
      time_granularity: 0.5
      linear_granularity: 0.05
      prune_distance: 2.0
      prune_plan: true
      publish_cost_grid_pc: false
      publish_evaluation: true
      publish_global_plan: true
      publish_local_plan: true
      publish_trajectories: true
      publish_transformed_plan: true
      shorten_transformed_plan: true
      trajectory_generator_name: dwb_plugins::StandardTrajectoryGenerator
      angular_granularity: 0.025
      transform_tolerance: 0.2
      xy_goal_tolerance: 0.25
      trans_stopped_velocity: 0.25
      marker_lifetime: 0.1
      forward_prune_distance: 2.0
      short_circuit_trajectory_evaluation: True
      stateful: True
      critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist", "ObstacleFootprint"]
      BaseObstacle.scale: 1.0
      PathAlign.scale: 32.0
      PathAlign.forward_point_distance: 0.1
      GoalAlign.scale: 32.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
      ObstacleFootprint.scale: 5.0
      Oscillation.oscillation_reset_angle: 0.2
      Oscillation.oscillation_reset_dist: 0.1
      Oscillation.oscillation_reset_time: 3.0
      Oscillation.scale: 1.0
      Oscillation.x_only_threshold: 0.05

controller_server_rclcpp_node:
  ros__parameters:
    use_sim_time: False

local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 12.0
      publish_frequency: 2.0
      global_frame: odom
      robot_base_frame: base_footprint
      use_sim_time: False
      rolling_window: true
      width: 15
      height: 15
      resolution: 0.05
      footprint: "[[0.525, 0.344], [0.525, -0.344], [-0.525, -0.344], [-0.525, 0.344]]"
      robot_radius: 0.627
      plugins: ["voxel_layer", "inflation_layer"]
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 2.58
        inflation_radius: 0.67
      voxel_layer:
        plugin: "nav2_costmap_2d::VoxelLayer"
        enabled: True
        publish_voxel_map: True
        origin_z: 0.0
        z_resolution: 0.05
        z_voxels: 16
        max_obstacle_height: 2.0
        mark_threshold: 0
        observation_sources: scan scan_front
        scan:
          topic: /scan_filtered
          max_obstacle_height: 2.0
          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
        scan_front:
          topic: /scan_front_filtered
          max_obstacle_height: 2.0
          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

      always_send_full_costmap: True
  local_costmap_client:
    ros__parameters:
      use_sim_time: False
  local_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: False

global_costmap:
  global_costmap:
    ros__parameters:
      update_frequency: 0.5
      publish_frequency: 0.5
      global_frame: map
      robot_base_frame: base_footprint
      use_sim_time: False
      footprint: "[[0.525, 0.344], [0.525, -0.344], [-0.525, -0.344], [-0.525, 0.344]]"
      robot_radius: 0.627
      resolution: 0.05
      track_unknown_space: true
      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: scan scan_front
        scan:
          topic: /scan_filtered
          max_obstacle_height: 2.0
          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
        scan_front:
          topic: /scan_front_filtered
          max_obstacle_height: 2.0
          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
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 2.58
        inflation_radius: 0.67
      always_send_full_costmap: True
  global_costmap_client:
    ros__parameters:
      use_sim_time: False
  global_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: False

map_server:
  ros__parameters:
    use_sim_time: False
    yaml_filename: "turtlebot3_world.yaml"

map_saver:
  ros__parameters:
    use_sim_time: False
    save_map_timeout: 5.0
    free_thresh_default: 0.25
    occupied_thresh_default: 0.65
    map_subscribe_transient_local: True

planner_server:
  ros__parameters:
    expected_planner_frequency: 20.0
    use_sim_time: False
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_navfn_planner/NavfnPlanner"
      tolerance: 0.5
      use_astar: false
      allow_unknown: true

planner_server_rclcpp_node:
  ros__parameters:
    use_sim_time: False

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

behavior_server:
  ros__parameters:
    costmap_topic: local_costmap/costmap_raw
    footprint_topic: local_costmap/published_footprint
    cycle_frequency: 10.0
    behavior_plugins: ["spin", "backup", "drive_on_heading", "wait"]
    spin:
      plugin: "nav2_behaviors/Spin"
    backup:
      plugin: "nav2_behaviors/BackUp"
    drive_on_heading:
      plugin: "nav2_behaviors/DriveOnHeading"
    wait:
      plugin: "nav2_behaviors/Wait"
    global_frame: odom
    robot_base_frame: base_footprint
    transform_tolerance: 0.1
    use_sim_time: False
    simulate_ahead_time: 2.0
    max_rotational_vel: 0.3
    min_rotational_vel: 0.05
    rotational_acc_lim: 0.3

robot_state_publisher:
  ros__parameters:
    use_sim_time: False

waypoint_follower:
  ros__parameters:
    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

According to this file, the robot moves from point A to point B according to acceleration and deceleration data. My main question is this: When it encounters any obstacle on the way to the target I sent, it reaches the point by bypassing this obstacle, but no slowdown is observed. In short, even if the robot detects the costmaps formed next to it while it is moving, it does not reduce the max_vel_x value and goes to the target point at high speed. I also demonstrated this in the video below. I would appreciate it if you could help me with this problem.

https://github.com/ros-planning/navigation2/assets/46001550/9786787c-8f0d-45b7-8a27-3ac94608a734

Note

I have just started working on ROS2 and NAV2. I am trying to transfer the AMR robot from the ROS1 system to the ROS2 system.

SteveMacenski commented 7 months ago

You likely need to tune DWB to get your desired behavior. There are many critic plugins and relative weights you can set to get the behavior that you want. Since DWB is a scoring-based local trajectory planner, fine tuning is required to balance the different objectives (and costmap settings).

frkngltk commented 7 months ago

Thanks @SteveMacenski. I fully understood the DWB structure and examined many structures on forum sites and this channel to see what each parameter does. OscillationCritic, BaseObstacleCritic, ObstacleFootprintCritic etc. I thought the structures would support this problem. In addition, structures such as cost_scaling_factor, which are among the local_costmap parameters, should affect the system. The main problem is that these parameters do not work in simulation environments such as Turtlebot, other than the ones I set myself. I think that in the NAV2 system, when an object or object moves close to the robot, it should slow down automatically due to the cost_map created. According to what you said, I understand that each parameter triggers this situation, is it true? Briefly, with which parameter or with what technique can I solve this problem? Shouldn't this situation be automatically active in the NAV2 system (since it is used with AMR devices)?

SteveMacenski commented 7 months ago

That is what the critic objective function are for, you can define that behavior. Different applications want different behaviors for different situations, MPPI, DWB, etc are built as general solutions that can be adapted to produce the behavior that you want :-)

SteveMacenski commented 7 months ago

I believe this was answered, can we close this out?

frkngltk commented 6 months ago

I can't say it's fully resolved, but I'm trying the parameters of your specification. I will also try the Collision monitor structure. Thank you.

frkngltk commented 6 months ago

Hello @SteveMacenski. I am trying the structures I mentioned. But I couldn't reach any conclusion. Here you can use critic plugins, cost_map settings, or Smoother Server, Collision Monitor, Velocity Smoother etc. in the Configuration Guide link. I used structures. According to my observation, there is no change. Frankly, when I run ROS1 with the same parameters on the same robot, I observe a speed slowdown in narrow areas, while the speed increases in wide areas. In the ROS2 nav2 system, there is no speed reduction in narrow or wide areas. I examined in detail the locations of the Critics parameters. I cannot observe any changes based on real-time observation as a result of the adjustment. Do you have any suggestions regarding this subject? I would be glad if you could help.

SteveMacenski commented 6 months ago

DWB and DWA from ROS 1 are not the same package, though they are related. Like I mentioned above, you will need to tune it to your application desires.

However, I might recommend trying out MPPI. I believe its a pretty big step function up in capabilities and I think it does more or less what you want out of the box how I tuned it.