Transforming PointCloud2

edit

Hi,

So I'm trying to transform a PointCloud2 using the method: transformPointCloud (const std::string &target_frame, const sensor_msgs::PointCloud2 &in, sensor_msgs::PointCloud2 &out, const tf::TransformListener &tf_listener)

Here is my code:

class SubscribeAndPublish
{
public:
  SubscribeAndPublish()
  {
    sub_ = n_.subscribe("/camera/depth/points", 1, &SubscribeAndPublish::callback, this);

    base_link = new std::string("base_link");
  }

  void callback(const sensor_msgs::PointCloud2::ConstPtr& msg)
  {
    tf::Transform transform;
    transform.setOrigin( tf::Vector3(0.0, 0.0, 0.0) );
    tf::Quaternion q;
    q.setRPY(0, 0, 0);
    transform.setRotation(q);
    br.sendTransform(tf::StampedTransform(transform, msg->header.stamp, "base_link", "camera_link"));

    pcl_ros::transformPointCloud(*base_link, transform, *msg, *msgOut);
}

private:
  ros::NodeHandle n_;
  ros::Subscriber sub_;
  tf::TransformBroadcaster br;
  boost::shared_ptr<sensor_msgs::PointCloud2> msgOut;
  boost::shared_ptr<tf::TransformListener> listener;
  std::string *base_link;
};

int main(int argc, char **argv)
{
  ros::init(argc, argv, "subscribe_and_publish");

  SubscribeAndPublish SAPObject;

  ros::Rate r(10);
  while(ros::ok()) {

    ros::spinOnce();
    r.sleep();
  }

  return 0;
}

I have created the transform broadcaster so that I could visualize the transform in rviz and using the same transform to transform the PointCloud since I need to do some calculations on it (find the highest z coordinate when the PointCloud is transformed to be horizontal, but I can't do it on the original cloud since the PointCloud is tilted). I haven't added any numbers to tf::Quaternion q; q.setRPY(0, 0, 0) atm, but I will later.

However when i run the code i get an error: Segmentation fault (core dumped).