Robotics StackExchange | Archived questions

displaying a point cloud in rviz

For experimentation's sake I am writing a node for a turtlebot that publishes data as if it were '/camera/depth/registered_points', a PointCloud2 message. I can not get rviz to show this data. I think I'm missing a basic understanding of frames. The rviz error is :

"Transform [sender=/turtlebottalktest191931389894558845] For frame [/map]: Fixed Frame [casterfront_link] does not exist"

Here is some of the code that creates the point cloud. I'm hoping in the end for some colored dots on the rviz screen. For brevity I have not included my 'import' section.

def make_cloud() :
    rospy.init_node('turtlebot_talktest', anonymous=True)
    #
    pub_cloud = rospy.Publisher("camera/depth_registered/points", PointCloud2)
    pcloud = PointCloud2()
    c_height = 6
    c_width = 6
    # make point cloud
    fields = [PointField('x',0, PointField.INT16, 1)]
    cloud2 =  [1,2,6,1,2,6,
        1,2,6,1,2,6,
        1,2,6,1,2,6,
        1,2,6,1,2,6,
        1,2,6,1,2,6,
        1,2,6,1,2,6] 
    cloud_struct = struct.Struct(pc2._get_struct_fmt(False, fields))
    buff = ctypes.create_string_buffer(cloud_struct.size * len(cloud2))
    point_step, pack_into = cloud_struct.size, cloud_struct.pack_into
    offset = 0
    for p in cloud2:
        pack_into(buff, offset, p)
        offset += point_step
    pcloud.header = Header()
    pcloud.header.stamp = rospy.Time.now()
    pcloud.header.frame_id = '/map'
    pcloud2 = PointCloud2(header=pcloud.header,
        height=c_height,
        width=c_width, 
        is_dense=False,
        is_bigendian=False,
        fields=fields,
        point_step=cloud_struct.size,
        row_step= len(cloud2) / c_height,
        data=buff.raw)
    pub_cloud.publish(pcloud2)
    return

I thought by changing the 'frame_id' to '/map' I was on the right track. Is there a simple change that I can make so that rviz will display these points?

Asked by david.c.liebman on 2014-01-16 07:07:24 UTC

Comments

Answers

You should add xyz as fields in the header. I have some MATLAB code for it but it should be straightforward to port it to Python, I hope you won't mind ;)

        f = pc.getFields;
        XField = rosmatlab.message('sensor_msgs/PointField',RTC.node);
        XField.setName('x');
        XField.setDatatype(7);
        XField.setCount(1);
        f.add(XField);
        YField = rosmatlab.message('sensor_msgs/PointField',RTC.node);
        YField.setName('y');
        YField.setDatatype(7);
        YField.setCount(1);
        YField.setOffset(4)
        f.add(YField);
        ZField = rosmatlab.message('sensor_msgs/PointField',RTC.node);
        ZField.setName('z');
        ZField.setDatatype(7);
        ZField.setCount(1);
        ZField.setOffset(8)
        f.add(ZField);

Cheers, Marco

Asked by marcoesposito1988 on 2014-02-12 10:09:21 UTC

Comments

It is totally true that I neglected X, Y, and Z. Just using one field value won't work. Thanks.

Asked by david.c.liebman on 2014-02-14 07:11:31 UTC

yes, obviously I don't understand a lot. BTW, which dimension is up/down, which dimension is left/right, and which is depth. In my 'one field' version I was hoping that there was only depth and that left/right and up/down were handled by the dimensions of the array.

Asked by david.c.liebman on 2014-02-14 08:53:39 UTC

You may have to setup Header properly with proper time stamps. Try to publish the pointcloud continuously with updated time stamps in header.

Also, I would try rostopic echo first to validate you have generated correct message format.

Asked by jihoonl on 2014-02-12 15:09:40 UTC

Comments

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)

  1. open rivz

  2. set fixed frame as map

  3. run a roscore

  4. add PointCloud2 topic in rviz and select the topic to listen to /my_pcl_topic

  5. Done! you should be able to see two points on 1, 1 and 1, 2

Asked by Oscar Lima on 2015-10-05 04:43:13 UTC

Comments