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 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 }