Commit 3fdca3c9 authored by Inès EL HADRI's avatar Inès EL HADRI 💤

Update cloud point converter

parent 0479b0f5
......@@ -3,7 +3,7 @@ import rclpy
import math
from rclpy.node import Node
from sensor_msgs.msg import LaserScan, PointCloud2
import sensor_msgs_py.point_cloud2
from sensor_msgs_py import point_cloud2
from std_msgs.msg import Header
rosNode= None
......@@ -13,10 +13,10 @@ def scan_callback(scanMsg):
obstacles = ranges_to_positions(scanMsg)
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" )
# rosNode.get_logger().info( f"\nheader:\n{scanMsg.header}\nnumber of ranges: {len(scanMsg.ranges)}\nSample: {sample}\n" )
# Publish a msg
pointCloud = sensor_msgs_py.point_cloud2.create_cloud_xyz32(Header(frame_id=scanMsg.header.frame_id), obstacles)
pointCloud = point_cloud2.create_cloud_xyz32(Header(frame_id=scanMsg.header.frame_id), obstacles)
point_publisher.publish(pointCloud)
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment