00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00036 #include <ros/ros.h>
00037 #include <sensor_msgs/PointCloud2.h>
00038 #include <tf/transform_listener.h>
00039 #include <pcl/ros/conversions.h>
00040 #include <pcl/point_cloud.h>
00041 #include <pcl/point_types.h>
00042 #include <pcl_ros/transforms.h>
00043
00044 #define PFLN printf("Line=%d\n", __LINE__);
00045
00046 using namespace std;
00047
00048
00049
00050 ros::Publisher pub;
00051 string frame_id;
00052 tf::TransformListener *listener_center_bumper_ptr;
00053
00054
00055 void topic_callback(const sensor_msgs::PointCloud2ConstPtr& msg)
00056 {
00057 static int counter=0; counter++;
00058 ROS_INFO("Message received n= %d",counter);
00059
00060
00061 pcl::PointCloud<pcl::PointXYZRGB> cloud_in;
00062 pcl::PointCloud<pcl::PointXYZRGB> cloud_trans;
00063
00064
00065 pcl::fromROSMsg(*msg,cloud_in);
00066
00067 tf::StampedTransform transform;
00068 try
00069 {
00070 listener_center_bumper_ptr->lookupTransform(frame_id, msg->header.frame_id, ros::Time(0), transform);
00071 }
00072 catch (tf::TransformException ex)
00073 {
00074 ROS_ERROR("%s",ex.what());
00075 }
00076
00077
00078 pcl_ros::transformPointCloud (cloud_in,cloud_trans,transform);
00079 cloud_trans.header.frame_id=frame_id;
00080 sensor_msgs::PointCloud2 msg_pub;
00081
00082 pcl::toROSMsg(cloud_trans, msg_pub);
00083 pub.publish(msg_pub);
00084 }
00085
00086
00087 int main(int argc, char **argv)
00088 {
00089 ROS_INFO("Started transform cloud nodelet");
00090 ros::init(argc,argv,"transform_cloud_nodelet");
00091
00092 ros::NodeHandle n;
00093
00094 tf::TransformListener listener_center_bumper;
00095 listener_center_bumper_ptr=&listener_center_bumper;
00096
00097
00098 string pointcloud_subscribed;
00099 if (!n.hasParam("pointcloud_to_transform"))
00100 {
00101 ROS_ERROR("Must set properly a input pointcloud");
00102 return -1;
00103 }
00104 else
00105 n.getParam("pointcloud_to_transform", pointcloud_subscribed);
00106
00107
00108 if (!n.hasParam("frame_id"))
00109 {
00110 ROS_ERROR("Must set properly a frame_id to transform");
00111 return -1;
00112 }
00113 else
00114 n.getParam("frame_id", frame_id);
00115
00116
00117 string pointcloud_transformed;
00118 if (!n.hasParam("pointcloud_transformed"))
00119 {
00120 ROS_ERROR("Must set properly a topic to publish the transformed cloud");
00121 return -1;
00122 }
00123 else
00124 n.getParam("pointcloud_transformed", pointcloud_transformed);
00125
00126 ros::Subscriber sub = n.subscribe(pointcloud_subscribed, 1, topic_callback);
00127 pub=n.advertise<sensor_msgs::PointCloud2>(pointcloud_transformed,1000);
00128
00129 ros::spin();
00130 return 1;
00131
00132
00133 }