pointcloud2 transform c++/python

asked 2018-05-20 20:26:16 -0500

lr101095
21 ●8 ●9 ●16

updated 2018-05-20 20:27:01 -0500

running kinetic, ubuntu 16.04, gazebo 7, sawyer simulator with intera 5.2, kinect/openni camera

warning: i am very new to ROS, C++ and Python.

trying to transform a point cloud (type PointCloud2, PointXYZRGBNormal) from the kinect camera frame to the base frame of sawyer and then publish the transformed pointcloud. I'm doing this so that i can select a point in the world frame for end-effector of sawyer to move to.

I have a script (in C++) that's publishing the point cloud mentioned above to a rostopic. I'll just briefly recount what i've tried to do.

The scripts for moving sawyer are writting in Python. So i tried to write a script that would subscribe to the pointcloud rostopic. In my python script, i was able to write a listener to the transformation tree and i was also able to write a lookupTransform in the frames that i'm dealing with. However, when i tried to perform the actual transformation, the transformPointCloud function i was using was not compatible with PointCloud2. I wasn't sure where to go from there and i wasn't sure which packages i should be using.
I tried to perform the transformation inside the C++ script that is publishing the pointcloud i'm working with. the publishing function actually takes the raw kinect point cloud (PointXYZRGB) and calculates normals for each point then re-pubslishes the pointcloud with normals (PointXYZRGBNormal). both the kinect pointcloud and the pointcloud with normals are in the form of PointCloud2 as stated above. the trouble i'm having with writing in C++ is that i'm not sure if i should subscribe to the publisher within the same script to do the transform, or just pass the point cloud messages between functions, or just to transform the point cloud messages inside the publishing function.

can anyone provide a good, and sufficiently detailed, example of pointcloud2 transformation in Python and/or C++? Preferably, an example that makes use of a transform listener would be most useful since the transformation tree is already set up inside the sawyer simulation.

I'm also getting confused with which tf packages i should be using since all the examples i've been looking at dont explicitly say which packages the functions come from. Adding to that, i'm also not sure of which packages i have nor do i know how to update them. So help regarding packages, package update and installation would be very helpful.

I know that was a lot of info and not much code to work off of. I wouldn't know which bits of code to share. If anything needs clarification, please leave a comment and i'll clarify as much as i can. i'll also share snippets of code in comments as necessary.

thanks in advance!

~ Luke

edit retag flag offensive close merge delete

add a comment

Python

import sensor_msgs.point_cloud2 as pc2 import tf2_ros import tf2_py as tf2 from sensor_msgs.msg import PointCloud2 from tf2_sensor_msgs.tf2_sensor_msgs import do_transform_cloud tf_buffer = tf2_ros.Buffer() tf_listener = tf2_ros.TransformListener(tf_buffer) ... try: trans = tf_buffer.lookup_transform(target_frame, msg.header.frame_id, msg.header.stamp, rospy.Duration(timeout)) except tf2.LookupException as ex: rospy.logwarn(ex) return except tf2.ExtrapolationException as ex: rospy.logwarn(ex) return cloud_out = do_transform_cloud(msg, trans)

C++

roslaunch transform_point_cloud demo.launch

The code:

#include <geometry_msgs/TransformStamped.h> #include <sensor_msgs/PointCloud2.h> #include <tf2_ros/transform_listener.h> ... tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; ... void pointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr& msg) { geometry_msgs::TransformStamped transform;\ try { transform = tf_buffer_.lookupTransform(target_frame_, msg->header.frame_id, msg->header.stamp, ros::Duration(timeout_)); sensor_msgs::PointCloud2 cloud_out; tf2::doTransform(*msg, cloud_out, transform); } catch (tf2::TransformException& ex) { ROS_WARN("%s", ex.what()); return; } ...

Comments

I'll try that in my python script and see how it goes. Would you possibly be able to provide an example in C++?

lr101095 ( 2018-05-21 20:08:56 -0500 )edit

I updated the answer with a C++ example - the repo linked to should build and run also (And maybe is a useful node by itself, there isn't a standalone pointcloud2 transforming node?). I'll make an identically functioning python node there later.

lucasw ( 2018-05-22 00:19:12 -0500 )edit

@lucasw i was able to compile a code, but i've encountered another error. Please see the link below and let me know what you think. thanks!

new error

lr101095 ( 2018-05-23 23:53:14 -0500 )edit

add a comment

pointcloud2 transform c++/python

1 Answer

Python

C++

pcl_ros

Comments

Question Tools

Stats

Related questions

pointcloud2 transform c++/python edit

1 Answer

Python

C++

pcl_ros

Comments

Question Tools

Stats

Related questions

pointcloud2 transform c++/python