Visualize all calculated trajectories in RViz

Question

Hi,

before a local planner chooses a best trajectory, it calculates a lot and compares them. Is there any chance to visualize all of the generated trajectories in RViz to see what's happening and how different parameters influence the calculation?

I was thinking about modifying the local planner and add a publisher which advertises all calculated trajectories, but I don't know if that would be a good way. Any ideas?

---

Originally posted by alex_rockt on ROS Answers with karma: 76 on 2013-06-11

Post score: 2

Answer 1

I have a Nearness Diagram node, which is gap/valley based so as with your description it calculates several possible 'paths' before selecting the best one to attempt to follow. I use RViz to visualize these by creating line strip markers, there are several different types of markers that you can use, though you may need to perform a few extra calculations in order to get the values you want for displaying in RViz. By doing this I can see all of the possible 'valleys' found at any given time. With even more work you could even go as far as to generate a nav_msgs/path which RViz will display but if you're just trying to do simple testing/debugging I wouldn't recommend this as it will be a lot of extra effort. A simple line strip marker works great for me, and if I don't want to see excessive markers while testing I simply uncheck the display in RViz.

---

Originally posted by jgilsenan with karma: 181 on 2013-09-16

This answer was ACCEPTED on the original site

Post score: 0

Answer 2

add this parameters to your dwa config file:

publish_traj_pc: true              # default: false
global_frame_id: map               # default: odom

Then in rviz add a pointcloud2 topic, make it listen to:

/move_base/DWAPlannerROS/trajectory_cloud

You should be able to see all dwa trajectory endpoints

It look like this: dwa_trajetories.png

credit: http://answers.ros.org/question/192143/dwa_planner-trajectory_cloud-topic-not-showing-up/

---

Originally posted by Oscar Lima with karma: 831 on 2016-07-27

This answer was NOT ACCEPTED on the original site

Post score: 1