transformMaintenance.cpp
Go to the documentation of this file.
00001 #include <math.h>
00002 #include <stdio.h>
00003 #include <stdlib.h>
00004 #include <ros/ros.h>
00005 
00006 #include <nav_msgs/Odometry.h>
00007 
00008 #include <tf/transform_datatypes.h>
00009 #include <tf/transform_listener.h>
00010 #include <tf/transform_broadcaster.h>
00011 
00012 const double PI = 3.1415926;
00013 const double rad2deg = 180 / PI;
00014 const double deg2rad = PI / 180;
00015 
00016 double timeOdomBefBA;
00017 double timeOdomAftBA;
00018 
00019 double rollRec, pitchRec, yawRec;
00020 double txRec, tyRec, tzRec;
00021 
00022 double transformBefBA[6] = {0};
00023 double transformAftBA[6] = {0};
00024 
00025 ros::Publisher *voData2PubPointer = NULL;
00026 tf::TransformBroadcaster *tfBroadcaster2Pointer = NULL;
00027 nav_msgs::Odometry voData2;
00028 tf::StampedTransform voDataTrans2;
00029 
00030 void transformAssociateToBA()
00031 {
00032   double txDiff = txRec - transformBefBA[3];
00033   double tyDiff = tyRec - transformBefBA[4];
00034   double tzDiff = tzRec - transformBefBA[5];
00035 
00036   double x1 = cos(yawRec) * txDiff + sin(yawRec) * tzDiff;
00037   double y1 = tyDiff;
00038   double z1 = -sin(yawRec) * txDiff + cos(yawRec) * tzDiff;
00039 
00040   double x2 = x1;
00041   double y2 = cos(pitchRec) * y1 - sin(pitchRec) * z1;
00042   double z2 = sin(pitchRec) * y1 + cos(pitchRec) * z1;
00043 
00044   txDiff = cos(rollRec) * x2 + sin(rollRec) * y2;
00045   tyDiff = -sin(rollRec) * x2 + cos(rollRec) * y2;
00046   tzDiff = z2;
00047 
00048   double sbcx = sin(-pitchRec);
00049   double cbcx = cos(-pitchRec);
00050   double sbcy = sin(-yawRec);
00051   double cbcy = cos(-yawRec);
00052   double sbcz = sin(rollRec);
00053   double cbcz = cos(rollRec);
00054 
00055   double sblx = sin(-transformBefBA[0]);
00056   double cblx = cos(-transformBefBA[0]);
00057   double sbly = sin(-transformBefBA[1]);
00058   double cbly = cos(-transformBefBA[1]);
00059   double sblz = sin(transformBefBA[2]);
00060   double cblz = cos(transformBefBA[2]);
00061 
00062   double salx = sin(-transformAftBA[0]);
00063   double calx = cos(-transformAftBA[0]);
00064   double saly = sin(-transformAftBA[1]);
00065   double caly = cos(-transformAftBA[1]);
00066   double salz = sin(transformAftBA[2]);
00067   double calz = cos(transformAftBA[2]);
00068 
00069   double srx = -sbcx*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly) 
00070              - cbcx*cbcz*(calx*saly*(cbly*sblz - cblz*sblx*sbly) 
00071              - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) 
00072              - cbcx*sbcz*(calx*caly*(cblz*sbly - cbly*sblx*sblz) 
00073              - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz);
00074   pitchRec = asin(srx);
00075 
00076   double srycrx = (cbcy*sbcz - cbcz*sbcx*sbcy)*(calx*saly*(cbly*sblz - cblz*sblx*sbly) 
00077                 - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) 
00078                 - (cbcy*cbcz + sbcx*sbcy*sbcz)*(calx*caly*(cblz*sbly - cbly*sblx*sblz) 
00079                 - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz) 
00080                 + cbcx*sbcy*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly);
00081   double crycrx = (cbcz*sbcy - cbcy*sbcx*sbcz)*(calx*caly*(cblz*sbly - cbly*sblx*sblz) 
00082                 - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz) 
00083                 - (sbcy*sbcz + cbcy*cbcz*sbcx)*(calx*saly*(cbly*sblz - cblz*sblx*sbly) 
00084                 - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) 
00085                 + cbcx*cbcy*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly);
00086   yawRec = -atan2(srycrx / cos(-pitchRec), crycrx / cos(-pitchRec));
00087 
00088   double srzcrx = sbcx*(cblx*cbly*(calz*saly - caly*salx*salz) 
00089                 - cblx*sbly*(caly*calz + salx*saly*salz) + calx*salz*sblx) 
00090                 - cbcx*cbcz*((caly*calz + salx*saly*salz)*(cbly*sblz - cblz*sblx*sbly) 
00091                 + (calz*saly - caly*salx*salz)*(sbly*sblz + cbly*cblz*sblx) 
00092                 - calx*cblx*cblz*salz) + cbcx*sbcz*((caly*calz + salx*saly*salz)*(cbly*cblz 
00093                 + sblx*sbly*sblz) + (calz*saly - caly*salx*salz)*(cblz*sbly - cbly*sblx*sblz) 
00094                 + calx*cblx*salz*sblz);
00095   double crzcrx = sbcx*(cblx*sbly*(caly*salz - calz*salx*saly) 
00096                 - cblx*cbly*(saly*salz + caly*calz*salx) + calx*calz*sblx) 
00097                 + cbcx*cbcz*((saly*salz + caly*calz*salx)*(sbly*sblz + cbly*cblz*sblx) 
00098                 + (caly*salz - calz*salx*saly)*(cbly*sblz - cblz*sblx*sbly) 
00099                 + calx*calz*cblx*cblz) - cbcx*sbcz*((saly*salz + caly*calz*salx)*(cblz*sbly 
00100                 - cbly*sblx*sblz) + (caly*salz - calz*salx*saly)*(cbly*cblz + sblx*sbly*sblz) 
00101                 - calx*calz*cblx*sblz);
00102   rollRec = atan2(srzcrx / cos(-pitchRec), crzcrx / cos(-pitchRec));
00103 
00104   x1 = cos(rollRec) * txDiff - sin(rollRec) * tyDiff;
00105   y1 = sin(rollRec) * txDiff + cos(rollRec) * tyDiff;
00106   z1 = tzDiff;
00107 
00108   x2 = x1;
00109   y2 = cos(pitchRec) * y1 + sin(pitchRec) * z1;
00110   z2 = -sin(pitchRec) * y1 + cos(pitchRec) * z1;
00111 
00112   txDiff = cos(yawRec) * x2 - sin(yawRec) * z2;
00113   tyDiff = y2;
00114   tzDiff = sin(yawRec) * x2 + cos(yawRec) * z2;
00115 
00116   txRec = transformAftBA[3] + txDiff;
00117   tyRec = transformAftBA[4] + tyDiff;
00118   tzRec = transformAftBA[5] + tzDiff;
00119 }
00120 
00121 void voDataHandler(const nav_msgs::Odometry::ConstPtr& voData)
00122 {
00123   if (fabs(timeOdomBefBA - timeOdomAftBA) < 0.005) {
00124 
00125     geometry_msgs::Quaternion geoQuat = voData->pose.pose.orientation;
00126     tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(rollRec, pitchRec, yawRec);
00127 
00128     txRec = voData->pose.pose.position.x;
00129     tyRec = voData->pose.pose.position.y;
00130     tzRec = voData->pose.pose.position.z;
00131 
00132     transformAssociateToBA();
00133 
00134     geoQuat = tf::createQuaternionMsgFromRollPitchYaw(rollRec, pitchRec, yawRec);
00135 
00136     voData2.header.stamp = voData->header.stamp;
00137     voData2.pose.pose.orientation.x = -geoQuat.y;
00138     voData2.pose.pose.orientation.y = -geoQuat.z;
00139     voData2.pose.pose.orientation.z = geoQuat.x;
00140     voData2.pose.pose.orientation.w = geoQuat.w;
00141     voData2.pose.pose.position.x = txRec;
00142     voData2.pose.pose.position.y = tyRec;
00143     voData2.pose.pose.position.z = tzRec;
00144     voData2PubPointer->publish(voData2);
00145 
00146     voDataTrans2.stamp_ = voData->header.stamp;
00147     voDataTrans2.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w));
00148     voDataTrans2.setOrigin(tf::Vector3(txRec, tyRec, tzRec));
00149     tfBroadcaster2Pointer->sendTransform(voDataTrans2);
00150   }
00151 }
00152 
00153 void odomBefBAHandler(const nav_msgs::Odometry::ConstPtr& odomBefBA)
00154 {
00155   timeOdomBefBA = odomBefBA->header.stamp.toSec();
00156 
00157   double roll, pitch, yaw;
00158   geometry_msgs::Quaternion geoQuat = odomBefBA->pose.pose.orientation;
00159   tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw);
00160 
00161   transformBefBA[0] = pitch;
00162   transformBefBA[1] = yaw;
00163   transformBefBA[2] = roll;
00164 
00165   transformBefBA[3] = odomBefBA->pose.pose.position.x;
00166   transformBefBA[4] = odomBefBA->pose.pose.position.y;
00167   transformBefBA[5] = odomBefBA->pose.pose.position.z;
00168 }
00169 
00170 void odomAftBAHandler(const nav_msgs::Odometry::ConstPtr& odomAftBA)
00171 {
00172   timeOdomAftBA = odomAftBA->header.stamp.toSec();
00173 
00174   double roll, pitch, yaw;
00175   geometry_msgs::Quaternion geoQuat = odomAftBA->pose.pose.orientation;
00176   tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw);
00177 
00178   transformAftBA[0] = pitch;
00179   transformAftBA[1] = yaw;
00180   transformAftBA[2] = roll;
00181 
00182   transformAftBA[3] = odomAftBA->pose.pose.position.x;
00183   transformAftBA[4] = odomAftBA->pose.pose.position.y;
00184   transformAftBA[5] = odomAftBA->pose.pose.position.z;
00185 }
00186 
00187 int main(int argc, char** argv)
00188 {
00189   ros::init(argc, argv, "transformMaintenance");
00190   ros::NodeHandle nh;
00191 
00192   ros::Subscriber voDataSub = nh.subscribe<nav_msgs::Odometry> 
00193                               ("/cam_to_init", 1, voDataHandler);
00194 
00195   ros::Subscriber odomBefBASub = nh.subscribe<nav_msgs::Odometry> 
00196                                  ("/bef_ba_to_init", 1, odomBefBAHandler);
00197 
00198   ros::Subscriber odomAftBASub = nh.subscribe<nav_msgs::Odometry> 
00199                                  ("/aft_ba_to_init", 1, odomAftBAHandler);
00200 
00201   ros::Publisher voData2Pub = nh.advertise<nav_msgs::Odometry> ("/cam2_to_init", 1);
00202   voData2PubPointer = &voData2Pub;
00203   voData2.header.frame_id = "/camera_init";
00204   voData2.child_frame_id = "/camera2";
00205 
00206   tf::TransformBroadcaster tfBroadcaster2;
00207   tfBroadcaster2Pointer = &tfBroadcaster2;
00208   voDataTrans2.frame_id_ = "/camera_init";
00209   voDataTrans2.child_frame_id_ = "/camera2";
00210 
00211   ros::spin();
00212 
00213   return 0;
00214 }


demo_rgbd
Author(s): Ji Zhang
autogenerated on Sun Oct 5 2014 23:25:12