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
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)
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
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)
00209 {
00210 fprintf(fid,"%d\t%f\t%f\t%f\t%f\t%f\t%f\t%f\n",cubes[i].id/10,
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)
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 }