ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
1 | initial version |
here is a sample code to publish a pointcloud2 with python in ros:
#!/usr/bin/env python
import rospy
import math
import sys
from sensor_msgs.msg import PointCloud2
import std_msgs.msg
import sensor_msgs.point_cloud2 as pcl2
if __name__ == '__main__':
'''
Sample code to publish a pcl2 with python
'''
rospy.init_node('pcl2_pub_example')
pcl_pub = rospy.Publisher("/my_pcl_topic", PointCloud2)
rospy.loginfo("Initializing sample pcl2 publisher node...")
#give time to roscore to make the connections
rospy.sleep(1.)
cloud_points = [[1.0, 1.0, 0.0],[1.0, 2.0, 0.0]]
#header
header = std_msgs.msg.Header()
header.stamp = rospy.Time.now()
header.frame_id = 'map'
#create pcl from points
scaled_polygon_pcl = pcl2.create_cloud_xyz32(header, cloud_points)
#publish
rospy.loginfo("happily publishing sample pointcloud.. !")
pcl_pub.publish(scaled_polygon_pcl)
open rivz
set fixed frame as map
run a roscore
add PointCloud2 topic in rviz and select the topic to listen to /my_pcl_topic
Done! you should be able to see two points on 1, 1 and 1, 2