1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
#!/usr/bin/python3
"""Change Lidar message into point cloud object"""
import rclpy
import math
from rclpy.node import Node
from sensor_msgs.msg import LaserScan, PointCloud2
from sensor_msgs_py import point_cloud2
from std_msgs.msg import Header
# Create node
rosNode= None
def scan_callback(scanMsg):
"""receive a lidar message and send a pointcloud message in topic"""
global rosNode
obstacles = ranges_to_positions(scanMsg)
# Sample for the tuto :
sample = [ [ round(p[0], 2), round(p[1], 2), round(p[2], 2) ] for p in obstacles[10:20] ]
# rosNode.get_logger().info( f"\nheader:\n{scanMsg.header}\nnumber of ranges: {len(scanMsg.ranges)}\nSample: {sample}\n" )
# Publish a msg
pointCloud = point_cloud2.create_cloud_xyz32(Header(frame_id=scanMsg.header.frame_id), obstacles)
point_publisher.publish(pointCloud)
def ranges_to_positions(scan_message: LaserScan) -> list:
"""change a lidar message into a pointcloud message"""
obstacles= []
angle= scan_message.angle_min
for aDistance in scan_message.ranges :
if 0.1 < aDistance and aDistance < 5.0 :
aPoint= [
math.cos(angle) * aDistance,
math.sin(angle) * aDistance,
0
]
obstacles.append(aPoint)
angle+= scan_message.angle_increment
return obstacles
rclpy.init()
rosNode= Node('scan_interpreter')
# Initialize a subscriber:
rosNode.create_subscription( LaserScan, 'scan', scan_callback, 10)
# Initialize a publisher:
point_publisher = rosNode.create_publisher(PointCloud2, '/scan_points', 10)
while True :
rclpy.spin_once( rosNode )
rosNode.destroy_node()
rclpy.shutdown()