[Solved] ROS - Display GoPiGo3 Distance Sensor In Rviz

GoPiGo
1

@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:

---
header:
  stamp:
    sec: 1628520026
    nanosec: 484912371
  frame_id: distance
radiation_type: 1
field_of_view: 0.4363323152065277
min_range: 0.019999999552965164
max_range: 3.0
range: 0.2619999945163727
---

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.

1 Like
2

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.

/K

2 Likes
3

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:
#!/bin/bash

cd ~/rosbot-on-gopigo3/handsonros2
. ~/rosbot-on-gopigo3/handsonros2/install/setup.bash


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
  • my killit.sh:
#!/bin/bash

# ~/rosbot-on-gopigo3/handsonros2/install/setup.bash
echo "Killing gopigo3_node"
killall gopigo3_node
echo "Killing distance_sensor"
killall 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)

DistanceSensor
DistanceSensor1837×1049 151 KB

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.

Dave_rviz2_side_view
Dave_rviz2_side_view1837×1049 180 KB

2 Likes
4

I had forgotten about that. I’ll have to add it to my launch files

Glad that worked out.

/K

2 Likes