1
$\begingroup$

Rosanswers logo

I am using ros2 and the navigation2 stack with dwb local planner.
Moving along a path works quite good until I come close to the goal.
As soon as I am about half a meter away from goal, local planner stops and recovery behavior starts.
No debug information thats helping in any way from local_controller.

Tried with default params and played with planner costs but didn't help.

animated problem

Logs:

[bt_navigator-9] 1576919591.457560223: [osg_1.bt_navigator] [INFO]      Begin navigating from current location to (16.29, -5.22)
[controller_server-6] 1576919591.484765495: [osg_1.controller_server] [INFO]    Received a goal, begin computing control effort.
[controller_server-6] 1576919592.538323109: [osg_1.controller_server] [INFO]    Preempting the goal. Passing the new path to the planner.
[controller_server-6] 1576919593.537786932: [osg_1.controller_server] [INFO]    Preempting the goal. Passing the new path to the planner.
[controller_server-6] 1576919594.588264789: [osg_1.controller_server] [INFO]    Preempting the goal. Passing the new path to the planner.
[controller_server-6] 1576919595.588693952: [osg_1.controller_server] [INFO]    Preempting the goal. Passing the new path to the planner.
[controller_server-6] 1576919596.588160183: [osg_1.controller_server] [INFO]    Preempting the goal. Passing the new path to the planner.
[controller_server-6] 1576919597.587782147: [osg_1.controller_server] [INFO]    Preempting the goal. Passing the new path to the planner.
[controller_server-6] 1576919598.638209477: [osg_1.controller_server] [INFO]    Preempting the goal. Passing the new path to the planner.
[controller_server-6] 1576919599.638641570: [osg_1.controller_server] [INFO]    Preempting the goal. Passing the new path to the planner.
[controller_server-6] 1576919600.638103478: [osg_1.controller_server] [INFO]    Preempting the goal. Passing the new path to the planner.
[controller_server-6] 1576919601.637982831: [osg_1.controller_server] [INFO]    Preempting the goal. Passing the new path to the planner.
[controller_server-6] 1576919602.687874252: [osg_1.controller_server] [INFO]    Preempting the goal. Passing the new path to the planner.
[controller_server-6] 1576919603.687692602: [osg_1.controller_server] [INFO]    Preempting the goal. Passing the new path to the planner.
[controller_server-6] 1576919604.688053416: [osg_1.controller_server] [INFO]    Preempting the goal. Passing the new path to the planner.
[controller_server-6] 1576919605.738370498: [osg_1.controller_server] [INFO]    Preempting the goal. Passing the new path to the planner.
[controller_server-6] 1576919606.737878037: [osg_1.controller_server] [INFO]    Preempting the goal. Passing the new path to the planner.
[controller_server-6] 1576919607.738398407: [osg_1.controller_server] [INFO]    Preempting the goal. Passing the new path to the planner.
[bt_navigator-9] 1576919608.102711864: [osg_1.bt_navigator] [INFO]      Received goal preemption request
[bt_navigator-9] 1576919608.103001664: [osg_1.bt_navigator] [INFO]      Begin navigating from current location to (15.96, -3.88)
[controller_server-6] 1576919608.738182619: [osg_1.controller_server] [INFO]    Preempting the goal. Passing the new path to the planner.
[controller_server-6] 1576919609.787711291: [osg_1.controller_server] [INFO]    Preempting the goal. Passing the new path to the planner.
[controller_server-6] 1576919610.187985470: [osg_1.controller_server] [ERROR]   Failed to make progress
[controller_server-6] 1576919610.188201041: [osg_1.controller_server_rclcpp_node] [WARN]        [follow_path] [ActionServer] Aborting handle.
[controller_server-6] 1576919610.189754250: [osg_1.local_costmap.local_costmap] [INFO]  Received request to clear entirely the local_costmap
[controller_server-6] 1576919610.190666358: [osg_1.local_costmap.local_costmap] [INFO]  Received request to clear entirely the local_costmap
[planner_server-7] 1576919610.192350578: [osg_1.global_costmap.global_costmap] [INFO]   Received request to clear entirely the global_costmap
[recoveries_server-8] 1576919610.195166780: [osg_1.recoveries_server] [INFO]    Attempting spin
[recoveries_server-8] 1576919610.195413958: [osg_1.recoveries_server] [INFO]    Turning -1.57 for spin recovery.
[recoveries_server-8] 1576919611.194813710: [osg_1.recoveries_server] [INFO]    spin running...
[recoveries_server-8] 1576919612.193241933: [osg_1.recoveries_server] [INFO]    spin running...
[recoveries_server-8] 1576919612.595722476: [osg_1.recoveries_server] [INFO]    spin completed successfully
[recoveries_server-8] 1576919612.598501340: [osg_1.recoveries_server] [INFO]    Attempting wait
[recoveries_server-8] 1576919613.598036424: [osg_1.recoveries_server] [INFO]    wait running...



/osg_1/planner_server:
  ros__parameters:
    allow_unknown: True
    planner_plugin_ids: ["GridBased"]
    planner_plugin_types: ["nav2_navfn_planner/NavfnPlanner"]
    tolerance: 0
    use_astar: True
    use_sim_time: True

/osg_1/controller_server:
  ros__parameters:
    BaseObstacle.class: "BaseObstacle"
    BaseObstacle.scale: 0.02
    BaseObstacle.sum_scores: False
    GoalAlign.aggregation_type: "last"
    GoalAlign.class: "GoalAlign"
    GoalAlign.forward_point_distance: 0.325
    GoalAlign.scale: 0
    GoalDist.aggregation_type: "last"
    GoalDist.class: "GoalDist"
    GoalDist.scale: 16
    Oscillation.class: "Oscillation"
    Oscillation.oscillation_reset_angle: 0.2
    Oscillation.oscillation_reset_dist: 0.05
    Oscillation.oscillation_reset_time: -1
    Oscillation.scale: 1
    Oscillation.x_only_threshold: 0.05
    PathAlign.aggregation_type: "last"
    PathAlign.class: "PathAlign"
    PathAlign.forward_point_distance: 0.325
    PathAlign.scale: 12
    PathDist.aggregation_type: "last"
    PathDist.class: "PathDist"
    PathDist.scale: 8
    RotateToGoal.class: "RotateToGoal"
    RotateToGoal.scale: 4
    RotateToGoal.xy_goal_tolerance: 0.25
    acc_lim_theta: 2
    acc_lim_x: 2
    acc_lim_y: 0
    controller_frequency: 20
    controller_plugin_ids: ["FollowPath"]
    controller_plugin_types: ["dwb_core::DWBLocalPlanner"]
    critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
    debug_trajectory_details: True
    decel_lim_theta: -2
    decel_lim_x: -2
    decel_lim_y: 0
    discretize_by_time: False
    goal_checker_name: "dwb_plugins::SimpleGoalChecker"
    max_speed_xy: 1.3
    max_vel_theta: 1
    max_vel_x: 1.3
    max_vel_y: 0
    min_speed_theta: 0
    min_speed_xy: 0
    min_theta_velocity_threshold: 0.001
    min_vel_x: 0
    min_vel_y: 0
    min_x_velocity_threshold: 0.001
    min_y_velocity_threshold: 0.5
    prune_distance: 1
    prune_plan: True
    publish_cost_grid_pc: True
    publish_evaluation: True
    publish_global_plan: True
    publish_local_plan: True
    publish_trajectories: True
    publish_transformed_plan: True
    sim_time: 2.2
    trajectory_generator_name: "dwb_plugins::StandardTrajectoryGenerator"
    transform_tolerance: 0.2
    use_sim_time: True
    vx_samples: 25
    vy_samples: 0
    xy_goal_tolerance: 0.1
    yaw_goal_tolerance: 0.25


/osg_1/local_costmap/local_costmap:
  ros__parameters:
    3d_obstacle_layer.combination_method: 1
    3d_obstacle_layer.decay_model: 0
    3d_obstacle_layer.enabled: True
    3d_obstacle_layer.map_save_duration: 60
    3d_obstacle_layer.mapping_mode: False
    3d_obstacle_layer.mark_threshold: 0
    3d_obstacle_layer.observation_sources: "rgbd1_clear rgbd1_mark"
    3d_obstacle_layer.publish_voxel_map: True
    3d_obstacle_layer.rgbd1_clear.clear_after_reading: False
    3d_obstacle_layer.rgbd1_clear.clearing: True
    3d_obstacle_layer.rgbd1_clear.data_type: "PointCloud2"
    3d_obstacle_layer.rgbd1_clear.decay_acceleration: 1
    3d_obstacle_layer.rgbd1_clear.enabled: True
    3d_obstacle_layer.rgbd1_clear.expected_update_rate: 0
    3d_obstacle_layer.rgbd1_clear.horizontal_fov_angle: 1.04
    3d_obstacle_layer.rgbd1_clear.inf_is_valid: False
    3d_obstacle_layer.rgbd1_clear.marking: False
    3d_obstacle_layer.rgbd1_clear.max_obstacle_height: 3
    3d_obstacle_layer.rgbd1_clear.max_z: 10
    3d_obstacle_layer.rgbd1_clear.min_obstacle_height: 0
    3d_obstacle_layer.rgbd1_clear.min_z: 0.1
    3d_obstacle_layer.rgbd1_clear.model_type: 0
    3d_obstacle_layer.rgbd1_clear.observation_persistence: 0
    3d_obstacle_layer.rgbd1_clear.obstacle_range: 2.5
    3d_obstacle_layer.rgbd1_clear.sensor_frame: ""
    3d_obstacle_layer.rgbd1_clear.topic: "/osg_1/custom_camera/custom_points"
    3d_obstacle_layer.rgbd1_clear.vertical_fov_angle: 0.7
    3d_obstacle_layer.rgbd1_clear.vertical_fov_padding: 0
    3d_obstacle_layer.rgbd1_clear.voxel_filter: False
    3d_obstacle_layer.rgbd1_mark.clear_after_reading: True
    3d_obstacle_layer.rgbd1_mark.clearing: False
    3d_obstacle_layer.rgbd1_mark.data_type: "PointCloud2"
    3d_obstacle_layer.rgbd1_mark.decay_acceleration: 0
    3d_obstacle_layer.rgbd1_mark.enabled: True
    3d_obstacle_layer.rgbd1_mark.expected_update_rate: 0
    3d_obstacle_layer.rgbd1_mark.horizontal_fov_angle: 1.04
    3d_obstacle_layer.rgbd1_mark.inf_is_valid: False
    3d_obstacle_layer.rgbd1_mark.marking: True
    3d_obstacle_layer.rgbd1_mark.max_obstacle_height: 5
    3d_obstacle_layer.rgbd1_mark.max_z: 10
    3d_obstacle_layer.rgbd1_mark.min_obstacle_height: 0.2
    3d_obstacle_layer.rgbd1_mark.min_z: 0
    3d_obstacle_layer.rgbd1_mark.model_type: 0
    3d_obstacle_layer.rgbd1_mark.observation_persistence: 0
    3d_obstacle_layer.rgbd1_mark.obstacle_range: 2.5
    3d_obstacle_layer.rgbd1_mark.sensor_frame: ""
    3d_obstacle_layer.rgbd1_mark.topic: "/osg_1/custom_camera/custom_points"
    3d_obstacle_layer.rgbd1_mark.vertical_fov_angle: 0.7
    3d_obstacle_layer.rgbd1_mark.vertical_fov_padding: 0
    3d_obstacle_layer.rgbd1_mark.voxel_filter: True
    3d_obstacle_layer.track_unknown_space: True
    3d_obstacle_layer.transform_tolerance: 0.2
    3d_obstacle_layer.update_footprint_enabled: True
    3d_obstacle_layer.voxel_decay: 2
    3d_obstacle_layer.voxel_size: 0.05
    always_send_full_costmap: True
    clearable_layers: ["obstacle_layer"]
    footprint: "[]"
    footprint_padding: 0.01
    global_frame: "odom"
    height: 10
    inflation_layer.cost_scaling_factor: 3
    inflation_layer.enabled: True
    inflation_layer.inflate_unknown: False
    inflation_layer.inflation_radius: 0.55
    lethal_cost_threshold: 100
    map_topic: "/osg_1/map"
    observation_sources: ""
    obstacle_layer.combination_method: 1
    obstacle_layer.enabled: True
    obstacle_layer.footprint_clearing_enabled: True
    obstacle_layer.max_obstacle_height: 2
    obstacle_layer.observation_sources: "scan"
    obstacle_layer.scan.clearing: True
    obstacle_layer.scan.data_type: "LaserScan"
    obstacle_layer.scan.expected_update_rate: 0
    obstacle_layer.scan.inf_is_valid: False
    obstacle_layer.scan.marking: True
    obstacle_layer.scan.max_obstacle_height: 2
    obstacle_layer.scan.min_obstacle_height: 0
    obstacle_layer.scan.observation_persistence: 0
    obstacle_layer.scan.obstacle_range: 20
    obstacle_layer.scan.raytrace_range: 21
    obstacle_layer.scan.sensor_frame: ""
    obstacle_layer.scan.topic: "/osg_1/scan"
    origin_x: 0
    origin_y: 0
    plugin_names: ["obstacle_layer", "3d_obstacle_layer", "inflation_layer"]
    plugin_types: ["nav2_costmap_2d::ObstacleLayer", "spatio_temporal_voxel_layer/SpatioTemporalVoxelLayer", "nav2_costmap_2d::InflationLayer"]
    publish_frequency: 1
    resolution: 0.05
    robot_base_frame: "base_link"
    robot_radius: 0.8
    rolling_window: True
    track_unknown_space: False
    transform_tolerance: 0.3
    trinary_costmap: True
    unknown_cost_value: 255
    update_frequency: 5
    use_maximum: False
    use_sim_time: True
    width: 10

Originally posted by madmax on ROS Answers with karma: 496 on 2019-12-20

Post score: 0

Improve this question
$\endgroup$

1 Answer 1

Reset to default
1
$\begingroup$

Rosanswers logo

This looks like a classic robot controller tuning issue, though I wouldn't discount an actual issue yet.

Have you tried this in simulation with the same robot / settings to reproduce? I'd be interested if this behavior changed if you modified the weights or DWB critics. If it gets tangibly worse or better, then that's on the right track.

Have you tried using the default parameters and seeing if it works with that? Then slowly changing all your parameters and figure out what this is a function of.

What's the cause of recovery behaviors? You didn't include a log snippet.

Originally posted by stevemacenski with karma: 8272 on 2019-12-20

This answer was ACCEPTED on the original site

Post score: 2

Original comments

Comment by madmax on 2019-12-21:
Added logs, but doesn't say much to me.
In real world and simulation it was the same behavior.

I took default nav2 params as you suggested and it worked!
Then I went through every change step by step and it took me to the xy_goal_tolerance param. Setting this one to 0.2 instead of 0.25, the controller can't reach his goal.
Debugging this I find a bit hard as I didn't find any rviz helpers or helpful log output.

Comment by stevemacenski on 2019-12-21:
Failed to make progress tells me alot, that means there may be an issue in your tuning of the controller or the progress tracker for your robot is too sensative.

If you say something trivial like 0.5 cm difference in position radius, that also supports a tuning issue to work through

Comment by madmax on 2019-12-22:
Thanks, for the help! Will look into this.

Improve this answer
$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.