pcloud_rot.cpp
Go to the documentation of this file.
00001 /*
00002  * pcloud_rot.cpp
00003  *
00004  *  Created on: Jan 11, 2015
00005  *      Author: lab-118
00006  */
00007 
00008 
00009 #include <ros/ros.h>
00010 #include <sstream>
00011 #include <iostream>
00012 #include <time.h>
00013 #include <stdlib.h>
00014 #include <string>
00015 #include <sensor_msgs/PointCloud2.h>
00016 #include <pcl_conversions/pcl_conversions.h>
00017 #include <pcl/point_cloud.h>
00018 #include <pcl/point_types.h>
00019 #include <pcl/common/transforms.h>
00020 #include <pcl/common/eigen.h>
00021 
00022         ros::Publisher rotated_cloud;
00023         Eigen::Matrix4f rotMatrixX ;
00024         Eigen::Matrix4f rotMatrixY ;
00025         Eigen::Matrix4f rotMatrixZ ;
00026         Eigen::Matrix4f rot_matrix ;
00027 
00028 void rotCallback(const sensor_msgs::PointCloud2ConstPtr &msg)
00029 {
00030                 sensor_msgs::PointCloud2 transformed_cloud;
00031 
00032                 pcl::PointCloud<pcl::PointXYZRGB> in_pointcloud ;
00033 
00034                 pcl::fromROSMsg(*msg , in_pointcloud);
00035                 pcl::PointCloud<pcl::PointXYZRGB> out_pointcloud;
00036                 pcl::transformPointCloud(in_pointcloud , out_pointcloud , rot_matrix );
00037                 pcl::toROSMsg(out_pointcloud , transformed_cloud);
00038                 rotated_cloud.publish(transformed_cloud);
00039 }
00040 
00041 int main (int argc , char** argv)
00042 {
00043         ros::init(argc , argv , "cloud_rot");
00044         ros::NodeHandle n ;
00045         ros::Rate loop_rate(10) ;
00046 
00047         char robot[50] = {0} ;
00048 
00049         sprintf(robot,"%s_%s",argv[2], argv[1]);
00050         
00051         std::string pre(robot);
00052         std::string name(argv[2]);
00053         
00054         rotated_cloud = n.advertise<sensor_msgs::PointCloud2>(std::string("/"+pre+"/"+name+"_Asus_Camera/depth/fixed_points"),100);
00055         ros::Subscriber cloud_sub = n.subscribe(std::string("/"+pre+"/"+name+"_Asus_Camera/depth/points"),1000, rotCallback);
00056 
00057         double rotx = 0.0 ;
00058         double roty = M_PI_2 ;
00059         double rotz = -M_PI_2 ;
00060 
00061         rotMatrixX <<
00062                         1.0, 0.0, 0.0, 0.0,
00063                         0.0, cos(rotx), -sin(rotx), 0.0,
00064                         0.0, sin(rotx), cos(rotx), 0.0,
00065                         0.0, 0.0, 0.0, 1.0;
00066 
00067         rotMatrixY <<
00068                         cos(roty), 0.0, sin(roty), 0.0,
00069                         0.0, 1.0, 0.0, 0.0,
00070                         -sin(roty), 0.0, cos(roty), 0.0,
00071                         0.0, 0.0, 0.0, 1.0;
00072 
00073         rotMatrixZ <<
00074                         cos(rotz), -sin(rotz), 0.0, 0.0,
00075                         sin(rotz), cos(rotz), 0.0, 0.0,
00076                         0.0, 0.0, 1.0, 0.0,
00077                         0.0, 0.0, 0.0, 1.0;
00078 
00079         rot_matrix = (rotMatrixX * rotMatrixY)*rotMatrixZ;
00080 
00081         while(ros::ok())
00082         {
00083                 ros::spinOnce();
00084                 loop_rate.sleep();
00085         }
00086         return 0;
00087 }
00088 


pcloud_rot
Author(s): RoboTiCan
autogenerated on Mon Jun 22 2015 12:02:50