slam_exporter: slam_exporter: slam_exporter.cpp Source File

00001 #include <ros/ros.h>
00002 #include <sensor_msgs/PointCloud.h>
00003 #include <std_msgs/String.h>
00004 #include <sensor_msgs/PointCloud2.h>
00005 #include <tf/transform_listener.h>
00006 #include "globals.icc"
00007 #include <pcl_ros/transforms.h>
00008 
00009 #include <fstream>
00010 using std::ofstream;
00011 #include <iostream>
00012 using std::cout;
00013 using std::endl;
00014 
00015 tf::TransformListener *tl_;
00016 
00017 bool needRequest_, requested_;
00018 std::string fixed_frame_;
00019 std::string robot_frame_;
00020 
00021 bool getTransform(double *t, double *ti, double *rP, double *rPT, tf::TransformListener *listener, ros::Time time)
00022 {
00023   tf::StampedTransform transform;
00024 
00025   std::string error_msg;
00026   bool success = listener->waitForTransform(fixed_frame_, robot_frame_, time, ros::Duration(3.0), ros::Duration(0.01),
00027                                             &error_msg);
00028 
00029   if (!success)
00030   {
00031     ROS_WARN("Could not get transform, ignoring point cloud! %s", error_msg.c_str());
00032     return false;
00033   }
00034 
00035   listener->lookupTransform(fixed_frame_, robot_frame_, time, transform);
00036 
00037   double mat[9];
00038   double x = transform.getOrigin().getX() * 100;
00039   double y = transform.getOrigin().getY() * 100;
00040   double z = transform.getOrigin().getZ() * 100;
00041   mat[0] = transform.getBasis().getRow(0).getX();
00042   mat[1] = transform.getBasis().getRow(0).getY();
00043   mat[2] = transform.getBasis().getRow(0).getZ();
00044 
00045   mat[3] = transform.getBasis().getRow(1).getX();
00046   mat[4] = transform.getBasis().getRow(1).getY();
00047   mat[5] = transform.getBasis().getRow(1).getZ();
00048 
00049   mat[6] = transform.getBasis().getRow(2).getX();
00050   mat[7] = transform.getBasis().getRow(2).getY();
00051   mat[8] = transform.getBasis().getRow(2).getZ();
00052 
00053   t[0] = mat[4];
00054   t[1] = -mat[7];
00055   t[2] = -mat[1];
00056   t[3] = 0.0;
00057 
00058   t[4] = -mat[5];
00059   t[5] = mat[8];
00060   t[6] = mat[2];
00061   t[7] = 0.0;
00062 
00063   t[8] = -mat[3];
00064   t[9] = mat[6];
00065   t[10] = mat[0];
00066   t[11] = 0.0;
00067 
00068   // translation
00069   t[12] = -y;
00070   t[13] = z;
00071   t[14] = x;
00072   t[15] = 1;
00073   M4inv(t, ti);
00074   Matrix4ToEuler(t, rPT, rP);
00075 
00076   return true;
00077 }
00078 
00079 void writePose(int j, double rP[3], double rPT[3])
00080 {
00081   char pose_str[13];
00082   sprintf(pose_str, "scan%03d.pose", j);
00083   ofstream pose(pose_str);
00084   pose << rP[0] << " " << rP[1] << " " << rP[2] << endl << deg(rPT[0]) << " " << deg(rPT[1]) << " " << deg(rPT[2]);
00085   pose.close();
00086 }
00087 
00088 void reqCallback(const std_msgs::String::ConstPtr& e)
00089 {
00090   ROS_INFO_STREAM("Request received: " << e->data);
00091   requested_ = true;
00092 }
00093 
00094 void pcCallback(const sensor_msgs::PointCloud::ConstPtr& untransformed_cloud)
00095 {
00096   //ignore first scan (tf can't transform it and its incomplete anyway)
00097   static bool first = true;
00098   if (first)
00099   {
00100     first = false;
00101     return;
00102   }
00103 
00104   if (needRequest_ && !requested_)
00105   {
00106     return;
00107   }
00108 
00109   static int j = 0;
00110 
00111   // ----- write transform fixed_frame --> robot_frame to pose file
00112   double t[16], ti[16], rP[3], rPT[3];
00113   bool success = getTransform(t, ti, rP, rPT, tl_, untransformed_cloud->header.stamp);
00114   if (!success)
00115     return;
00116 
00117   writePose(j, rP, rPT);
00118 
00119   // ----- transform point cloud from sensor frame to robot_frame
00120   boost::shared_ptr<sensor_msgs::PointCloud> cloud(new sensor_msgs::PointCloud);
00121   tl_->transformPointCloud(robot_frame_, *untransformed_cloud, *cloud);
00122 
00123   if (!success)
00124     return;
00125 
00126   char scan_str[11];
00127   sprintf(scan_str, "scan%03d.3d", j++);
00128   ofstream scan(scan_str);
00129 
00130   size_t i;
00131   double p[3];
00132   for (i = 0; i < cloud->points.size(); i++)
00133   {
00134     p[0] = cloud->points[i].y * -100;
00135     p[1] = cloud->points[i].z * 100;
00136     p[2] = cloud->points[i].x * 100;
00137 
00138     // transform3(ti, p);
00139 
00140     if (!isnan(p[0]) && !isnan(p[1]) && !isnan(p[2]))
00141     {
00142       scan << p[0] << " " << p[1] << " " << p[2] << endl;
00143     }
00144   }
00145   scan.close();
00146   ROS_INFO("wrote %zu points to file %s (backlog: %f s)", i, scan_str, (ros::Time::now() - cloud->header.stamp).toSec());
00147 
00148   requested_ = false;
00149 }
00150 
00151 inline int32_t findChannelIndex(const sensor_msgs::PointCloud2ConstPtr& cloud, const std::string& channel)
00152 {
00153   for (size_t i = 0; i < cloud->fields.size(); ++i)
00154   {
00155     if (cloud->fields[i].name == channel)
00156     {
00157       return i;
00158     }
00159   }
00160 
00161   return -1;
00162 }
00163 
00164 void pc2aCallback(const sensor_msgs::PointCloud2Ptr& untransformed_cloud)
00165 {
00166   static int j = 0;
00167 
00168   // ----- write transform fixed_frame --> robot_frame to pose file
00169   double t[16], ti[16], rP[3], rPT[3];
00170   bool success = getTransform(t, ti, rP, rPT, tl_, untransformed_cloud->header.stamp);
00171   if (!success)
00172     return;
00173 
00174   writePose(j, rP, rPT);
00175 
00176   // ----- transform point cloud from sensor frame to robot_frame
00177   boost::shared_ptr<sensor_msgs::PointCloud2> cloud(new sensor_msgs::PointCloud2);
00178   success = pcl_ros::transformPointCloud(robot_frame_, *untransformed_cloud, *cloud, *tl_);
00179   if (!success)
00180     return;
00181 
00182   char scan_str[11];
00183   sprintf(scan_str, "scan%03d.3d", j++);
00184   ofstream scan(scan_str);
00185 
00186   int32_t xi = findChannelIndex(cloud, "x");
00187   int32_t yi = findChannelIndex(cloud, "y");
00188   int32_t zi = findChannelIndex(cloud, "z");
00189   int32_t rgbi = findChannelIndex(cloud, "rgb");
00190 
00191   if (xi == -1 || yi == -1 || zi == -1)
00192   {
00193     return;
00194   }
00195 
00196   const uint32_t xoff = cloud->fields[xi].offset;
00197   const uint32_t yoff = cloud->fields[yi].offset;
00198   const uint32_t zoff = cloud->fields[zi].offset;
00199   uint32_t rgboff = -1;
00200   if (rgbi != -1)
00201     rgboff = cloud->fields[rgbi].offset;
00202   const uint32_t point_step = cloud->point_step;
00203   const size_t point_count = cloud->width * cloud->height;
00204 
00205   if (point_count == 0)
00206   {
00207     return;
00208   }
00209 
00210   const uint8_t* ptr = &cloud->data.front();
00211   size_t i;
00212   double p[3];
00213   for (i = 0; i < point_count; ++i)
00214   {
00215     p[0] = *reinterpret_cast<const float*>(ptr + yoff) * -100;
00216     p[1] = *reinterpret_cast<const float*>(ptr + zoff) * 100;
00217     p[2] = *reinterpret_cast<const float*>(ptr + xoff) * 100;
00218 
00219     // transform3(ti, p);
00220 
00221     if (!isnan(p[0]) && !isnan(p[1]) && !isnan(p[2]))
00222     {
00223       scan << p[0] << " " << p[1] << " " << p[2];
00224 
00225       if (rgbi != -1)
00226       {
00227         uint32_t rgb = *reinterpret_cast<const uint32_t*>(ptr + rgboff);
00228         int r = ((rgb >> 16) & 0xff);
00229         int g = ((rgb >> 8) & 0xff);
00230         int b = (rgb & 0xff);
00231 
00232         scan << " " << r << " " << g << " " << b;
00233       }
00234       scan << endl;
00235     }
00236 
00237     ptr += point_step;
00238   }
00239 
00240   scan.close();
00241   ROS_INFO("wrote %zu points to file %s (backlog: %f s)", i, scan_str, (ros::Time::now() - cloud->header.stamp).toSec());
00242 }
00243 
00244 int main(int argc, char **argv)
00245 {
00246   ros::init(argc, argv, "slam_exporter");
00247 
00248   // Only dump points to file when request received.
00249   if (argc > 1 && strcmp(argv[1], "--withrequest") == 0)
00250   {
00251     ROS_INFO("Scan will only be exported when requested as defined by parameter.");
00252     needRequest_ = true;
00253   }
00254   else
00255     needRequest_ = false;
00256 
00257   requested_ = false;
00258 
00259   ros::NodeHandle n;
00260   ros::NodeHandle pn("~");
00261 
00262   tl_ = new tf::TransformListener(ros::Duration(60.0));
00263 
00264   bool use_point_cloud2;
00265   pn.param("fixed_frame", fixed_frame_, std::string("odom_combined"));
00266   pn.param("robot_frame", robot_frame_, std::string("base_link"));
00267   pn.param("use_point_cloud2", use_point_cloud2, true);
00268 
00269   ros::Subscriber sub; // must be declared outside of "if" scope
00270   if (use_point_cloud2)
00271     sub = n.subscribe("points2_in", 100, pc2aCallback);
00272   else
00273     sub = n.subscribe("points_in", 100, pcCallback);
00274 
00275   ros::Subscriber scanRequest = n.subscribe("/request", 1, reqCallback);
00276 
00277   ROS_INFO("slam_exporter initialized with fixed_frame = \"%s\", robot_frame = \"%s\", ", fixed_frame_.c_str(), robot_frame_.c_str());
00278   ros::spin();
00279   return 0;
00280 }