transform_finder.cpp
Go to the documentation of this file.
00001 // yliu 
00002 // transform_finder
00003 // urdf_cam_frame | new_cam_frame | mounting_frame
00004 // 06/23/2011
00005 // 08/03/2011
00006 
00007 #include "ros/ros.h"
00008 #include "ros/topic.h"
00009 #include <iostream>
00010 #include "std_msgs/String.h"
00011 #include <string>
00012 #include <fstream>
00013 #include <tf/transform_datatypes.h>
00014 #include <sstream>
00015 #include <map>
00016 #include <camera_pose_calibration/CameraCalibration.h>
00017 #include <kdl_parser/kdl_parser.hpp>
00018 #include <sensor_msgs/CameraInfo.h>
00019 #include <sensor_msgs/JointState.h>
00020 #include <geometry_msgs/TransformStamped.h>
00021 #include <robot_state_publisher/robot_state_publisher.h>
00022 #include <kdl/frames.hpp>
00023 #include <boost/regex.hpp>
00024 #include <tf/transform_listener.h>
00025 #include <time.h>
00026 #include <boost/bind.hpp>
00027 #include <math.h>
00028 #include <iomanip>
00029 #include <vector>
00030 
00031 
00032 
00033 
00034 
00035 class T_Finder
00036 {
00037 public:
00038 
00039 
00040 ros::NodeHandle n;
00041 tf::TransformListener listener; // need some time to build up internal buffer
00042 ros::Publisher pub;
00043 //ros::Publisher reset_pub; 
00044 //ros::ServiceClient reset_client;
00045 ros::ServiceClient monitor_client;
00046 
00047 std::string new_cam_ns; //camera name space
00048 std::string urdf_cam_ns;
00049 std::string new_cam_id; //camera frame id
00050 std::string urdf_cam_id;
00051 // **
00052 // **
00053 // **
00054 std::string mounting_frame;
00055 tf::StampedTransform prev_s_transform;
00056 //int measurement_count;
00057 int printOnce_f;
00058 int printOnce_t;
00059 int printOnce_a;
00060 std::string f_log_file;
00061 std::string t_log_file;
00062 std::string avg_res_file;
00063 
00064 
00065 ros::Time prev_stamp0;
00066 bool is_1st_time;
00067 bool reset_flag;
00068 std::vector<KDL::Twist> best_prev_twists; // best twists for different tf configurations between mountin frame and urdf cam frame.
00069 std::vector<KDL::Frame> best_prev_frames; 
00070 std::vector<int> prev_weights; 
00071 std::vector<KDL::Twist>::iterator it;
00072 std::vector<KDL::Twist> best_prev_twists_rel1; // best previous twists relative to the the first best twist. 
00073 
00074 KDL::Twist prev_twist;
00075 KDL::Frame prev_frame;
00076 int prev_weight;
00077 double prev_p[3];
00078 double prev_rpy[3];
00079 int callback_count;
00080 
00081 //
00082 std::string prev_new_cam_id;
00083 std::string prev_urdf_cam_id;
00084 std::string prev_mounting_frame;
00085 
00086 
00087 void MyCallback(const camera_pose_calibration::CameraCalibration::ConstPtr& msg)
00088 {
00089 
00090 
00091         n.getParam("new_cam_ns", new_cam_ns);
00092         n.getParam("urdf_cam_ns", urdf_cam_ns);
00093 
00094         std::string tp1;
00095         tp1 = new_cam_ns + "/camera_info";
00096         sensor_msgs::CameraInfo::ConstPtr msg1 = ros::topic::waitForMessage<sensor_msgs::CameraInfo>(tp1,n);
00097         new_cam_id = msg1->header.frame_id;
00098 
00099         std::string tp2;
00100         tp2 = urdf_cam_ns + "/camera_info";
00101         sensor_msgs::CameraInfo::ConstPtr msg2 = ros::topic::waitForMessage<sensor_msgs::CameraInfo>(tp2, n);
00102         urdf_cam_id = msg2->header.frame_id;
00103         
00104         n.getParam("mounting_frame", mounting_frame);
00105 
00106         printf("New camera frame: %s\n", new_cam_id.c_str());
00107         printf("New camera's mounting frame: %s\n", mounting_frame.c_str());
00108         printf("Urdf camera frame: %s\n", urdf_cam_id.c_str());
00109         
00110         // ------------------------------------------------
00111 
00112         // reset the lists if the target frames are found different from last time
00113         if (callback_count > 0 )
00114         {
00115                 if (  (prev_new_cam_id != new_cam_id) || (prev_mounting_frame != mounting_frame) || (prev_urdf_cam_id != urdf_cam_id) )
00116                 {
00117                         best_prev_twists.clear();
00118                         best_prev_frames.clear();
00119                         best_prev_twists_rel1.clear();
00120                         prev_weights.clear();
00121                         callback_count = 0;
00122                         is_1st_time = true;
00123                         printOnce_f = 1;
00124                         printOnce_t = 1;
00125                         printOnce_a = 1;
00126 
00127                         printf("Note: change of frames!\n ");
00128                 }
00129         }
00130 
00131 
00132 
00133 
00134         callback_count++;
00135         printf("Calibration: \033[32;1m%d\033[0m\n", callback_count);   
00136         
00137         printf("--%f\n", msg->time_stamp[0].toSec());
00138         printf("--%f\n", prev_stamp0.toSec());
00139         if ( (msg->time_stamp[0].toSec() != prev_stamp0.toSec()) && (is_1st_time == false) )   // check if the cache for previous measurements has been cleared.
00140         {
00141                 reset_flag = true;
00142         }
00143         //prev_stamp0.sec = msg->time_stamp[0].sec;
00144         //prev_stamp0.nsec = msg->time_stamp[0].nsec;
00145         prev_stamp0 = msg->time_stamp[0];
00146 
00147         ros::Time TheMoment = msg->time_stamp[msg->m_count-1];
00148         
00149         //--------------------------------------------------------
00150 
00151         tf::StampedTransform s_transform;
00152         int TransformFound = 0; 
00153         
00154         while(TransformFound==0)
00155         {       
00156                 TransformFound=1;
00157                 try{
00158                         // get the transform | from, to
00159                         listener.lookupTransform(mounting_frame, urdf_cam_id, TheMoment, s_transform);
00160                 }
00161                 catch (tf::TransformException ex){ 
00162                         //ROS_ERROR("%s",ex.what());
00163                         printf(".");
00164                         TransformFound=0;
00165                 }
00166         }
00167         
00168         
00169 
00170         
00171 
00172 
00173         printf("----\n");
00174 
00175         KDL::Rotation R1; // the rotation of the urdf cam frame w.r.t. the link frame on which the new sensor is mounted.
00176         KDL::Vector p1;  // the origin displacement of the urdf cam frame w.r.t to the link frame on which the new sensor is mounted.
00177         p1.x(s_transform.getOrigin().x());
00178         p1.y(s_transform.getOrigin().y());
00179         p1.z(s_transform.getOrigin().z());
00180 
00181         //printf("p1 is [%f, %f, %f]\n\n", p1.x(), p1.y(), p1.z());
00182 
00183         // Static public member function of KDL::Rotation
00184         R1 = KDL::Rotation::Quaternion(s_transform.getRotation().x(),
00185                                        s_transform.getRotation().y(),
00186                                        s_transform.getRotation().z(),
00187                                        s_transform.getRotation().w());  //radian
00188 
00189         //-debug-
00190         printf("TF Quaternion:\n %10f, %10f, %10f, %10f\n", s_transform.getRotation().x(), s_transform.getRotation().y(), s_transform.getRotation().z(), s_transform.getRotation().w() );
00191 
00192         //--------------------------------------------------------
00193 
00194 
00195         int cnt = msg->camera_id.size();
00196         //printf("Number of cameras received: %d\n", cnt);
00197         KDL::Rotation CamR[cnt];
00198         int new_cam_index = -1;
00199         int urdf_cam_index = -1;
00200         KDL::Rotation R2; // the rotation of the new cam w.r.t. the urdf cam
00201         KDL::Vector p2;  // the origin displacement of the new cam w.r.t to the calibrated (urdf) cam
00202 
00203 
00204         for (int i=0; i<cnt; i++)
00205         {
00206                 //printf("\n");
00207                 //printf("camera_id  :%s", msg->camera_id[i].c_str());
00208 
00209                 if (msg->camera_id[i].compare(new_cam_id) == 0)
00210                 {
00211                         //printf("  --->  new camera ");
00212                         new_cam_index = i;
00213                 }
00214                 //
00215                 if (msg->camera_id[i].compare(urdf_cam_id) == 0)
00216                 {
00217                         //printf("  --->  urdf camera ");
00218                         urdf_cam_index = i;
00219                 }
00220                 //
00221                 //printf("\n");
00222 
00223                 CamR[i]=KDL::Rotation::Quaternion(msg->camera_pose[i].orientation.x, msg->camera_pose[i].orientation.y, msg->camera_pose[i].orientation.z, msg->camera_pose[i].orientation.w); // KDL::Rotation objects
00224                 
00225                 //-debug-
00226                 printf("CC-%d Quaternion:\n %10f, %10f, %10f, %10f\n",i, msg->camera_pose[i].orientation.x, msg->camera_pose[i].orientation.y, msg->camera_pose[i].orientation.z, msg->camera_pose[i].orientation.w );
00227 
00228 
00229         }
00230 
00231         printf("\n");
00232 
00233         
00234         R2 = CamR[urdf_cam_index].Inverse() * CamR[new_cam_index];
00235         p2.x(msg->camera_pose[new_cam_index].position.x - msg->camera_pose[urdf_cam_index].position.x);
00236         p2.y(msg->camera_pose[new_cam_index].position.y - msg->camera_pose[urdf_cam_index].position.y);
00237         p2.z(msg->camera_pose[new_cam_index].position.z - msg->camera_pose[urdf_cam_index].position.z);
00238         //printf("p2' is [%f, %f, %f]\n\n", p2.x(), p2.y(), p2.z());
00239         p2 = CamR[urdf_cam_index].Inverse() * p2;
00240         //printf("p2 is [%f, %f, %f]\n\n", p2.x(), p2.y(), p2.z());
00241 
00242         //--------------------------------------------------------
00243         KDL::Rotation R; 
00244         KDL::Vector p;  
00245         double kRoll, kPitch, kYaw;
00246         double px, py, pz;
00247         double qx,qy,qz,qw;
00248         R =    R1 * R2 ;     // single pose best
00249         p = p1 + R1 * p2;    // single pose best
00250         px = p.x(); py = p.y(); pz = p.z(); 
00251         R.GetRPY(kRoll, kPitch, kYaw);
00252         R.GetQuaternion( qx,qy,qz,qw);
00253 
00254         //-debug-
00255         printf("Combined Quaternion:\n %10f, %10f, %10f, %10f \n\n",qx,qy,qz,qw);
00256         printf("Rotation M:\n %10f, %10f, %10f, \n %10f, %10f, %10f \n %10f, %10f, %10f \n", R.data[0],R.data[1],R.data[2],   R.data[3],R.data[4],R.data[5],   R.data[6],R.data[7],R.data[8] );
00257 
00258         //--------------------------------------------------------
00259         // Convert to twist, so we can average bests from different urdf-cam-frame/mounting-frame configurations
00260         KDL::Frame fm = KDL::Frame(R, p);
00261         KDL::Twist tw;
00262         tw = KDL::diff(KDL::Frame::Identity(), fm);
00263         //if ( is_1st_time == false )
00264         //{
00265         //      if (tw.rot.data[0]*prev_twist.rot.data[0]<0)  // regulate the sign, since we need to perform averaging later.
00266         //      {
00267         //              tw.rot.data[0] = -tw.rot.data[0];
00268         //              tw.rot.data[1] = -tw.rot.data[1];
00269         //              tw.rot.data[2] = -tw.rot.data[2];
00270         //      }
00271         //}
00272         double vx, vy, vz, wx, wy, wz;
00273         vx = tw.vel.data[0];
00274         vy = tw.vel.data[1];
00275         vz = tw.vel.data[2];
00276         wx = tw.rot.data[0];
00277         wy = tw.rot.data[1];
00278         wz = tw.rot.data[2];
00279 
00280         //--------------------------------------------------------
00281         // logging 
00282         printf("\nlogging... (.ros/)\n");
00283                 
00284         time_t rawtime;
00285         struct tm * timeinfo;
00286         time(&rawtime);
00287         timeinfo = localtime(&rawtime);
00288          
00289         
00290         std::ofstream fwriter(f_log_file.c_str(), std::ios::out | std::ios::app);  // append 
00291         std::ofstream twriter(t_log_file.c_str(), std::ios::out | std::ios::app);   
00292         
00293 
00294         if (fwriter.is_open())
00295         {
00296                 if (reset_flag == true)
00297                 {
00298                         fwriter<<"--urdf_cam_frame moved, measurement cache reset!--"<<std::endl;
00299                 }
00300 
00301                 if (    printOnce_f == 1 )
00302                 {
00303                         fwriter<<"----------------------"<<std::endl;
00304                         fwriter<<"parent_frame: "<< mounting_frame <<std::endl;
00305                         fwriter<<"child_frame:  "<< new_cam_id <<std::endl;
00306                         printOnce_f = 0;
00307                 }
00308         
00309                 fwriter<< std::setw(3) << callback_count<<". " << timeinfo->tm_year+1900<<"-"
00310                        << std::setfill('0') << std::setw(2) << timeinfo->tm_mon+1<<"-"
00311                        << std::setfill('0') << std::setw(2) << timeinfo->tm_mday <<" "
00312                        << std::setfill('0') << std::setw(2) << timeinfo->tm_hour
00313                        <<":"<< std::setfill('0') << std::setw(2) << timeinfo->tm_min
00314                        <<":"<< std::setfill('0') << std::setw(2) << timeinfo->tm_sec;
00315                 
00316                 fwriter<<std::setfill(' ');
00317                 fwriter<< " p:"<<std::setw(15)<< px
00318                        <<std::setw(15)<< py
00319                        <<std::setw(15)<< pz<< "      Q:";
00320                 fwriter<<std::setw(15)<< qx
00321                        <<std::setw(15)<< qy 
00322                        <<std::setw(15)<< qz
00323                        <<std::setw(15)<< qw<<std::endl;
00324         }
00325         else
00326         {
00327                 printf("Error opening file.\n\n");
00328         }
00329         
00330 
00331         if (twriter.is_open())
00332         {
00333                 if (reset_flag == true)
00334                 {
00335                         twriter<<"--urdf_cam_frame moved, measurement cache reset!--"<<std::endl;
00336                 }       
00337 
00338                 if (  printOnce_t == 1  )
00339                 {
00340                         twriter<<"----------------------"<<std::endl;
00341                         twriter<<"parent_frame: "<< mounting_frame <<std::endl;
00342                         twriter<<"child_frame:  "<< new_cam_id <<std::endl;
00343                         printOnce_t = 0;
00344                 }
00345 
00346                 twriter<< std::setw(3) << callback_count<<". "<< timeinfo->tm_year+1900<<"-"
00347                        << std::setfill('0') << std::setw(2) << timeinfo->tm_mon+1<<"-"
00348                        << std::setfill('0') << std::setw(2) << timeinfo->tm_mday <<" "
00349                        << std::setfill('0') << std::setw(2) <<timeinfo->tm_hour
00350                        <<":"<< std::setfill ('0') << std::setw(2) <<timeinfo->tm_min
00351                        <<":"<< std::setfill ('0') << std::setw(2) <<timeinfo->tm_sec;
00352                 
00353                 twriter<<std::setfill(' ');
00354                 twriter<< " v:"<< std::setw(15)<<vx
00355                        <<std::setw(15)<<vy
00356                        <<std::setw(15)<<vz << "      w:"
00357                        <<std::setw(15)<<wx
00358                        <<std::setw(15)<<wy
00359                        <<std::setw(15)<<wz<<std::endl;
00360         }
00361         else
00362         {
00363                 printf("Error opening file. \n\n");
00364         }
00365 
00366         twriter.close();
00367         fwriter.close();
00368 
00369 
00370 
00371         
00372 
00373         //--------------------------------------------------------
00374         // Averaging 
00375 
00376 
00377 
00378 
00379         if ( reset_flag == true)
00380         {
00381                 best_prev_twists.push_back(prev_twist);
00382                 best_prev_frames.push_back(prev_frame);
00383                 
00384                 // 'rel1' means relative to the 1st calibration result
00385 
00386                 KDL::Twist prev_twist_rel1 = KDL::diff(     best_prev_frames[0], prev_frame);
00388                 //KDL::Twist prev_twist_rel1 = KDL::diff(     best_prev_twists[0],
00389                 //                                          prev_twist  );
00390                 
00391                 
00392 
00393                 printf("rel_to_1: %10f, %10f, %10f, %10f, %10f, %10f\n", prev_twist_rel1.vel.data[0], prev_twist_rel1.vel.data[1], prev_twist_rel1.vel.data[2], prev_twist_rel1.rot.data[0], prev_twist_rel1.rot.data[1], prev_twist_rel1.rot.data[2]);
00394                 best_prev_twists_rel1.push_back(prev_twist_rel1);
00395                 
00396                 //weights
00397                 prev_weights.push_back(prev_weight);            
00398 
00399                 std::ofstream iwriter("debug_info", std::ios::app); 
00400                 if (iwriter.is_open()) // i for intermediate
00401                 {
00402 
00403                         iwriter<<"F"<< best_prev_frames.size() <<":     "<<" p:"<< std::setw(15)<<px
00404                                           << std::setw(15)<<py
00405                                           << std::setw(15)<<pz <<"      Q:"
00406                                           << std::setw(15)<<qx
00407                                           << std::setw(15)<<qy
00408                                           << std::setw(15)<<qz
00409                                           << std::setw(15)<<qw <<"    |    weight:"<<prev_weight<<std::endl;
00410                         
00411                 }
00412                 else
00413                 {
00414                         printf("Error opening file.\n\n");
00415                 }
00416                 iwriter.close();
00417                 
00418         }
00419 
00420 
00421 
00422 
00423 
00424         if ( best_prev_twists.size() > 0)
00425         {       
00426                 double rsum[6] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
00427                 int c = (int)best_prev_twists.size();
00428                 
00429                 double prev_total_w = 0.0;
00430                 for ( int k = 0; k < c; k++)
00431                 {
00432                         prev_total_w += (double)prev_weights[k];
00433                 }
00434         
00435                 for ( int i = 0; i < c; i++)
00436                 {
00437                         double w = prev_weights[i];
00438                         // w = 1;
00439                         rsum[0] += best_prev_twists_rel1[i].vel.data[0]*w;
00440                         rsum[1] += best_prev_twists_rel1[i].vel.data[1]*w;
00441                         rsum[2] += best_prev_twists_rel1[i].vel.data[2]*w;
00442                         rsum[3] += best_prev_twists_rel1[i].rot.data[0]*w;
00443                         rsum[4] += best_prev_twists_rel1[i].rot.data[1]*w;
00444                         rsum[5] += best_prev_twists_rel1[i].rot.data[2]*w;                      
00445                 }       
00446 
00447 
00448                 
00449                 double c_w = (double)msg->m_count;
00450                 double total_w = prev_total_w + c_w;
00451 
00452                 KDL::Twist tw_rel1 = KDL::diff(best_prev_frames[0], fm);  // !!         
00453  
00454 
00455                 KDL::Twist tw_av_rel1 = KDL::Twist(KDL::Vector( (rsum[0] + tw_rel1.vel.data[0]*c_w)/total_w,  (rsum[1] + tw_rel1.vel.data[1]*c_w)/total_w,  (rsum[2] + tw_rel1.vel.data[2]*c_w)/total_w), 
00456                                                    KDL::Vector( (rsum[3] + tw_rel1.rot.data[0]*c_w)/total_w,  (rsum[4] + tw_rel1.rot.data[1]*c_w)/total_w,  (rsum[5] + tw_rel1.rot.data[2]*c_w)/total_w ) ); //?
00457                 KDL::Twist tw_av = KDL::addDelta(best_prev_twists[0], tw_av_rel1);  //dt is always 1.
00458                 
00459                 KDL::Frame frame_av = KDL::addDelta(KDL::Frame::Identity(), tw_av);
00460                 KDL::Rotation R_av = frame_av.M;
00461                 KDL::Vector p_av = frame_av.p; 
00462 
00463                 px = p_av.x(); py = p_av.y(); pz = p_av.z(); 
00464                 R_av.GetRPY(kRoll, kPitch, kYaw);
00465                 R_av.GetQuaternion( qx,qy,qz,qw);
00466 
00467         }
00468 
00469         //-----------------------------------------------------
00470         std::ofstream awriter(avg_res_file.c_str(), std::ios::out | std::ios::app); 
00471         if (awriter.is_open())
00472         {
00473                 if (reset_flag == true)
00474                 {
00475                         awriter<<"urdf_cam_frame movded, measurement cache reset!"<<std::endl;
00476                 }       
00477                 if (printOnce_a == 1)
00478                 {
00479                         awriter<<"----------------------"<<std::endl;
00480                         awriter<<"parent_frame: "<< mounting_frame <<std::endl;
00481                         awriter<<"child_frame:  "<< new_cam_id <<std::endl;
00482                         printOnce_a = 0;
00483                 }       
00484                 awriter<<  std::setw(3) << callback_count<<". ";
00485                 
00486                 awriter<<" p:"<<std::setw(15)<< px
00487                        <<std::setw(15)<< py
00488                        <<std::setw(15)<< pz <<"      Q:";
00489                 awriter<<std::setw(15)<< qx
00490                        <<std::setw(15)<< qy 
00491                        <<std::setw(15)<< qz
00492                        <<std::setw(15)<< qw<<std::endl;
00493         }
00494         else
00495         {
00496                 printf("Error opening file. \n\n");
00497         }
00498         awriter.close();
00499 
00500 
00501         prev_twist = tw;
00502         prev_frame = fm;
00503         prev_p[0]=p[0]; prev_p[1]=p[1]; prev_p[2]=p[2];
00504         prev_rpy[0]=kRoll; prev_rpy[1]=kPitch; prev_rpy[2]= kYaw;
00505 
00506         //--------------------------------------------------------
00507         
00508 
00509 
00510         //--------------------------------------------------------
00511         // publish updated static transform information 
00512                 
00513         geometry_msgs::TransformStamped T;
00514         T.transform.translation.x = px;
00515         T.transform.translation.y = py;
00516         T.transform.translation.z = pz;
00517 
00518         T.transform.rotation.x = qx;
00519         T.transform.rotation.y = qy;
00520         T.transform.rotation.z = qz; 
00521         T.transform.rotation.w = qw;
00522 
00523         T.header.frame_id = mounting_frame;
00524         T.child_frame_id = new_cam_id;
00525         pub.publish( T );
00526 
00527 
00528         //--------------------------------------------------------
00529 
00530 
00531 
00532 
00533         //----------------------------------------------------------------
00534         is_1st_time = false;
00535         reset_flag = false;
00536         prev_weight = msg->m_count;
00537 
00538 
00539         //----------------------------------------------------------------
00540         prev_new_cam_id = new_cam_id;
00541         prev_urdf_cam_id = urdf_cam_id;
00542         prev_mounting_frame = mounting_frame;
00543 
00544 
00545         //----------------------------------------------------------------
00546         printf("Done.\n");
00547 
00548 }
00549 
00550 };
00551 
00552 
00553 
00554 
00555 
00556 
00557 
00558 int main(int argc, char **argv)
00559 {
00560         ros::init(argc, argv, "transform_finder");
00561         
00562         T_Finder my_finder;
00563 
00564         //my_finder.measurement_count = 0;
00565         my_finder.prev_s_transform.setOrigin(tf::Vector3(0.0, 0.0, 0.0));
00566         my_finder.prev_s_transform.setRotation( tf::Quaternion(1.0, 0.0, 0.0, 0.0));
00567                 
00568 
00569 
00570         // ***
00571 
00572 
00573         my_finder.prev_new_cam_id = "";
00574         my_finder.prev_urdf_cam_id = "";
00575         my_finder.prev_mounting_frame = "";
00576 
00577         // ** 
00578         // **
00579 
00580         printf("\n\033[32;1m transform_finder ready...\033[0m\n\n");
00581 
00582 
00583         my_finder.printOnce_f = 1;
00584         my_finder.printOnce_t = 1;
00585         my_finder.printOnce_a = 1;
00586 
00587         my_finder.f_log_file = "calibration_log_frame";
00588         my_finder.t_log_file = "calibration_log_twist";
00589         my_finder.avg_res_file = "averaged_result";
00590         
00591         my_finder.prev_stamp0 = ros::Time();
00592         my_finder.is_1st_time = true;
00593         my_finder.reset_flag = false;
00594 
00595                
00596         std::ofstream iwriter("debug_info", std::ios::out | std::ios::trunc); 
00597         iwriter.close();
00598 
00599         std::ofstream awriter(my_finder.avg_res_file.c_str(), std::ios::out | std::ios::trunc); 
00600         awriter.close();
00601         
00602         my_finder.callback_count = 0;
00603 
00604 
00605         //---------------------------------------------------
00606         // **
00607 
00608 
00609         //---------------------------------------------------
00610         //reset_pub = n.advertise<std_msgs::Empty>("reset", 10);
00611 
00612         // ***
00613         //my_finder.monitor_client = my_finder.n.serviceClient<camera_pose_calibration::FramePair>("need_to_monitor_tf");
00614         //ros::Duration(0.2).sleep();
00615 
00616         //my_finder.reset_client = my_finder.n.serviceClient<camera_pose_calibration::Reset>("reset_measurement_history");
00617 
00618         my_finder.pub = my_finder.n.advertise<geometry_msgs::TransformStamped>("camera_pose_static_transform_update", 10); // this is to provide an immediate way to examine the newly added frame. 
00619         ros::Subscriber sub = my_finder.n.subscribe("camera_calibration", 1000, &T_Finder::MyCallback, &my_finder);
00620 
00621         //ros::Subscriber sub = my_finder.n.subscribe<camera_pose_calibration::CameraCalibration>("camera_calibration", 1000, boost::bind(&T_Finder::MyCallback, &my_finder, _1));
00624         ros::spin();
00625 
00626 
00627 
00628 
00629 
00630         return 0;
00631    }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


camera_pose_toolkits
Author(s): Yiping Liu
autogenerated on Thu Aug 15 2013 12:02:52