Hey all,
So, last time I worked with tuning the cost scoring parameters of base_local_planner it was tricky due to no way to easily tell how each term in the cost function was influencing the scored trajectories. To address this problem, I've created a class that outputs a PointCloud2 on a topic called cost_cloud under the base_local_planner namespace that contains the cost information for each component of the cost function as well as the total cost for each point the MapGrid maintained by the TrajectoryPlanner class. You can find the code here:
https://github.com/ericperko/ros_navigation . You can find some sample images from rviz at
http://www.dropbox.com/gallery/1970299/1/BaseLocalPlannerVizSamples?h=885d40 , files named by which cost value they are visualizing (hover over the thumbnails to see the filenames or click for the gallery view and it will be listed below the thumbnails at the top), to give you all an idea of what this code outputs (red is lowest cost).
There are a few steps to actually being able to use this functionality:
- Checkout the updated Nav Stack from https://github.com/ericperko/ros_navigation or otherwise apply commit https://github.com/ericperko/ros_navigation/commit/00ec9847100942767a80524b401fc8c26e652d02 to your existing base_local_planner package & recompile it.
- Update your base_local_planner_params.yaml file with two more parameters:
- "publish_cost_grid_pc" to true. Default is false so that people who aren't interested in seeing the point cloud aren't wasting cycles making & publishing it.
- "global_frame_id" to whatever your local costmap's global frame is. By default, this is "odom" to match the setup in the nav stack setup (http://www.ros.org/wiki/navigation/Tutorials/RobotSetup ). This is the frame id set for the output point cloud so will determine how it draws in the map.
- Now you should be able to verify that base_local_planner is outputting a PointCloud using something like "rostopic hz /move_base/TrajectoryPlannerROS/cost_cloud"
-
In order to visualize the PointCloud in a meaningful way, you will have to apply the patch in https://code.ros.org/trac/ros-pkg/ticket/4610 to RViz so that you can treat arbitrary channels as intensity data. The visualization will be much more readable if you also make sure to check the "Use full RGB spectrum" box so that gradients are more apparent.
- Have fun selecting different components of the cost function and seeing what sort of affect they have on the potential field that base_local_planner is sampling trajectories from! Note that we don't output any points that have any cost component greater than max cost and would therefore be illegal for the planner to try to execute.
Feedback would be very welcome and I hope this comes in handy. I know it's already helped me out by letting me see that my obstacle cost field doesn't extend as far as I might like.
Eitan, any feedback on changes that are required to make this more general for use in, for example, dwa_local_planner would be great. Also, the C++ API of TrajectoryPlanner would likely have to change slightly if we wanted to move the visualizer and it's use of ROS topics and parameters up to TrajectoryPlannerROS. Specifically, the MapGrid would either need to become public or we'd need to add a getter for it; currently it's private to the TrajectorPlanner, so that's where the MapGridVisualizer is instantiated. Either that or pass in a number of variables in the constructor/setter methods (especially the scaling values). Let me know what you think would be the best idea for improving it, as I was currently going for a minimally invasive patch to start with and would like to get a more finalized patch together before opening a ticket to add this to the nav_stack.
- Eric