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/ros/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   transformIncre[0] = transformBefMapped[0] - transformSum[0];
00058   transformIncre[1] = transformBefMapped[1] - transformSum[1];
00059   transformIncre[2] = transformBefMapped[2] - transformSum[2];
00060 
00061   transformMapped[0] = transformAftMapped[0] - transformIncre[0];
00062   transformMapped[1] = transformAftMapped[1] - transformIncre[1];
00063   transformMapped[2] = transformAftMapped[2] - transformIncre[2];
00064 
00065   x1 = cos(transformMapped[2]) * transformIncre[3] - sin(transformMapped[2]) * transformIncre[4];
00066   y1 = sin(transformMapped[2]) * transformIncre[3] + cos(transformMapped[2]) * transformIncre[4];
00067   z1 = transformIncre[5];
00068 
00069   x2 = x1;
00070   y2 = cos(transformMapped[0]) * y1 - sin(transformMapped[0]) * z1;
00071   z2 = sin(transformMapped[0]) * y1 + cos(transformMapped[0]) * z1;
00072 
00073   transformMapped[3] = transformAftMapped[3] 
00074                      - (cos(transformMapped[1]) * x2 + sin(transformMapped[1]) * z2);
00075   transformMapped[4] = transformAftMapped[4] - y2;
00076   transformMapped[5] = transformAftMapped[5] 
00077                      - (-sin(transformMapped[1]) * x2 + cos(transformMapped[1]) * z2);
00078 }
00079 
00080 void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry)
00081 {
00082   if (fabs(timeOdomBefMapped - timeOdomAftMapped) < 0.005) {
00083 
00084     double roll, pitch, yaw;
00085     geometry_msgs::Quaternion geoQuat = laserOdometry->pose.pose.orientation;
00086     tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw);
00087 
00088     transformSum[0] = -pitch;
00089     transformSum[1] = -yaw;
00090     transformSum[2] = roll;
00091 
00092     transformSum[3] = laserOdometry->pose.pose.position.x;
00093     transformSum[4] = laserOdometry->pose.pose.position.y;
00094     transformSum[5] = laserOdometry->pose.pose.position.z;
00095 
00096     transformAssociateToMap();
00097 
00098     geoQuat = tf::createQuaternionMsgFromRollPitchYaw
00099               (transformMapped[2], -transformMapped[0], -transformMapped[1]);
00100 
00101     laserOdometry2.header.stamp = laserOdometry->header.stamp;
00102     laserOdometry2.pose.pose.orientation.x = -geoQuat.y;
00103     laserOdometry2.pose.pose.orientation.y = -geoQuat.z;
00104     laserOdometry2.pose.pose.orientation.z = geoQuat.x;
00105     laserOdometry2.pose.pose.orientation.w = geoQuat.w;
00106     laserOdometry2.pose.pose.position.x = transformMapped[3];
00107     laserOdometry2.pose.pose.position.y = transformMapped[4];
00108     laserOdometry2.pose.pose.position.z = transformMapped[5];
00109     pubLaserOdometry2Pointer->publish(laserOdometry2);
00110 
00111     laserOdometryTrans2.stamp_ = laserOdometry->header.stamp;
00112     laserOdometryTrans2.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w));
00113     laserOdometryTrans2.setOrigin(tf::Vector3(transformMapped[3], transformMapped[4], transformMapped[5]));
00114     tfBroadcaster2Pointer->sendTransform(laserOdometryTrans2);
00115   }
00116 }
00117 
00118 void odomBefMappedHandler(const nav_msgs::Odometry::ConstPtr& odomBefMapped)
00119 {
00120   timeOdomBefMapped = odomBefMapped->header.stamp.toSec();
00121 
00122   double roll, pitch, yaw;
00123   geometry_msgs::Quaternion geoQuat = odomBefMapped->pose.pose.orientation;
00124   tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw);
00125 
00126   transformBefMapped[0] = -pitch;
00127   transformBefMapped[1] = -yaw;
00128   transformBefMapped[2] = roll;
00129 
00130   transformBefMapped[3] = odomBefMapped->pose.pose.position.x;
00131   transformBefMapped[4] = odomBefMapped->pose.pose.position.y;
00132   transformBefMapped[5] = odomBefMapped->pose.pose.position.z;
00133 }
00134 
00135 void odomAftMappedHandler(const nav_msgs::Odometry::ConstPtr& odomAftMapped)
00136 {
00137   timeOdomAftMapped = odomAftMapped->header.stamp.toSec();
00138 
00139   double roll, pitch, yaw;
00140   geometry_msgs::Quaternion geoQuat = odomAftMapped->pose.pose.orientation;
00141   tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw);
00142 
00143   transformAftMapped[0] = -pitch;
00144   transformAftMapped[1] = -yaw;
00145   transformAftMapped[2] = roll;
00146 
00147   transformAftMapped[3] = odomAftMapped->pose.pose.position.x;
00148   transformAftMapped[4] = odomAftMapped->pose.pose.position.y;
00149   transformAftMapped[5] = odomAftMapped->pose.pose.position.z;
00150 }
00151 
00152 int main(int argc, char** argv)
00153 {
00154   ros::init(argc, argv, "transformMaintenance");
00155   ros::NodeHandle nh;
00156 
00157   ros::Subscriber subLaserOdometry = nh.subscribe<nav_msgs::Odometry> 
00158                                      ("/cam_to_init", 5, laserOdometryHandler);
00159 
00160   ros::Subscriber subOdomBefMapped = nh.subscribe<nav_msgs::Odometry> 
00161                                      ("/bef_mapped_to_init_2", 5, odomBefMappedHandler);
00162 
00163   ros::Subscriber subOdomAftMapped = nh.subscribe<nav_msgs::Odometry> 
00164                                      ("/aft_mapped_to_init_2", 5, odomAftMappedHandler);
00165 
00166   ros::Publisher pubLaserOdometry2 = nh.advertise<nav_msgs::Odometry> ("/cam_to_init_2", 5);
00167   pubLaserOdometry2Pointer = &pubLaserOdometry2;
00168   laserOdometry2.header.frame_id = "/camera_init_2";
00169   laserOdometry2.child_frame_id = "/camera";
00170 
00171   tf::TransformBroadcaster tfBroadcaster2;
00172   tfBroadcaster2Pointer = &tfBroadcaster2;
00173   laserOdometryTrans2.frame_id_ = "/camera_init_2";
00174   laserOdometryTrans2.child_frame_id_ = "/camera";
00175 
00176   ros::spin();
00177 
00178   return 0;
00179 }


loam_back_and_forth
Author(s): Ji Zhang
autogenerated on Tue Jan 7 2014 11:20:25