Robotics StackExchange | Archived questions

DWB local planner final in-place rotation never happen RotateToGoal

I'm currently using movebaseflex with dwblocalplanner for navigation.

The robot drives smooth and nice, but never get final in-place rotation to the goal pose.
At the very end dwb planner /evaluation gives me:

RotateToGoal: score: -1

My robot is quite big, got nice motors and decent imu, the time between machines is synchronized.

My DWB planner parameters are the following: 

footprint: [[ 0.7000,  0.4500],
        [ 0.8000,  0.0000], # arrow
        [ 0.7000, -0.4500],
        [ 0.0000, -0.4500],
        [-0.7500, -0.4500],
        [-0.7500,  0.0000],
        [-0.7500,  0.4500],
        [ 0.0000,  0.4500]]
controllers:
  - name: 'BaseLocalPlanner'
    type: 'nav_core_adapter::LocalPlannerAdapter'

LocalPlannerAdapter:
  planner_name: dwb_local_planner::DWBLocalPlanner

DWBLocalPlanner:
  # Robot Configuration Parameters
  update_frequency: 5.0
  publish_frequency: 3.0

  max_vel_x: 0.3
  min_vel_x: 0.15

  max_vel_y: 0.0
  min_vel_y: 0.0
  max_vel_theta: 0.5

  acc_lim_x: 1.2
  acc_lim_y: 0
  acc_lim_theta: 1.4

  decel_lim_x: -1.2
  decel_lim_y: 0
  decel_lim_theta: -1.4

  max_speed_xy: 0.35
  min_speed_xy: 0.18
  min_speed_theta: 0.35

  # Goal checking
  goal_checker_name: dwb_plugins::SimpleGoalChecker



  critics: ["RotateToGoal", "Oscillation", "ObstacleFootprint", "PathAlign", "GoalAlign", "PathDist", "GoalDist"]

  RotateToGoal:  
    scale: 32.0  
    slowing_factor: 5.0  
    lookahead_time: -1.0  
  ObstacleFootprint:  
    scale: 0.01  
    max_scaling_factor: 0.2  
    scaling_speed: 0.25  
  PathDist:  
    scale: 32.0  
  GoalDist:  
    scale: 32.0  
  PathAlign:  
    scale: 16.0  
    forward_point_distance: 0.325  
  GoalAlign:  
    scale: 32.0  

  xy_goal_tolerance: 0.1  
  yaw_goal_tolerance: 0.05  

  trans_stopped_velocity: 0.1  
  rot_stopped_velocity: 0.05  

  # Forward Simulation Parameters  
  sim_time: 1.7   
  linear_granularity: 0.05  
  vx_samples: 3  
  vtheta_samples: 30

Has somebody faced this issue before?

"controller_frequency" param is set to 4.0
Also, I'm using velocity smoother, params are: 

<param name="speed_lim_v"  value="0.35"/>
   <param name="speed_lim_w"  value="0.7"/>
   <param name="accel_lim_v"  value="1.2"/>
   <param name="accel_lim_w"  value="3.2"/>
   <param name="decel_factor"  value="0.6"/>
   <param name="frequency"  value="100"/>
   <param name="robot_feedback"  value="0"/>

Asked by artemiialessandrini on 2020-06-10 05:28:51 UTC

Comments

Answers

Getting a -1 means that the trajectory got an exception

In the LocalPlanEvaluation message, you should have many different TrajectoryScore objects with different numerical scores, so hopefully one of them is not -1.

if they're ALL -1, it means that Rotate to Goal is either A) having trouble slowing down near the goal or B) unable to find a rotation command without some forward motion

Asked by David Lu on 2020-06-10 10:45:40 UTC

Comments

Thanks for answering, @David Lu
Unfortunately, none of the suggestions helped, maybe try more tweaking later.
A current workaround is to switch to TEB planner in a final parking stage

Asked by artemiialessandrini on 2020-06-29 06:50:28 UTC

Hello!I met the same problem. It is the problem of B)unable to find a rotation command without forward motion. You can change the EPSILON variable to a larger number to allow a slow forward motion.

Asked by danningzhao on 2023-03-14 10:54:07 UTC

Hello!I met the same problem. It is the problem of B)unable to find a rotation command without forward motion. You can change the EPSILON variable to a larger number to allow a slow forward motion.

OK...That is my old answer. In my case, the real reason why it cannot rotate to the goal is because of the lookahead time parameter and the scale of RotateToGoal. I have a lookahead_time=0.1, with scale=1000, then it works pretty good. For changing lookahead_time, its better to check the source code of RotateToGoal cost function definition. For scale, I choose it by observation and experiments. After robot reaches the position of goal, what we only want is that the robot can reach the goal attitude. What it need to do is only rotation: we only care about its theta velocity. However, sometimes other cost function value will be different. In order to let RotateToGoal cost function dominate the total cost, I choose scale1000 (100 sometimes doesn't work), or even larger.

Asked by danningzhao on 2023-06-05 13:59:35 UTC

Comments