How to fill up a PointCloud Message with data in Python?

asked 2015-04-13 06:54:43 -0500

Holzroller
62 ●5 ●6 ●13

updated 2015-04-16 01:54:47 -0500

Hey guys,

I want to set up a PointCloud Message in my python main. I already read the docs: http://docs.ros.org/api/sensor_msgs/h... But I don't catch it. This is what I got so far (spread over the different classes):

import rospy
from geometry_msgs.msg import Point
from sensor_msgs.msg import PointCloud

self.stelldaten = self.rospy.Publisher('Stelldaten', PointCloud, queue_size=10)

rospy.init_node('LaserCutter GUI', anonymous=True)

    try:
        rospy.spin()
    except KeyboardInterrupt:
        print "Shutting Down"

I don't understand how to fill up the PointCloud with data. I googled quite a bit, but the most, if not all, examples are for C++. Can somebody enlighten me, please?

edit according to marguedas comment/answer:

I think i got the basic idea, the problem is that the program shuts down without any error which makes debugging quite hard. First of all, let me answer your questions. The points are generated in a Kivy based gui, so I basically got three arrays (one for x, one for y and one for the time - so I am using the z coordinate to transmit information about the time). The information i collect in the x and y vectors are speed values for a stepper motor. To conlude: for each point I have two speed values and one time value. I tryed to use the PointCloud, cause I don't like the way to publish matrices in ROS (at least so far...). The following shows the code i got so far (only the parts important for this specific problem):

import rospy
import std_msgs.msg
from geometry_msgs.msg import Point
from sensor_msgs.msg import PointCloud

class Lasercutter(TabbedPanel):
        def __init__(self, **kwargs):
            super(Lasercutter, self).__init__(**kwargs)

            self.stelldaten = rospy.Publisher('Stelldaten', PointCloud, queue_size=10)
            self.stelldaten_tabelle = PointCloud()
            self.h = std_msgs.msg.Header()
            self.h.stamp = rospy.Time.now()

    def bahnberechnung(self):
        anzahl_punkte = 10 # i got 10 points, for example
        self.stelldaten_tabelle.header = self.h
        self.stelldaten_tabelle.points = anzahl_punkte
        self.stelldaten_tabelle.channels = 3
        point = Point()

        for i in range(0, anzahl_punkte):

            point.x = self.q1_v[i]
            point.y = self.q2_v[i]
            point.z = self.tges[i]
            self.stelldaten_tabelle.points[i] = point.x
            self.stelldaten_tabelle.points[i] = point.y
            self.stelldaten_tabelle.points[i] = point.z

        self.stelldaten.publish(self.stelldaten_tabelle)

class TabbedPanelApp(App):

    def build(self):
        # diese Node arbeitet unter folgendem Namen
        rospy.init_node('LaserCutterGUI', anonymous=True)
        return Lasercutter()

if __name__ == '__main__':
    TabbedPanelApp().run()

As I mentioned i get no errors, the program just gets killed (sometimes you can blame it on Kivy, but that doesnt help me to find the problem, right? :D). I get prints until the line : point = Point()

Thanks again for any advice, I really appreciate it!

edit retag flag offensive close merge delete

add a comment

2 Answers

Sort by » oldest newest most voted

answered 2015-04-13 12:07:23 -0500

marguedas

3606 ●17 ●109 ●57

updated 2015-04-17 22:03:39 -0500

It's more a comment but I publish it as an answer because of the lack of space.
Where does your pointcloud come from? a callback ? What information do you have ? only range or also intensity ?

Steps:
1) create a PointCloud
2) fill header: http://answers.ros.org/question/60209... + set a frame_id (link of your depth sensor)
3) resize pointcloud according to your number of points
4) fill pointcloud
for i in range(numberOfPoints): pointcloud.points[i].x = inputPoints[i].x pointcloud.points[i].y = inputPoints[i].y pointcloud.points[i].z = inputPoints[i].z 5) publish the pointcloud

EDIT:

Your current error is due to the fact that you need to resize the channel data structure instead of assigning it a scalar value.
I've never tried to use pointcloud for this kind of purpose. I'm not sure that is the best structure for your application. To my understanding you just need a way to convey tuples of size3. Is that right?

If I was you I would not be using a PointCloud structure but rather something like a Vector3 Array. You create a custom message which is an array of Vector3
You can check here for custom message generation
you do the following

    self.vectordaten_tabelle = Vector3Array()
   anzahl_punkte = 10 # Assming that you know the size will be 10, otherwise you can check the size of your input arrays
   vec3 = Vector3()

    for i in range(0, anzahl_punkte):
        vec3.x = self.q1_v[i]
        vec3.y = self.q2_v[i]
        vec3.z = self.tges[i]
        self.vectordaten_tabelle.append(vec3)

This way you will endup with a vector array filled with your data without bothering with all the additional fields that would be useless in PoinCloud in your case.

Hope this suits your application,

edit flag offensive delete link

Comments

i edited my question accordingly to your answer, can you pls take another look? Thanks :)

Holzroller ( 2015-04-16 00:34:05 -0500 )edit

thats right! Ill give it a shot on monday, but your way seems much more appropriate than mine. Thanks so much for your help :)

Holzroller ( 2015-04-18 10:20:11 -0500 )edit

Just one more question! I made it pretty far with your excellent explanation! self.vectordaten_tabelle.append(vec3) in that line the programm crushes. My custom message looks like: geometry_msgs/Vector3[] Vector3DArray. Everything else is equivalent to your code above. Any ideas?

Holzroller ( 2015-04-20 06:09:31 -0500 )edit

my bad, you have to specify the structure you want to add your element to, if your message is geometry_msgs/Vector3[] Vector3DArray
you need to add element to the Vector3DArray array, so:
self.vectordaten_tabelle.Vector3DArray.append(vec3)

marguedas ( 2015-04-22 09:39:36 -0500 )edit

thanks again! you saved me a ton of research! the array gets appended now, but every entry gets overridden with the newest vec3. So my message is an array of vector3() (with anzahl_punkte entrys), but every entry is e.g. x = 0, y = 0, z = 0.5, cause those are the values of the last vec3() i appended

Holzroller ( 2015-04-24 09:21:53 -0500 )edit

My bad, I haven't tested any of this. You need to create vec3 inside the loop.

marguedas ( 2015-04-28 22:42:48 -0500 )edit

add a comment

answered 2015-10-10 19:14:45 -0500

Oscar Lima

831 ●6 ●30 ●16 https://www.dfki.de/en...

Here is an example on how to publish a sensor_msgs/PointCloud in ros by using python:

#!/usr/bin/env python
import rospy

from sensor_msgs.msg import PointCloud
from geometry_msgs.msg import Point32
import std_msgs.msg

if __name__ == '__main__':
    '''
    Publishes example pointcloud
    '''
    rospy.init_node('point_polygon_scaler')
    pointcloud_publisher = rospy.Publisher("/my_pointcloud_topic", PointCloud)
    rospy.loginfo("pcl_publish_example")
    #giving some time for the publisher to register
    rospy.sleep(0.5)
    #declaring pointcloud
    my_awesome_pointcloud = PointCloud()
    #filling pointcloud header
    header = std_msgs.msg.Header()
    header.stamp = rospy.Time.now()
    header.frame_id = 'map'
    my_awesome_pointcloud.header = header
    #filling some points
    my_awesome_pointcloud.points.append(Point32(1.0, 1.0, 0.0))
    my_awesome_pointcloud.points.append(Point32(2.0, 2.0, 0.0))
    my_awesome_pointcloud.points.append(Point32(3.0, 3.0, 0.0))
    #publish
    print "happily publishing pointcloud"
    pointcloud_publisher.publish(my_awesome_pointcloud)
    #exit. we are done!
    print "bye bye..."

If you want to use sensor_msgs/Pointcloud2 then:

#!/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)

enjoy !

edit flag offensive delete link

Comments

Your solution really help a lot for a beginner like me! Thank you so much! May I know which reference can i refer to to change pointcloud2 colour based on sensor value ? my application is to do something like heatmap. Do you think it is suitable to use pointcloud2 for this purpose ?

KinWah ( 2018-10-24 07:41:53 -0500 )edit

If its 2D I would go for a nav_msgs/OccupancyGrid type, if 3D then octomap (see octomap heat map video.

Oscar Lima ( 2018-10-24 17:55:42 -0500 )edit

add a comment

How to fill up a PointCloud Message with data in Python?

2 Answers

Comments

Comments

Question Tools

Stats

Related questions

How to fill up a PointCloud Message with data in Python? edit

2 Answers

Comments

Comments

Question Tools

Stats

Related questions

How to fill up a PointCloud Message with data in Python?