point cloud transform with static_transform_publisher

edit

Hi,ereryone,

I have a problem about point cloud transform, can you help me.

I want to transform a pointcloud (come from a tilt kinect sensor) in camera_depth_optical to base_link frame.but I cannot get a right result.

I subscribe a sensor_msgs::PointCloud2 from kinect(as camera/depth/points),then I transform the pointclouds into pcl::pointcloud(pcl::pointxyz) use pcl::fromROSMsg. My code is like this:

typedef pcl::PointXYZ PointT;
   typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
   static ros::Publisher pub;
 void callback(const sensor_msgs::PointCloud2ConstPtr& input)
  {
     PointCloud::Ptr cloud (new PointCloud);
     PointCloud::Ptr cloud_out (new PointCloud);
     PointCloud::Ptr cloud_out2 (new PointCloud);
     sensor_msgs::PointCloud2::Ptr cloud_pub(new sensor_msgs::PointCloud2);
     pcl::fromROSMsg(*input, *cloud);
     cloud->header.frame_id = "/camera_depth_optical";
     cloud->header.stamp = ros::Time::now().toNSec();
    cloud->is_dense = false;

  try{
    tf::StampedTransform transform;
    listener->waitForTransform("/camera_depth_optical", "/camera_depth", ros::Time(0), ros::Duration(3.0));
    listener->lookupTransform( "/camera_depth_optical",  "/camera_depth",ros::Time(0), transform);
    pcl_ros::transformPointCloud( *cloud, *cloud_out, transform );
    cloud_out->header.frame_id = "camera_depth";
}
   catch(tf::TransformException& ex){
    ROS_ERROR("Received an exception trying to transform a point from \"camera_depth_optical\" to \"camera_depth\": %s", ex.what());
}
cloud_out->header.stamp = ros::Time::now().toNSec();
pcl_conversions::toPCL(ros::Time::now(), cloud_out->header.stamp);


try{
    tf::StampedTransform transform0;
    listener2->waitForTransform("/camera_depth", "/camera_link", ros::Time(0), ros::Duration(3.0));
    listener2->lookupTransform( "/camera_depth",  "/camera_link",ros::Time(0), transform0);

    pcl_ros::transformPointCloud( *cloud_out, *cloud_out1, transform0 );
    cloud_out1->header.frame_id = "camera_link";
}
   catch(tf::TransformException& ex){
    ROS_ERROR("Received an exception trying to transform a point from \"camera_depth\" to \"camera_link\": %s", ex.what());
}
cloud_out1->header.stamp = ros::Time::now().toNSec();
pcl_conversions::toPCL(ros::Time::now(), cloud_out1->header.stamp);

try{
    tf::StampedTransform transform0;
    listener2->waitForTransform("/camera_link", "/base_link", ros::Time(0), ros::Duration(3.0));
    listener2->lookupTransform( "/camera_link",  "/base_link",ros::Time(0), transform0);

    pcl_ros::transformPointCloud( *cloud_out1, *cloud_out2, transform0 );
    cloud_out2->header.frame_id = "base_link";
}
   catch(tf::TransformException& ex){
    ROS_ERROR("Received an exception trying to transform a point from \"camera_link\" to \"base_link\": %s", ex.what());
}
cloud_out2->header.stamp = ros::Time::now().toNSec();
pcl_conversions::toPCL(ros::Time::now(), cloud_out2->header.stamp);

pcl::toROSMsg(*cloud_out2, *cloud_pub);
cloud_pub->header.frame_id = "base_link";
cloud_pub->header.stamp = ros::Time::now();
pub.publish(*cloud_pub);
ROS_INFO("cloud_pub published!");

cloud->points.clear();
cloud_out->points.clear();
cloud_out1->points.clear();
cloud_out2->points.clear();
}

my launch file is :

<launch>

<arg name="camera" default="camera" />

<!-- start sensor-->
<include file="$(find openni_launch)/launch/openni.launch">
    <arg name="camera" default="$(arg camera)"/>
</include>

<node pkg="tf" type="static_transform_publisher" name="camera_optical_link" args="0 0 0 -0.5 0.5 -0.5 0.5 camera_depth_frame camera_depth_optical_frame 25" />

<node pkg="tf" type="static_transform_publisher" name="camera_base" args="0 -0.02 0 0.0 0.0 0.0 1.0 camera_link camera_depth_frame 25" />

<node pkg="tf" type="static_transform_publisher" name="point_link_broadcaster" args="0.0 1.29 0.0 0.5 -0.5 0.5 0.5 base_link camera_link 25" />

<node pkg="pub_point" type="pub_point" name="pub_point" output="screen">
      <remap from="/input_points" to="$(arg camera)/depth/points"/>
</node>
<launch>

Is that right ? Can you give me some suggest ? thank you very much!

edit:

Hi, ereryone. I have modify the three static_transform_publisher together as one ... (more)