Range sensor

Range sensors are robot sensor permitting to detect obstacles and determine a distance to it. Basic range sensors (infrared, ultrasonic, laser) produce a unique measure considering a given direction at a time. By making the sensor rotating, it is possible to get measurements on a plan around the sensor.

Hokuhyo, equipping the Tbot, is typically a kind of rotating lidar sensor (laser imaging or light detection and ranging). The goal here is to integrate an almost 360 obstacle detection to generate safe robot movement.

More complex lidar permits 3D measurements (i.e. in several plans at a time).

Read Scan Data

The pibots already run urg_node_driver ros node. This node publish laser scan into a \scan topic. It is possible to check it with ros2 topic list and ros2 topic echo scan.

Gazebo simulator also simulate laser scan, but the scan topic and/or the laser-scan frame can have different names.

ros2 launch tbot_sim challenge-1.launch.py

Now you can visualize it on rviz2 program. Start rviz2, add a flux laserScan and configure it in /scan topic. Nothing appears and it is normal. Rviz2 global option is configured on map frame, and nothing permits to set the position of the laser sensor in the map. The laser-scan frame is named laser. Change this information into global options and set the laser-scan size to 0,1 for a better display.

A first node logging the scan

First, we will initialize a node scan_echo to print the scan data into a terminal.

Edit a new script file scan_echo en configure your package to recognize it as a ROS2 node.

Test your scan_echo node:

ros2 run tutorial_pkg scan_echo

#!python3
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import LaserScan

rosNode= None

def scan_callback(scanMsg):
    global rosNode
    rosNode.get_logger().info( f"scan:\n{scanMsg}" )

rclpy.init()
rosNode= Node('scan_interpreter')
rosNode.create_subscription( LaserScan, 'scan', scan_callback, 10)

rclpy.spin( rosNode )

scanInterpret.destroy_node()
rclpy.shutdown()

Test your scan_echo node, with the pibot and on simulations.

You can notice that ros_logger print information into info channel (informative), but other channel exits for errors and warnings.

At this point your echo node print the entire scan message, but we do not care about the distance measurements, we want only some meta-data. Modify the logger to print the information into the header and the number of ranges.

From LaserScan to PointCloud

LaserScan provides both the recorded bean distances (ranges) and the meta information permitting converting the distances on points in a regular Cartesian frame (i.e. the angle between beans).

In a python, the conversion would look like this:

obstacles= []
angle= scanMsg.angle_min
for aDistance in scanMsg.ranges :
    if 0.1 < aDistance and aDistance < 5.0 :
        aPoint= [
            math.cos(angle) * aDistance,
            math.sin(angle) * aDistance
        ]
        obstacles.append(aPoint)
    angle+= scanMsg.angle_increment

The exercise consists in modifying the scan callback function to generate the point cloud list. To log a sample of the point cloud:

sample= [ [ round(p[0], 2), round(p[1], 2) ] for p in  obstacles[10:20] ]
self.get_logger().info( f" obs({len(obstacles)}) ...{sample}..." )

Finally, it is possible to publish this result in a PointCloud message and to visualize it on rviz2 in a superposition of the LaserScan.

PointCloud is based on geometry_msgs.Point32 with float coordinate. The creation of Point32 will require explicite cast.

aPoint= Point32()
aPoint.x= (float)(math.cos(angle) * aDistance)
aPoint.y= (float)(math.sin( angle ) * aDistance)
aPoint.z= (float)(0)
  • PointCloud mesage as part of the sensor_msgs pkg (yes we are using the deprecated one, and we should not)