Ask Your Question
0

displaying a point cloud in rviz

asked 2014-01-16 06:07:24 -0500

david.c.liebman gravatar image

updated 2014-01-28 17:19:04 -0500

ngrennan gravatar image

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=/turtlebot_talktest_19193_1389894558845] For frame [/map]: Fixed Frame [caster_front_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?

edit retag flag offensive close merge delete

3 Answers

Sort by ยป oldest newest most voted
0

answered 2014-02-12 09:09:21 -0500

marcoesposito1988 gravatar image

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

edit flag offensive delete link more

Comments

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

david.c.liebman gravatar imagedavid.c.liebman ( 2014-02-14 06:11:31 -0500 )edit

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.

david.c.liebman gravatar imagedavid.c.liebman ( 2014-02-14 07:53:39 -0500 )edit
0

answered 2015-10-05 04:43:13 -0500

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

edit flag offensive delete link more
0

answered 2014-02-12 14:09:40 -0500

jihoonl gravatar image

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.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2014-01-16 06:07:24 -0500

Seen: 4,866 times

Last updated: Oct 05 '15