ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

pointcloud2 transform c++/python

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

lr101095 gravatar image

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

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
add a comment

1 Answer

Sort by ยป oldest newest most voted
5

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

lucasw gravatar image

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

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 image lr101095  ( 2018-05-21 20:08:56 -0600 )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 image lucasw  ( 2018-05-22 00:19:12 -0600 )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 image lr101095  ( 2018-05-23 23:53:14 -0600 )edit
add a comment

Question Tools

3 followers

subscribe to rss feed

Stats

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

Seen: 7,760 times

Last updated: May 23 '18

Related questions

Do I need to know any specific computer language or framework to use ROS?

Reading PointCloud2 in C++

How to use Python setuptools to build a Python extension

Is anybody in ROS community using PySide?

How to programmatically reset Gazebo

gazebo crashes when calling delete_model service call

Any good examples of using dynamic_reconfigure for a python node?

where do I get a gazebo diff drive plugin?

Material-dependent transparency in Gazebo

Importing XML to gazebo

ROS Answers is licensed under Creative Commons Attribution 3.0 Content on this site is licensed under a Creative Commons Attribution Share Alike 3.0 license.
about | faq | help | privacy policy | terms of service
Powered by Askbot version 0.10.2