PTAMVisualizer.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <ptam_com/PointCloud.h>
00003 #include <ptam_com/KeyFrame_srv.h>
00004 #include <ptam_com/KeyFrame_msg.h>
00005 #include <sensor_msgs/PointCloud2.h>
00006 #include <ptam/PTAMVisualizerParamsConfig.h>
00007 #include <dynamic_reconfigure/server.h>
00008 #include <visualization_msgs/Marker.h>
00009 #include <ptam/AxesArray.h>
00010 
00011 #include <unistd.h>
00012 #include <sys/types.h>
00013 #include <pwd.h>
00014 
00015 
00016 typedef dynamic_reconfigure::Server<ptam::PTAMVisualizerParamsConfig> ReconfigureServer;
00017 ReconfigureServer *reconfServer_;
00018 
00019 // global variables
00020 bool show_pc_;
00021 bool show_kfs_;
00022 bool show_all_kfs_;
00023 unsigned int kf_lifetime_;
00024 bool show_path_;
00025 unsigned int path_length_;
00026 sensor_msgs::PointCloud2* pPC2;
00027 struct passwd *pw;
00028 int KFFlags_;
00029 unsigned int lastKFid;
00030 visualization_msgs::Marker path;
00031 ros::Publisher pub_path;
00032 AxesArray tripods;
00033 AxesArray tripodshistory;
00034 
00035 void Config(ptam::PTAMVisualizerParamsConfig &config, uint32_t level);
00036 void exportPC(std::string prefix);
00037 void pathCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr & msg);
00038 
00039 
00040 int main(int argc, char **argv)
00041 {
00042   ros::init(argc, argv, "ptam_visualizer");
00043   ros::NodeHandle n;
00044   ros::ServiceClient PC2client = n.serviceClient<ptam_com::PointCloud>("vslam/pointcloud");
00045   ros::ServiceClient KFclient = n.serviceClient<ptam_com::KeyFrame_srv>("vslam/keyframes");
00046   ros::Publisher pub_cloud = n.advertise<sensor_msgs::PointCloud2> ("vslam/pc2", 1);
00047   ros::Publisher pub_kfs = n.advertise<ptam_com::KeyFrame_msg> ("vslam/kfs",1);
00048   ros::Publisher pub_marker = n.advertise<visualization_msgs::MarkerArray>("vslam/kf_visualization_array", 1);
00049   pub_path = n.advertise<visualization_msgs::Marker>("vslam/path_visualization", 1);
00050 
00051   ros::Subscriber sub_path = n.subscribe("vslam/pose",1,&pathCallback);
00052 
00053   ptam_com::PointCloud srv_pc2;
00054   ptam_com::KeyFrame_srv srv_kfs;
00055   unsigned int pathidx=0;
00056 
00057   pw = getpwuid(getuid());
00058 
00059   reconfServer_ = new ReconfigureServer(ros::NodeHandle("~"));
00060   ReconfigureServer::CallbackType f = boost::bind(&Config , _1, _2);
00061   reconfServer_->setCallback(f);
00062 
00063   pPC2=NULL;
00064 
00065   srv_pc2.request.flags = 0;
00066   lastKFid=0;
00067   tripodshistory.init(1);
00068 
00069   path.id=0;
00070   path.lifetime=ros::Duration(1);
00071   path.header.frame_id = "/world";
00072   path.header.stamp = ros::Time::now();
00073   path.ns = "pointcloud_publisher";
00074   path.action = visualization_msgs::Marker::ADD;
00075   path.type = visualization_msgs::Marker::LINE_STRIP;
00076   path.color.r=1.0;
00077   path.color.g=1.0;
00078   path.color.a=1.0;
00079   path.scale.x=0.01;
00080   path.pose.orientation.w=1.0;
00081 
00082   while(ros::ok())
00083   {
00084     visualization_msgs::MarkerArray cubes;
00085     if(show_pc_)
00086       if(PC2client.call(srv_pc2))
00087       {
00088         pPC2=&(srv_pc2.response.pointcloud);
00089         pub_cloud.publish(srv_pc2.response.pointcloud);
00090       }
00091     if(KFFlags_==1)
00092       srv_kfs.request.flags=lastKFid;
00093     else
00094       srv_kfs.request.flags = KFFlags_;
00095 
00096     if(KFclient.call(srv_kfs))
00097     {
00098       pub_kfs.publish(srv_kfs.response);
00099 
00100       tripods.init(kf_lifetime_);
00101       double pos[3], att[4];
00102       for(int i=srv_kfs.response.KFids.size()-1;!(i<0);--i)     //first element is newest KF
00103       {
00104         memcpy(pos,&(srv_kfs.response.KFs[i].pose.pose.position.x),sizeof(double)*3);
00105         att[0] = srv_kfs.response.KFs[i].pose.pose.orientation.w;
00106         memcpy(att+1,&(srv_kfs.response.KFs[i].pose.pose.orientation.x),sizeof(double)*3);
00107         tripods.addAxes(pos,att,srv_kfs.response.KFids[i]);
00108         if(lastKFid<=srv_kfs.response.KFids[i])
00109         {
00110           tripodshistory.addAxes(pos,att,srv_kfs.response.KFids[i]);
00111           lastKFid=srv_kfs.response.KFids[i]+1;
00112         }
00113       }
00114     }
00115     else
00116     {
00117       //reset if there is no map
00118       tripods.clearAxes();
00119       tripodshistory.clearAxes();
00120       path.points.clear();
00121       pathidx=0;
00122     }
00123 
00124     if(show_kfs_ & !show_all_kfs_)
00125     {
00126       cubes = tripods.getAxes();
00127       pub_marker.publish(cubes);
00128     }
00129 
00130     if(show_all_kfs_)
00131     {
00132       cubes = tripodshistory.getAxes();
00133       pub_marker.publish(cubes);
00134     }
00135 
00136     if(show_path_)
00137       pub_path.publish(path);
00138 
00139     usleep(1e6);
00140     ros::spinOnce();
00141   }
00142 
00143   delete reconfServer_;
00144   return 0;
00145 }
00146 
00147 void pathCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr & msg)
00148 {
00149   double pos[3], att[4];
00150   memcpy(pos,&(msg->pose.pose.position.x),sizeof(double)*3);
00151   att[0] = msg->pose.pose.orientation.w;
00152   memcpy(att+1,&(msg->pose.pose.orientation.x),sizeof(double)*3);
00153 
00154   TooN::Vector<3> center = tripods.getCenter(pos,att);
00155   geometry_msgs::Point p;
00156   memcpy(&(p.x),&(center[0]),sizeof(double)*3);
00157   path.points.push_back(p);
00158   while(path.points.size()>path_length_)
00159     path.points.erase(path.points.begin());
00160 
00161 }
00162 
00163 void exportPC(std::string prefix)
00164 {
00165   FILE* fid;
00166   std::string strnsec;
00167   std::stringstream out;
00168   out << ros::Time::now().nsec;
00169   strnsec = out.str();
00170 
00171   std::string slash ="/";
00172   std::string strpc ="_PC";
00173   std::string filename = pw->pw_dir+slash+prefix+strnsec+strpc;
00174   fid = fopen(filename.c_str(),"w");
00175   if(fid!=NULL && pPC2!=NULL)
00176   {
00177     for(unsigned int i=0;i<pPC2->width;++i)
00178     {
00179       float* elemf = (float*)&(pPC2->data[i*pPC2->point_step]);
00180       int* elemi = (int*)&(pPC2->data[i*pPC2->point_step+3*sizeof(float)]);
00181       fprintf(fid,"%f\t%f\t%f\t%d\t%d\t%d\n",*elemf,*(elemf+1),*(elemf+2),*(elemi),*(elemi+1),*(elemi+2));
00182     }
00183     fclose(fid);
00184     ROS_INFO_STREAM("Point cloud exported to: " << filename);
00185   }
00186   else if(fid==NULL)
00187     ROS_WARN_STREAM("could not open file: " << filename);
00188   else
00189     ROS_WARN_STREAM("Could not export point cloud to file. Do we actually have one to export?");
00190 }
00191 
00192 void saveMap(std::string prefix)
00193 {
00194   FILE* fid;
00195   std::string strnsec;
00196   std::stringstream out;
00197   out << ros::Time::now().nsec;
00198   strnsec = out.str();
00199 
00200   std::string slash ="/";
00201   std::string strkf ="_KF";
00202   std::string filename = pw->pw_dir+slash+prefix+strnsec+strkf;
00203   fid = fopen(filename.c_str(),"w");
00204   if((fid!=NULL) & (pPC2!=NULL) & (tripodshistory.getAxes().markers.size()>0) & (path.points.size()>0))
00205   {
00206 
00207     std::vector<visualization_msgs::Marker> cubes = tripodshistory.getAxes().markers;
00208     for(unsigned int i=0;i<cubes.size();i+=3)   // 3 markers per tripod
00209     {
00210       fprintf(fid,"%d\t%f\t%f\t%f\t%f\t%f\t%f\t%f\n",cubes[i].id/10,    // ID is stored by *10 since for each tripod there are 3 markers
00211               cubes[i].pose.position.x,cubes[i].pose.position.y,cubes[i].pose.position.z,
00212               cubes[i].pose.orientation.w,cubes[i].pose.orientation.x,cubes[i].pose.orientation.y,cubes[i].pose.orientation.z);
00213     }
00214     fclose(fid);
00215     ROS_INFO_STREAM("KFs exported to: " << filename);
00216 
00217     std::string strpath ="_Path";
00218     filename = pw->pw_dir+slash+prefix+strnsec+strpath;
00219     fid = fopen(filename.c_str(),"w");
00220     for(unsigned int i=0;i<path.points.size();++i)      // 3 markers per tripod
00221     {
00222       fprintf(fid,"%f\t%f\t%f\n",path.points[i].x,path.points[i].y,path.points[i].z);
00223     }
00224     fclose(fid);
00225     ROS_INFO_STREAM("Path exported to: " << filename);
00226 
00227     std::string strpc ="_PC";
00228     filename = pw->pw_dir+slash+prefix+strnsec+strpc;
00229     fid = fopen(filename.c_str(),"w");
00230     for(unsigned int i=0;i<pPC2->width;++i)
00231     {
00232       float* elemf = (float*)&(pPC2->data[i*pPC2->point_step]);
00233       int* elemi = (int*)&(pPC2->data[i*pPC2->point_step+3*sizeof(float)]);
00234       fprintf(fid,"%f\t%f\t%f\t%d\t%d\t%d\n",*elemf,*(elemf+1),*(elemf+2),*(elemi),*(elemi+1),*(elemi+2));
00235     }
00236     fclose(fid);
00237     ROS_INFO_STREAM("Point cloud exported to: " << filename);
00238   }
00239   else if(fid==NULL)
00240     ROS_WARN_STREAM("could not open file: " << filename);
00241   else
00242     ROS_WARN_STREAM("Could not export KFs and path to file. Do we actually have one to export?");
00243 }
00244 
00245 void Config(ptam::PTAMVisualizerParamsConfig& config, uint32_t level)
00246 {
00247   show_pc_ = config.ShowPC;
00248   show_kfs_ = config.ShowKFs;
00249   show_all_kfs_ = config.ShowAllKFs;
00250   kf_lifetime_ = config.KFLifetime;
00251   show_path_ = config.ShowPath;
00252   KFFlags_ = config.KFFlags;
00253   path_length_ = config.PathLength;
00254   if(config.ExportPC)
00255   {
00256     config.ExportPC=false;
00257     exportPC(config.ExportPrefix);
00258   }
00259   if(config.SaveMap)
00260   {
00261     config.SaveMap=false;
00262     saveMap(config.ExportPrefix);
00263   }
00264 }


ptam
Author(s): Markus Achtelik , Stephan Weiss , Simon Lynen
autogenerated on Sun Oct 5 2014 23:52:33