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

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)

roslaunch transform_point_cloud demo.launch

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