Send pointcloud2 from matlab to ROS

asked 2014-03-06 04:23:52 -0500

AbrahamAyala gravatar image

Hi guys: i would like to convert a matlab vector of double type to a pointcloud2 message and publish it to ROS. For now I have a matlab vector that has the format of: buff_bat = [ x1 y1 z1 x2 y2 z2 ........] where x1 y1 z1 .... are the coordinates of each point in my synthetic point cloud. so far I have the next code in matlab :

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

            messageBufferbatpoints = org.ros.internal.message.MessageBuffers.dynamicBuffer();
            streambatpoint = org.jboss.netty.buffer.ChannelBufferOutputStream(messageBufferbatpoints);
            streambatpoint.write(buff_bat);
            msg_bat_points.setHeight(1);
            msg_bat_points.setWidth(numofpoints);
            msg_bat_points.setPointStep(3);
            msg_bat_points.setRowStep(numofpoints);
            msg_bat_points.setData(streambatpoint.buffer().copy());
            pub_bat_points.publish(msg_bat_points);

My problem or misunderstanding on how to build the pointcloud2 is principally in streambatpoint.write(buff_bat); since this will convert my data to int and then when doing msg_bat_points.setData(streambatpoint.buffer().copy()); will round up all the decimal of my points. As far as I understand in the definition docs of sensor_msgs/pointcloud2 the data[] field should be of type int, and as I understand here go the coordinates in the order defined by the Fields[] array right? so my question is then how to set double or float 32 coordinates in the data[] field? thank you for the help in advance.

edit retag flag offensive close merge delete