transformMaintenance.cpp
Go to the documentation of this file.
00001 #include <math.h>
00002 #include <time.h>
00003 #include <stdio.h>
00004 #include <stdlib.h>
00005 #include <ros/ros.h>
00006 
00007 #include <nav_msgs/Odometry.h>
00008 #include <sensor_msgs/Imu.h>
00009 #include <sensor_msgs/PointCloud2.h>
00010 
00011 #include <tf/transform_datatypes.h>
00012 #include <tf/transform_broadcaster.h>
00013 
00014 #include <opencv/cv.h>
00015 #include <opencv2/highgui/highgui.hpp>
00016 
00017 #include <pcl_conversions/pcl_conversions.h>
00018 #include <pcl/point_cloud.h>
00019 #include <pcl/point_types.h>
00020 #include <pcl/filters/voxel_grid.h>
00021 #include <pcl/kdtree/kdtree_flann.h>
00022 
00023 const double PI = 3.1415926;
00024 const double rad2deg = 180 / PI;
00025 const double deg2rad = PI / 180;
00026 
00027 double timeOdomBefMapped;
00028 double timeOdomAftMapped;
00029 
00030 float transformSum[6] = {0};
00031 float transformIncre[6] = {0};
00032 float transformMapped[6] = {0};
00033 float transformBefMapped[6] = {0};
00034 float transformAftMapped[6] = {0};
00035 
00036 ros::Publisher *pubLaserOdometry2Pointer = NULL;
00037 tf::TransformBroadcaster *tfBroadcaster2Pointer = NULL;
00038 nav_msgs::Odometry laserOdometry2;
00039 tf::StampedTransform laserOdometryTrans2;
00040 
00041 void transformAssociateToMap()
00042 {
00043   float x1 = cos(transformSum[1]) * (transformBefMapped[3] - transformSum[3]) 
00044            - sin(transformSum[1]) * (transformBefMapped[5] - transformSum[5]);
00045   float y1 = transformBefMapped[4] - transformSum[4];
00046   float z1 = sin(transformSum[1]) * (transformBefMapped[3] - transformSum[3]) 
00047            + cos(transformSum[1]) * (transformBefMapped[5] - transformSum[5]);
00048 
00049   float x2 = x1;
00050   float y2 = cos(transformSum[0]) * y1 + sin(transformSum[0]) * z1;
00051   float z2 = -sin(transformSum[0]) * y1 + cos(transformSum[0]) * z1;
00052 
00053   transformIncre[3] = cos(transformSum[2]) * x2 + sin(transformSum[2]) * y2;
00054   transformIncre[4] = -sin(transformSum[2]) * x2 + cos(transformSum[2]) * y2;
00055   transformIncre[5] = z2;
00056 
00057   float sbcx = sin(transformSum[0]);
00058   float cbcx = cos(transformSum[0]);
00059   float sbcy = sin(transformSum[1]);
00060   float cbcy = cos(transformSum[1]);
00061   float sbcz = sin(transformSum[2]);
00062   float cbcz = cos(transformSum[2]);
00063 
00064   float sblx = sin(transformBefMapped[0]);
00065   float cblx = cos(transformBefMapped[0]);
00066   float sbly = sin(transformBefMapped[1]);
00067   float cbly = cos(transformBefMapped[1]);
00068   float sblz = sin(transformBefMapped[2]);
00069   float cblz = cos(transformBefMapped[2]);
00070 
00071   float salx = sin(transformAftMapped[0]);
00072   float calx = cos(transformAftMapped[0]);
00073   float saly = sin(transformAftMapped[1]);
00074   float caly = cos(transformAftMapped[1]);
00075   float salz = sin(transformAftMapped[2]);
00076   float calz = cos(transformAftMapped[2]);
00077 
00078   float srx = -sbcx*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly) 
00079             - cbcx*cbcz*(calx*saly*(cbly*sblz - cblz*sblx*sbly) 
00080             - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) 
00081             - cbcx*sbcz*(calx*caly*(cblz*sbly - cbly*sblx*sblz) 
00082             - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz);
00083   transformMapped[0] = -asin(srx);
00084 
00085   float srycrx = (cbcy*sbcz - cbcz*sbcx*sbcy)*(calx*saly*(cbly*sblz - cblz*sblx*sbly) 
00086                - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) 
00087                - (cbcy*cbcz + sbcx*sbcy*sbcz)*(calx*caly*(cblz*sbly - cbly*sblx*sblz) 
00088                - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz) 
00089                + cbcx*sbcy*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly);
00090   float crycrx = (cbcz*sbcy - cbcy*sbcx*sbcz)*(calx*caly*(cblz*sbly - cbly*sblx*sblz) 
00091                - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz) 
00092                - (sbcy*sbcz + cbcy*cbcz*sbcx)*(calx*saly*(cbly*sblz - cblz*sblx*sbly) 
00093                - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) 
00094                + cbcx*cbcy*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly);
00095   transformMapped[1] = atan2(srycrx / cos(transformMapped[0]), crycrx / cos(transformMapped[0]));
00096   
00097   float srzcrx = sbcx*(cblx*cbly*(calz*saly - caly*salx*salz) 
00098                - cblx*sbly*(caly*calz + salx*saly*salz) + calx*salz*sblx) 
00099                - cbcx*cbcz*((caly*calz + salx*saly*salz)*(cbly*sblz - cblz*sblx*sbly) 
00100                + (calz*saly - caly*salx*salz)*(sbly*sblz + cbly*cblz*sblx) 
00101                - calx*cblx*cblz*salz) + cbcx*sbcz*((caly*calz + salx*saly*salz)*(cbly*cblz 
00102                + sblx*sbly*sblz) + (calz*saly - caly*salx*salz)*(cblz*sbly - cbly*sblx*sblz) 
00103                + calx*cblx*salz*sblz);
00104   float crzcrx = sbcx*(cblx*sbly*(caly*salz - calz*salx*saly) 
00105                - cblx*cbly*(saly*salz + caly*calz*salx) + calx*calz*sblx) 
00106                + cbcx*cbcz*((saly*salz + caly*calz*salx)*(sbly*sblz + cbly*cblz*sblx) 
00107                + (caly*salz - calz*salx*saly)*(cbly*sblz - cblz*sblx*sbly) 
00108                + calx*calz*cblx*cblz) - cbcx*sbcz*((saly*salz + caly*calz*salx)*(cblz*sbly 
00109                - cbly*sblx*sblz) + (caly*salz - calz*salx*saly)*(cbly*cblz + sblx*sbly*sblz) 
00110                - calx*calz*cblx*sblz);
00111   transformMapped[2] = atan2(srzcrx / cos(transformMapped[0]), crzcrx / cos(transformMapped[0]));
00112 
00113   x1 = cos(transformMapped[2]) * transformIncre[3] - sin(transformMapped[2]) * transformIncre[4];
00114   y1 = sin(transformMapped[2]) * transformIncre[3] + cos(transformMapped[2]) * transformIncre[4];
00115   z1 = transformIncre[5];
00116 
00117   x2 = x1;
00118   y2 = cos(transformMapped[0]) * y1 - sin(transformMapped[0]) * z1;
00119   z2 = sin(transformMapped[0]) * y1 + cos(transformMapped[0]) * z1;
00120 
00121   transformMapped[3] = transformAftMapped[3] 
00122                      - (cos(transformMapped[1]) * x2 + sin(transformMapped[1]) * z2);
00123   transformMapped[4] = transformAftMapped[4] - y2;
00124   transformMapped[5] = transformAftMapped[5] 
00125                      - (-sin(transformMapped[1]) * x2 + cos(transformMapped[1]) * z2);
00126 }
00127 
00128 void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry)
00129 {
00130   if (fabs(timeOdomBefMapped - timeOdomAftMapped) < 0.005) {
00131 
00132     double roll, pitch, yaw;
00133     geometry_msgs::Quaternion geoQuat = laserOdometry->pose.pose.orientation;
00134     tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw);
00135 
00136     transformSum[0] = -pitch;
00137     transformSum[1] = -yaw;
00138     transformSum[2] = roll;
00139 
00140     transformSum[3] = laserOdometry->pose.pose.position.x;
00141     transformSum[4] = laserOdometry->pose.pose.position.y;
00142     transformSum[5] = laserOdometry->pose.pose.position.z;
00143 
00144     transformAssociateToMap();
00145 
00146     geoQuat = tf::createQuaternionMsgFromRollPitchYaw
00147               (transformMapped[2], -transformMapped[0], -transformMapped[1]);
00148 
00149     laserOdometry2.header.stamp = laserOdometry->header.stamp;
00150     laserOdometry2.pose.pose.orientation.x = -geoQuat.y;
00151     laserOdometry2.pose.pose.orientation.y = -geoQuat.z;
00152     laserOdometry2.pose.pose.orientation.z = geoQuat.x;
00153     laserOdometry2.pose.pose.orientation.w = geoQuat.w;
00154     laserOdometry2.pose.pose.position.x = transformMapped[3];
00155     laserOdometry2.pose.pose.position.y = transformMapped[4];
00156     laserOdometry2.pose.pose.position.z = transformMapped[5];
00157     pubLaserOdometry2Pointer->publish(laserOdometry2);
00158 
00159     laserOdometryTrans2.stamp_ = laserOdometry->header.stamp;
00160     laserOdometryTrans2.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w));
00161     laserOdometryTrans2.setOrigin(tf::Vector3(transformMapped[3], transformMapped[4], transformMapped[5]));
00162     tfBroadcaster2Pointer->sendTransform(laserOdometryTrans2);
00163   }
00164 }
00165 
00166 void odomBefMappedHandler(const nav_msgs::Odometry::ConstPtr& odomBefMapped)
00167 {
00168   timeOdomBefMapped = odomBefMapped->header.stamp.toSec();
00169 
00170   double roll, pitch, yaw;
00171   geometry_msgs::Quaternion geoQuat = odomBefMapped->pose.pose.orientation;
00172   tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw);
00173 
00174   transformBefMapped[0] = -pitch;
00175   transformBefMapped[1] = -yaw;
00176   transformBefMapped[2] = roll;
00177 
00178   transformBefMapped[3] = odomBefMapped->pose.pose.position.x;
00179   transformBefMapped[4] = odomBefMapped->pose.pose.position.y;
00180   transformBefMapped[5] = odomBefMapped->pose.pose.position.z;
00181 }
00182 
00183 void odomAftMappedHandler(const nav_msgs::Odometry::ConstPtr& odomAftMapped)
00184 {
00185   timeOdomAftMapped = odomAftMapped->header.stamp.toSec();
00186 
00187   double roll, pitch, yaw;
00188   geometry_msgs::Quaternion geoQuat = odomAftMapped->pose.pose.orientation;
00189   tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw);
00190 
00191   transformAftMapped[0] = -pitch;
00192   transformAftMapped[1] = -yaw;
00193   transformAftMapped[2] = roll;
00194 
00195   transformAftMapped[3] = odomAftMapped->pose.pose.position.x;
00196   transformAftMapped[4] = odomAftMapped->pose.pose.position.y;
00197   transformAftMapped[5] = odomAftMapped->pose.pose.position.z;
00198 }
00199 
00200 int main(int argc, char** argv)
00201 {
00202   ros::init(argc, argv, "transformMaintenance");
00203   ros::NodeHandle nh;
00204 
00205   ros::Subscriber subLaserOdometry = nh.subscribe<nav_msgs::Odometry> 
00206                                      ("/cam_to_init", 5, laserOdometryHandler);
00207 
00208   ros::Subscriber subOdomBefMapped = nh.subscribe<nav_msgs::Odometry> 
00209                                      ("/bef_mapped_to_init_2", 5, odomBefMappedHandler);
00210 
00211   ros::Subscriber subOdomAftMapped = nh.subscribe<nav_msgs::Odometry> 
00212                                      ("/aft_mapped_to_init_2", 5, odomAftMappedHandler);
00213 
00214   ros::Publisher pubLaserOdometry2 = nh.advertise<nav_msgs::Odometry> ("/cam_to_init_2", 5);
00215   pubLaserOdometry2Pointer = &pubLaserOdometry2;
00216   laserOdometry2.header.frame_id = "/camera_init_2";
00217   laserOdometry2.child_frame_id = "/camera";
00218 
00219   tf::TransformBroadcaster tfBroadcaster2;
00220   tfBroadcaster2Pointer = &tfBroadcaster2;
00221   laserOdometryTrans2.frame_id_ = "/camera_init_2";
00222   laserOdometryTrans2.child_frame_id_ = "/camera";
00223 
00224   ros::spin();
00225 
00226   return 0;
00227 }


loam_back_and_forth
Author(s): Ji Zhang
autogenerated on Fri Oct 17 2014 20:02:54