Ask Your Question
0

pointcloud2 transform c++/python

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

lr101095 gravatar image

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.

  1. 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.

  2. 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

1 Answer

Sort by ยป oldest newest most voted
1

answered 2018-05-21 12:55:27 -0500

lucasw gravatar image

updated 2018-05-23 00:03:18 -0500

Python

There is a doTransform example in #q12890https://answers.ros.org/question/1289... in comment form, I've fleshed it out here:

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++

Fleshed out C++ example

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;
  }
  ...

pcl_ros

pcl_ros has a tf1 transformPointCloud function, it would be nice if that could be updated to use tf2. It would be fewer lines of code and less error prone with timestamping than the do transform methods.

edit flag offensive delete link more

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 gravatar imagelr101095 ( 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 gravatar imagelucasw ( 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 gravatar imagelr101095 ( 2018-05-23 23:53:14 -0500 )edit

Your Answer

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

Add Answer

Question Tools

3 followers

Stats

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

Seen: 1,178 times

Last updated: May 23 '18