navigation
navigation copied to clipboard
ros-planning
Oscillation cost function doesn't work with DWA local planner for rotations
AFAIU oscillation cost function should prevent rotational oscillations while rotating in place.
But in DWA local planner it's not so.
In place trajectory can be supplied by LatchedStopRotateController only, because
SimpleTrajectoryGenerator discards such trajectories here
https://github.com/ros-planning/navigation/blob/956c2c9e6fddce5484343cfc97dbc4e7217edcc2/base_local_planner/src/simple_trajectory_generator.cpp#L193
But before checking it by SimpleScoredSamplingPlanner in DWAPlanner::checkTrajectory,
oscillation cost function is reset here
https://github.com/ros-planning/navigation/blob/956c2c9e6fddce5484343cfc97dbc4e7217edcc2/dwa_local_planner/src/dwa_planner.cpp#L220
So, it's never actually used.
While forward/backward oscillations are still penalized ok.
Seems like removing the last line should help.
What you say is true. checkTrajectory is meant as a sort-of stateless check, but it does change the state of (and thus ignore) the oscillation cost function.
Are you finding scenarios where oscillations are a big problem while turning in place?
@DLu I doubt I had such scenarios. But, what is the reason to keep a feature that doesn't work? This only misleads people.
There are two separate use cases for checkTrajectory. If you are using it for checking if rotating to the goal is in collision, then that line is not needed. However, if you are using the checkTrajectory method in some external test class, you would not want it to return oscillation errors just because the sequence of individual trajectories fed to the method appeared to be oscillating. So I think the method makes some sense as is.
So, currently, rotation oscillation cost function doesn't work in any situation, and that line is needed only for external test classes use case. Then, maybe it's worth removing this functionality at all? BTW, why it's limited to only in-place oscillations, why not to prohibit rotational oscillations at speed? Thanks.
Can you elaborate on what you mean that the rotation oscillation doesn't work in any situation?
Rotation oscillation functionality in dwa_local_planner doesn't work neither when called from LatchedStopRotateController nor when called from SimpleTrajectoryGenerator. It's elaborated in initial issue description.
If you want to allow in place rotations even not near the goal, you can set min_trans_vel to -1.
When not using the LatchedStopRotateController the trajectories are scored here and not within DWAPlanner::checkTrajectory, and that calculation will include the oscillation costs.
It has been a while since this was mentioned and even though it might not be the exact behavior I notice, it appears that I have rotational oscillations when I give a goal to the robot that is opposite to its orientation, with plenty of space and no obstacles.
The robot generally navigates amazingly, and If I give any other goal that is a bit side ways to the robot the oscillations stop and the robot navigates fine.
I tried setting min_vel_trans: -1 without any success.
Hi @DatSpace , @pavloblindnology
I did some modifications to DWA local planner and Base local planner. My modified DWA local planner currently support to navigate the robot to the a goal that is opposite to its orientation. I have tested it in Gazebo and also on my real Robot.
I totally made 4 changes to both DWA local planner and Base local planner. The files I modified are: (1) dwa_local_planner/include/dwa_local_planner/dwa_planner_ros.h (2) dwa_local_planner/src/dwa_planner_ros.cpp (3) base_local_planner/include/base_local_planner/latched_stop_rotate_controller.h (4) base_local_planner/src/latched_stop_rotate_controller.cpp
You can refer to the changes of my fork of navigation stack:
https://github.com/HappySamuel/navigation
I have marked the changes i added/modified with //SAMUEL in those files.
Hope this can help you guys' usage.
Best, Samuel
Hi @Oklahoma1122
You have any questions that would like to ask?
Best, Samuel