dwa planner too small velocities

Hi,

I am using the DWA planner with the ros navigation stack to perform autonomous driving with an omnidirectional legged/wheeled robot.

The problem is that the robot generates too small velocities and it remains almost still for a while, before rotating to the target or something like this, even if I defined the minveltrans to 0.1, the velocities sometimes are smaller as in this screenshot: https://ibb.co/L0wLt5g

These are the params considered:

DWAPlannerROS:

max_vel_x: 0.25
min_vel_x: -0.25

max_vel_y: 0.25
min_vel_y: -0.25

max_vel_theta: 0.2
min_vel_theta: -0.2


max_vel_trans: 0.35
min_vel_trans: 0.1

theta_stopped_vel: 0.1
trans_stopped_vel: 0.1


acc_lim_theta: 2
acc_lim_x: 2.5
acc_lim_y: 2.5

yaw_goal_tolerance: 0.1
xy_goal_tolerance: 0.1

sim_time: 3.5
vx_samples: 20
vy_samples: 20
vth_samples: 40

controller_frequency: 20.0

EDIT:

Move base in the launch file:

<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">

    <remap if="$(arg use_rtabmap)" from="map" to="rtabmap/proj_map"/>
    <remap if="$(arg use_octomap)" from="map" to="projected_map"/>

    <remap from="/cmd_vel" to="/centauro_plan/cmd_vel"/>
    <remap from="odom" to="T265/T265_camera/odom/sample"/>

    <rosparam file="$(find centauro_2dnav)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find centauro_2dnav)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find centauro_2dnav)/config/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find centauro_2dnav)/config/global_costmap_params.yaml" command=

Asked by aled96 on 2021-01-27 05:48:00 UTC

Comments

In the picture it seems like you are trying to back into the obstacles on the costmap, am I seeing this right? In rviz you can show the footprint, from costmap_common or similar .yaml. It seems there might be a collision. The terminal in the lower left corner is the cmd_vel topic and not odometry I asume?

Asked by Dragonslayer on 2021-01-27 07:12:57 UTC

Yes, the robot starts in the origin and moves backward to the target. Now it should just rotate to achieve its objective. But the cmd_vel generated (yes, the lower left terminal) is small and the robot remains still

Great for the footprint in Rviz, i did not know about it ! Thanks !

By the way, yes they may touch, but the footprint is allowed to enter a little bit in the object inflation radius radius ? Is not everything considered as lethal

Asked by aled96 on 2021-01-27 08:03:40 UTC

This is the updated version in which you can see also the footprint

https://ibb.co/TMcpJVQ

Because before using DWA a tried with the default local planner (trajectoryPlanner) and in that case the footprint enters in the inflation radius area. With the DWA this is forbidden ? Or maybe I have to set some parameters differently ?

Thanks

Asked by aled96 on 2021-01-27 08:19:16 UTC

Has dwa rached its goal? I dont know the scale, but base center might be within goal tolerance. Are you using a normal move_base node? Those small commands, at least in a real world robot, might just be position corrections for sensor noise, are they wildely fluctuating? Also there is some functionality that might slow your robot down as it approaches the obstacle --> parameters:

  scaling_speed: 0.45 #The absolute value of the velocity at which to start scaling the robot's footprint, in m/s 
  max_scaling_factor: 0.2 #The maximum factor to scale the robot's footprint by 
  occdist_sclae: 0.1 #The weighting for how much the controller should attempt to avoid obstacles

Some time back I can remember there was discussion about slowing down when near obstacles. Iam nomore sure wether the question was about how to achieve it or why it happens. Real robot? Maybe the values are bellow the threshold for the mc so no movement but move_base wants to still sneak up on the target.

Asked by Dragonslayer on 2021-01-27 08:41:05 UTC

No, it did not reach the target (otherwise I get notification in the terminal). Up to now I am working only in simulation by the way.

Yes, I think I am using a normal move_base node. I update the question with the move base part in the launch file.

Can you explain me better the params you mentioned ?

Asked by aled96 on 2021-01-27 08:52:58 UTC

The planner is complex, those parameters might be the issue or not, the comments are what I know about them. But what is your actual goal here, just get a little closer? move_base should just abort because it doesnt get closer? In the end move base will try to reach, then depending on setting, try a clearing rotation and try again, usually at this point if nothing changed it should abort. For how long does it stay in this state of minimal comands, do you abort manually?

Asked by Dragonslayer on 2021-01-27 09:28:18 UTC

My objective is to reach a wall, avoiding in obstacles during the path.

When the robot arrives near the target (and so near the obstacle), the generated velocities fall down and robot remains almost still for more than 2 minutes.. So I abort manually. No errors or other things are visualized in the terminal as errors/warnings from the navigation nodes.

Asked by aled96 on 2021-01-27 09:55:43 UTC

But, if we are near the target (as in https://ibb.co/TMcpJVQ) the robot moves really slow and it takes several minutes for a rotation of 3°.

If instead, as in https://ibb.co/8bjQk92, the robot is avoiding an obstacle, the footprint enters in the inflation area and the robot hits the obstacle...

Is not possible to reach the target close to an object and avoid obstacles during navigation ?

Asked by aled96 on 2021-01-27 10:23:18 UTC

So you try to achieve something similar to "docking" at a wall? The problem is that the wall is an obstacle. There is lots of talk about how to best switch planner/behaviour/or paramters at runtime/executiontime. Dynamic reconfigure might work (I dont know for sure). I had a similar problem and my approach is to switch controllers. Move_base takes me near the goal and then a completely other node takes over the last inches. Do you need to touch the wall? In your other pic it looks like the robot comes very close. Maybe try setting this parameter to false:

latch_xy_goal_tolerance: false #If goal tolerance is latched, if the robot ever reaches the goal xy location it will simply rotate in place, even if it ends up outside the goal tolerance while it is doing so.

To me it seems the robot is doing just that try to orient itself close to the goal. If you look at the rectangular shape of the footprint if the robot would be facing the wall, it would be very close.

Asked by Dragonslayer on 2021-01-27 12:42:38 UTC

Yeah, it would be nice to touch the wall if possible. While latch_xy_goal_tolerance is already to false.

Yes, I may think to consider a target a 10-15 cm more far from the wall, then, just drive for 10 cm with another controller, maybe assuming to have no obstacle near the robot, except for the target wall.

By the way, thank you very much for all your support !

Asked by aled96 on 2021-01-27 14:06:46 UTC

Your welcome. Yes, that might be the cleanest choice. They way the robot gets so close to obstacles is in any other case dangerous. Though. It might be the case that the goal is choosen as just the nearest free space to the goal. Maybe by "faking" a smaller footprint you could make it creep to the wall. To avoid other obstacles you would need to choose a silly high scaling_factor at relatively low scaling speed. But somehow you might get the appropriate comands. But it might take longer to figure those settings out, and it would just be a hack, not a clean solution.

Asked by Dragonslayer on 2021-01-27 15:17:28 UTC