@keithW Are you able to display the GoPiGo3 distance sensor topic in Rviz?
I set up a Range display on the /distance_sensor/distance topic, which is published once per second, and set the policy to keep all or keep last - no diff, I don’t see the distance cone.
I do see that rviz is ignoring the message though:
8258] [rviz2]: Message Filter dropping message: frame ‘distance’ at time 1628519123.414
This is what my distance topic looks like:
Have you been able to visualize the distance sensor readings?
Maybe it has to do with having the distance sensor connected to the bot via a servo joint that isn’t getting published or something. Have to look into this deeper.
Looked deeper - no servo1 joint in urdf so probably cannot map distance to base_link to world.
I have not set this up. Did you write it yourself? It’s not launched as part of the GoPiGo3 driver, at least for me.
If I understand the messages are being published, you just can’t visualize in RVIZ? Two things come to mind - one is exactly what you said - lack of a transform to the base_link. The other is to make sure you’re picking the right display type in RVIZ. There is a “Range” type - it expects a Range message.
I migrated the Chapter 6 distance_sensor.py to ROS2, and
- changed the frame_id to “distance_sensor” to match
- the distance_sensor link in my dave.urdf
- and launch it in my ./runit.sh:
ros2 run ros2_gopigo3_node gopigo3_node --ros-args -p S1LPW:=2094 -p S1RPW:=750 -p S1SECTOR:=2.443 &
ros2 run ros2_gopigo3_node distance_sensor &
# Don't know how to kill ydlidar launch by name, so don't run in background - use cntl-c
ros2 launch ydlidar_ros2_driver ydlidar_launch.py
echo "Killing gopigo3_node"
echo "Killing distance_sensor"
echo "Don't know how to kill ydlidar_ros2_driver_node, remember to use cntl-c"
# Use when these are set up as lifecycle nodes
# ros2 lifecycle set gopigo3_node shutdown
# ros2 lifecycle set ydlidar_ros2_driver_node shutdown
- added range display, set topic to /distance_sensor/distance
The distance_sensor.py node publishes the correct format sensor_msgs/range. I did add FOV though:
self.msg_range = Range()
# setting frame_id to urdf link element "distance_sensor"
self.msg_range.header.frame_id = "distance_sensor"
self.msg_range.radiation_type = Range.INFRARED # LASER is closer to INFRARED than ULTRASOUND
self.msg_range.min_range = 0.0200 # 2 cm / 20 mm
self.msg_range.max_range = 3.000 # 3 m / 300 cm / 3000 mm
# self.msg_field_of_view = from_degrees(25.0) # +/- 12.5 degree FOV (~60cm at max range)
self.msg_range.field_of_view = math.radians(25.0) # +/- 12.5 degree FOV (~60cm at max range)
self.pub = self.create_publisher(Range, '~/distance', qos_profile=10)
And now have the laser_frame at the right level relative to the distance sensor by adding the laser_frame and a fixed joint to the urdf.
I had forgotten about that. I’ll have to add it to my launch files
Glad that worked out.