Revision history [back]

First, I would suggest setting the use_sonar parameter using a ROS launch file rather than changing and recompiling the p2os_driver code, so that you can more easily modify the parameter setting in the future, or (perhaps more usefully) you can have multiple launch files for various application configurations, perhaps some that use the sonars and some that do not.

But in regards to the main question, there are many ways to do this. For example, you could convert the sonar readings to a point cloud. But that doesn't really capture what a sonar is doing. To get a nice visualization in rviz that more accurately represents the (large) sensitivity cone of each sonar, you could the sensor_msgs/Range datatype, which you can visualize in rviz.

I have not done this myself yet, but I believe you need to define the coordinate frame of each sonar, using something like static_transform_publisher to define static offsets relative the the frame of the robot. Then for each message you receive on the /sonar topic, you would send out a bunch of sensor_msg/Range messages, one for each sonar.

All that being said, I don't have access to a Pioneer anymore, so I haven't tried any of this myself. Perhaps one of the Pioneer users will have a better idea.