Hi,
I was having a lot of trouble tuning the DWA planner in ROS Kinetic for a pretty large robot and part of my problem was that I had no idea how the costs were affecting the trajectories that were being chosen. Having gone through the nav stack source code, I realise now that some things didn't quite work how I thought they did.
To make tuning easier for myself, I modified the DWA planner code to publish the trajectory costs for each set of trajectories. I:
- created a new message type to store all the costs for a single trajectory (overall costs and the sub-costs such as obstacle costs, goal costs etc.)
- created another message type to store an array of all the trajectories generated at a particular instance
- indicated which trajectory was the best in a the set
Taking this data, I was able to publish the costs of the best trajectory, as shown in the below example:
Best trajectory has index: 102 # index of this trajectory, in the trajectory array
Overall cost: 455.06
Oscillation costs: 0
Obstacle costs: 5.06
Goal front costs: 150
Alignment costs: 0
Path costs: 0
Goal costs: 300
Twirling costs: 8.23756e-320
---------------------------------------
---------------------------------------
I was able to use this to determine which set of costs were having the largest influence on the selected trajectory being chosen, and which probably needed a higher weighting. Is this a feature that might be useful to PR into the nav stack? I'm mainly just trying to get some feedback into whether other people would use this since I've seen a number of questions on ROS answers with tuning-related issues, such as the below:
Thanks in advance for any feedback!