mapper.cpp
Go to the documentation of this file.
00001 #include <fstream>
00002 
00003 #include <boost/version.hpp>
00004 #include <boost/thread.hpp>
00005 #if BOOST_VERSION >= 104100
00006         #include <boost/thread/future.hpp>
00007 #endif // BOOST_VERSION >=  104100
00008 
00009 #include "ros/ros.h"
00010 #include "ros/console.h"
00011 #include "pointmatcher/PointMatcher.h"
00012 #include "pointmatcher/Timer.h"
00013 
00014 #include "pointmatcher_ros/point_cloud.h"
00015 #include "pointmatcher_ros/transform.h"
00016 #include "pointmatcher_ros/get_params_from_server.h"
00017 #include "pointmatcher_ros/ros_logger.h"
00018 
00019 #include "nav_msgs/Odometry.h"
00020 #include "tf/transform_broadcaster.h"
00021 #include "tf_conversions/tf_eigen.h"
00022 #include "tf/transform_listener.h"
00023 #include "eigen_conversions/eigen_msg.h"
00024 
00025 // Services
00026 #include "std_msgs/String.h"
00027 #include "std_srvs/Empty.h"
00028 #include "map_msgs/GetPointMap.h"
00029 #include "map_msgs/SaveMap.h"
00030 #include "ethzasl_icp_mapper/LoadMap.h"
00031 #include "ethzasl_icp_mapper/CorrectPose.h"
00032 #include "ethzasl_icp_mapper/SetMode.h"
00033 #include "ethzasl_icp_mapper/GetMode.h"
00034 #include "ethzasl_icp_mapper/GetBoundedMap.h" // FIXME: should that be moved to map_msgs?
00035 
00036 using namespace std;
00037 using namespace PointMatcherSupport;
00038 
00039 class Mapper
00040 {
00041         typedef PointMatcher<float> PM;
00042         typedef PM::DataPoints DP;
00043         
00044         ros::NodeHandle& n;
00045         ros::NodeHandle& pn;
00046         
00047         // Subscribers
00048         ros::Subscriber scanSub;
00049         ros::Subscriber cloudSub;
00050         
00051         // Publisher
00052         ros::Publisher mapPub;
00053         ros::Publisher outlierPub;
00054         ros::Publisher odomPub;
00055         ros::Publisher odomErrorPub;
00056         
00057         // Services
00058         ros::ServiceServer getPointMapSrv;
00059         ros::ServiceServer saveMapSrv;
00060         ros::ServiceServer loadMapSrv;
00061         ros::ServiceServer resetSrv;
00062         ros::ServiceServer correctPoseSrv;
00063         ros::ServiceServer setModeSrv;
00064         ros::ServiceServer getModeSrv;
00065         ros::ServiceServer getBoundedMapSrv;
00066 
00067         ros::Time mapCreationTime;
00068         ros::Time lastPoinCloudTime;
00069 
00070         PM::DataPointsFilters inputFilters;
00071         PM::DataPointsFilters mapPreFilters;
00072         PM::DataPointsFilters mapPostFilters;
00073         PM::DataPoints *mapPointCloud;
00074         PM::ICPSequence icp;
00075         unique_ptr<PM::Transformation> transformation;
00076         
00077         // multi-threading mapper
00078         #if BOOST_VERSION >= 104100
00079         typedef boost::packaged_task<PM::DataPoints*> MapBuildingTask;
00080         typedef boost::unique_future<PM::DataPoints*> MapBuildingFuture;
00081         boost::thread mapBuildingThread;
00082         MapBuildingTask mapBuildingTask;
00083         MapBuildingFuture mapBuildingFuture;
00084         bool mapBuildingInProgress;
00085         #endif // BOOST_VERSION >= 104100
00086 
00087         // Parameters
00088         bool useConstMotionModel; 
00089         bool processingNewCloud; 
00090         bool localizing;
00091         bool mapping;
00092         int minReadingPointCount;
00093         int minMapPointCount;
00094         int inputQueueSize; 
00095         double minOverlap;
00096         double maxOverlapToMerge;
00097         double tfRefreshPeriod;  
00098         string odomFrame;
00099         string mapFrame;
00100         string vtkFinalMapName; 
00101         
00102 
00103         PM::TransformationParameters TOdomToMap;
00104         boost::thread publishThread;
00105         boost::mutex publishLock;
00106         ros::Time publishStamp;
00107         
00108         tf::TransformListener tfListener;
00109         tf::TransformBroadcaster tfBroadcaster;
00110         
00111 public:
00112         Mapper(ros::NodeHandle& n, ros::NodeHandle& pn);
00113         ~Mapper();
00114         
00115 protected:
00116         void gotScan(const sensor_msgs::LaserScan& scanMsgIn);
00117         void gotCloud(const sensor_msgs::PointCloud2& cloudMsgIn);
00118         void processCloud(unique_ptr<DP> cloud, const std::string& scannerFrame, const ros::Time& stamp, uint32_t seq);
00119         void processNewMapIfAvailable();
00120         void setMap(DP* newPointCloud);
00121         DP* updateMap(DP* newPointCloud, const PM::TransformationParameters Ticp, bool updateExisting);
00122         void waitForMapBuildingCompleted();
00123         
00124         void publishLoop(double publishPeriod);
00125         void publishTransform();
00126         
00127         // Services
00128         bool getPointMap(map_msgs::GetPointMap::Request &req, map_msgs::GetPointMap::Response &res);
00129         bool saveMap(map_msgs::SaveMap::Request &req, map_msgs::SaveMap::Response &res);
00130         bool loadMap(ethzasl_icp_mapper::LoadMap::Request &req, ethzasl_icp_mapper::LoadMap::Response &res);
00131         bool reset(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res);
00132         bool correctPose(ethzasl_icp_mapper::CorrectPose::Request &req, ethzasl_icp_mapper::CorrectPose::Response &res);
00133         bool setMode(ethzasl_icp_mapper::SetMode::Request &req, ethzasl_icp_mapper::SetMode::Response &res);
00134         bool getMode(ethzasl_icp_mapper::GetMode::Request &req, ethzasl_icp_mapper::GetMode::Response &res);
00135         bool getBoundedMap(ethzasl_icp_mapper::GetBoundedMap::Request &req, ethzasl_icp_mapper::GetBoundedMap::Response &res);
00136 };
00137 
00138 Mapper::Mapper(ros::NodeHandle& n, ros::NodeHandle& pn):
00139         n(n),
00140         pn(pn),
00141         mapPointCloud(0),
00142         transformation(PM::get().REG(Transformation).create("RigidTransformation")),
00143         #if BOOST_VERSION >= 104100
00144         mapBuildingInProgress(false),
00145         #endif // BOOST_VERSION >= 104100
00146         useConstMotionModel(getParam<bool>("useConstMotionModel", false)),
00147         processingNewCloud(false),
00148         localizing(getParam<bool>("localizing", true)),
00149         mapping(getParam<bool>("mapping", true)),
00150         minReadingPointCount(getParam<int>("minReadingPointCount", 2000)),
00151         minMapPointCount(getParam<int>("minMapPointCount", 500)),
00152         inputQueueSize(getParam<int>("inputQueueSize", 10)),
00153         minOverlap(getParam<double>("minOverlap", 0.5)),
00154         maxOverlapToMerge(getParam<double>("maxOverlapToMerge", 0.9)),
00155         tfRefreshPeriod(getParam<double>("tfRefreshPeriod", 0.01)),
00156         odomFrame(getParam<string>("odom_frame", "odom")),
00157         mapFrame(getParam<string>("map_frame", "map")),
00158         vtkFinalMapName(getParam<string>("vtkFinalMapName", "finalMap.vtk")),
00159         TOdomToMap(PM::TransformationParameters::Identity(4, 4)),
00160         publishStamp(ros::Time::now()),
00161   tfListener(ros::Duration(30))
00162 {
00163 
00164         // Ensure proper states
00165         if(localizing == false)
00166                 mapping = false;
00167         if(mapping == true)
00168                 localizing = true;
00169 
00170         // set logger
00171         if (getParam<bool>("useROSLogger", false))
00172                 PointMatcherSupport::setLogger(new PointMatcherSupport::ROSLogger);
00173 
00174         // load configs
00175         string configFileName;
00176         if (ros::param::get("~icpConfig", configFileName))
00177         {
00178                 ifstream ifs(configFileName.c_str());
00179                 if (ifs.good())
00180                 {
00181                         icp.loadFromYaml(ifs);
00182                 }
00183                 else
00184                 {
00185                         ROS_ERROR_STREAM("Cannot load ICP config from YAML file " << configFileName);
00186                         icp.setDefault();
00187                 }
00188         }
00189         else
00190         {
00191                 ROS_INFO_STREAM("No ICP config file given, using default");
00192                 icp.setDefault();
00193         }
00194         if (getParam<bool>("useROSLogger", false))
00195                 PointMatcherSupport::setLogger(new PointMatcherSupport::ROSLogger);
00196         
00197         if (ros::param::get("~inputFiltersConfig", configFileName))
00198         {
00199                 ifstream ifs(configFileName.c_str());
00200                 if (ifs.good())
00201                 {
00202                         inputFilters = PM::DataPointsFilters(ifs);
00203                 }
00204                 else
00205                 {
00206                         ROS_ERROR_STREAM("Cannot load input filters config from YAML file " << configFileName);
00207                 }
00208         }
00209         else
00210         {
00211                 ROS_INFO_STREAM("No input filters config file given, not using these filters");
00212         }
00213         
00214         if (ros::param::get("~mapPreFiltersConfig", configFileName))
00215         {
00216                 ifstream ifs(configFileName.c_str());
00217                 if (ifs.good())
00218                 {
00219                         mapPreFilters = PM::DataPointsFilters(ifs);
00220                 }
00221                 else
00222                 {
00223                         ROS_ERROR_STREAM("Cannot load map pre-filters config from YAML file " << configFileName);
00224                 }
00225         }
00226         else
00227         {
00228                 ROS_INFO_STREAM("No map pre-filters config file given, not using these filters");
00229         }
00230         
00231         if (ros::param::get("~mapPostFiltersConfig", configFileName))
00232         {
00233                 ifstream ifs(configFileName.c_str());
00234                 if (ifs.good())
00235                 {
00236                         mapPostFilters = PM::DataPointsFilters(ifs);
00237                 }
00238                 else
00239                 {
00240                         ROS_ERROR_STREAM("Cannot load map post-filters config from YAML file " << configFileName);
00241                 }
00242         }
00243         else
00244         {
00245                 ROS_INFO_STREAM("No map post-filters config file given, not using these filters");
00246         }
00247 
00248         // topics and services initialization
00249         if (getParam<bool>("subscribe_scan", true))
00250                 scanSub = n.subscribe("scan", inputQueueSize, &Mapper::gotScan, this);
00251         if (getParam<bool>("subscribe_cloud", true))
00252                 cloudSub = n.subscribe("cloud_in", inputQueueSize, &Mapper::gotCloud, this);
00253         mapPub = n.advertise<sensor_msgs::PointCloud2>("point_map", 2, true);
00254         outlierPub = n.advertise<sensor_msgs::PointCloud2>("outliers", 2, true);
00255         odomPub = n.advertise<nav_msgs::Odometry>("icp_odom", 50, true);
00256         odomErrorPub = n.advertise<nav_msgs::Odometry>("icp_error_odom", 50, true);
00257         getPointMapSrv = n.advertiseService("dynamic_point_map", &Mapper::getPointMap, this);
00258         saveMapSrv = pn.advertiseService("save_map", &Mapper::saveMap, this);
00259         loadMapSrv = pn.advertiseService("load_map", &Mapper::loadMap, this);
00260         resetSrv = pn.advertiseService("reset", &Mapper::reset, this);
00261         correctPoseSrv = pn.advertiseService("correct_pose", &Mapper::correctPose, this);
00262         setModeSrv = pn.advertiseService("set_mode", &Mapper::setMode, this);
00263         getModeSrv = pn.advertiseService("get_mode", &Mapper::getMode, this);
00264         getBoundedMapSrv = pn.advertiseService("get_bounded_map", &Mapper::getBoundedMap, this);
00265 
00266         // refreshing tf transform thread
00267         publishThread = boost::thread(boost::bind(&Mapper::publishLoop, this, tfRefreshPeriod));
00268 }
00269 
00270 Mapper::~Mapper()
00271 {
00272         #if BOOST_VERSION >= 104100
00273         // wait for map-building thread
00274         if (mapBuildingInProgress)
00275         {
00276                 mapBuildingFuture.wait();
00277                 if (mapBuildingFuture.has_value())
00278                         delete mapBuildingFuture.get();
00279         }
00280         #endif // BOOST_VERSION >= 104100
00281         // wait for publish thread
00282         publishThread.join();
00283         // save point cloud
00284         if (mapPointCloud)
00285         {
00286                 mapPointCloud->save(vtkFinalMapName);
00287                 delete mapPointCloud;
00288         }
00289 }
00290 
00291 void Mapper::gotScan(const sensor_msgs::LaserScan& scanMsgIn)
00292 {
00293         if(localizing)
00294         {
00295                 const ros::Time endScanTime(scanMsgIn.header.stamp + ros::Duration(scanMsgIn.time_increment * (scanMsgIn.ranges.size() - 1)));
00296                 unique_ptr<DP> cloud(new DP(PointMatcher_ros::rosMsgToPointMatcherCloud<float>(scanMsgIn, &tfListener, odomFrame)));
00297                 processCloud(move(cloud), scanMsgIn.header.frame_id, endScanTime, scanMsgIn.header.seq);
00298         }
00299 }
00300 
00301 void Mapper::gotCloud(const sensor_msgs::PointCloud2& cloudMsgIn)
00302 {
00303         if(localizing)
00304         {
00305                 unique_ptr<DP> cloud(new DP(PointMatcher_ros::rosMsgToPointMatcherCloud<float>(cloudMsgIn)));
00306                 processCloud(move(cloud), cloudMsgIn.header.frame_id, cloudMsgIn.header.stamp, cloudMsgIn.header.seq);
00307         }
00308 }
00309 
00310 struct BoolSetter
00311 {
00312 public:
00313         bool toSetValue;
00314         BoolSetter(bool& target, bool toSetValue):
00315                 toSetValue(toSetValue),
00316                 target(target)
00317         {}
00318         ~BoolSetter()
00319         {
00320                 target = toSetValue;
00321         }
00322 protected:
00323         bool& target;
00324 };
00325 
00326 void Mapper::processCloud(unique_ptr<DP> newPointCloud, const std::string& scannerFrame, const ros::Time& stamp, uint32_t seq)
00327 {
00328         processingNewCloud = true;
00329         BoolSetter stopProcessingSetter(processingNewCloud, false);
00330 
00331         // if the future has completed, use the new map
00332         processNewMapIfAvailable();
00333         
00334         // IMPORTANT:  We need to receive the point clouds in local coordinates (scanner or robot)
00335         timer t;
00336         
00337         // Convert point cloud
00338         const size_t goodCount(newPointCloud->features.cols());
00339         if (goodCount == 0)
00340         {
00341                 ROS_ERROR("I found no good points in the cloud");
00342                 return;
00343         }
00344         
00345         // Dimension of the point cloud, important since we handle 2D and 3D
00346         const int dimp1(newPointCloud->features.rows());
00347 
00348         ROS_INFO("Processing new point cloud");
00349         {
00350                 timer t; // Print how long take the algo
00351                 
00352                 // Apply filters to incoming cloud, in scanner coordinates
00353                 inputFilters.apply(*newPointCloud);
00354                 
00355                 ROS_INFO_STREAM("Input filters took " << t.elapsed() << " [s]");
00356         }
00357         
00358         string reason;
00359         // Initialize the transformation to identity if empty
00360         if(!icp.hasMap())
00361         {
00362                 // we need to know the dimensionality of the point cloud to initialize properly
00363                 publishLock.lock();
00364                 TOdomToMap = PM::TransformationParameters::Identity(dimp1, dimp1);
00365                 publishLock.unlock();
00366         }
00367 
00368         // Fetch transformation from scanner to odom
00369         // Note: we don't need to wait for transform. It is already called in transformListenerToEigenMatrix()
00370         PM::TransformationParameters TOdomToScanner;
00371         try
00372         {
00373                 TOdomToScanner = PointMatcher_ros::eigenMatrixToDim<float>(
00374                                 PointMatcher_ros::transformListenerToEigenMatrix<float>(
00375                                 tfListener,
00376                                 scannerFrame,
00377                                 odomFrame,
00378                                 stamp
00379                         ), dimp1);
00380         }
00381         catch(tf::ExtrapolationException e)
00382         {
00383                 ROS_ERROR_STREAM("Extrapolation Exception. stamp = "<< stamp << " now = " << ros::Time::now() << " delta = " << ros::Time::now() - stamp);
00384                 return;
00385         }
00386 
00387 
00388         ROS_DEBUG_STREAM("TOdomToScanner(" << odomFrame<< " to " << scannerFrame << "):\n" << TOdomToScanner);
00389         ROS_DEBUG_STREAM("TOdomToMap(" << odomFrame<< " to " << mapFrame << "):\n" << TOdomToMap);
00390                 
00391         const PM::TransformationParameters TscannerToMap = transformation->correctParameters(TOdomToMap * TOdomToScanner.inverse());
00392         ROS_DEBUG_STREAM("TscannerToMap (" << scannerFrame << " to " << mapFrame << "):\n" << TscannerToMap);
00393         
00394         // Ensure a minimum amount of point after filtering
00395         const int ptsCount = newPointCloud->features.cols();
00396         if(ptsCount < minReadingPointCount)
00397         {
00398                 ROS_ERROR_STREAM("Not enough points in newPointCloud: only " << ptsCount << " pts.");
00399                 return;
00400         }
00401 
00402         // Initialize the map if empty
00403         if(!icp.hasMap())
00404         {
00405                 ROS_INFO_STREAM("Creating an initial map");
00406                 mapCreationTime = stamp;
00407                 setMap(updateMap(newPointCloud.release(), TscannerToMap, false));
00408                 // we must not delete newPointCloud because we just stored it in the mapPointCloud
00409                 return;
00410         }
00411         
00412         // Check dimension
00413         if (newPointCloud->features.rows() != icp.getInternalMap().features.rows())
00414         {
00415                 ROS_ERROR_STREAM("Dimensionality missmatch: incoming cloud is " << newPointCloud->features.rows()-1 << " while map is " << icp.getInternalMap().features.rows()-1);
00416                 return;
00417         }
00418         
00419         try 
00420         {
00421                 // Apply ICP
00422                 PM::TransformationParameters Ticp;
00423 
00424                 Ticp = icp(*newPointCloud, TscannerToMap);
00425 
00426                 ROS_DEBUG_STREAM("Ticp:\n" << Ticp);
00427                 
00428                 // Ensure minimum overlap between scans
00429                 const double estimatedOverlap = icp.errorMinimizer->getOverlap();
00430                 ROS_INFO_STREAM("Overlap: " << estimatedOverlap);
00431                 if (estimatedOverlap < minOverlap)
00432                 {
00433                         ROS_ERROR_STREAM("Estimated overlap too small, ignoring ICP correction!");
00434                         return;
00435                 }
00436                 
00437                 // Compute tf
00438                 publishStamp = stamp;
00439                 publishLock.lock();
00440                 TOdomToMap = Ticp * TOdomToScanner;
00441                 // Publish tf
00442                 tfBroadcaster.sendTransform(PointMatcher_ros::eigenMatrixToStampedTransform<float>(TOdomToMap, mapFrame, odomFrame, stamp));
00443                 publishLock.unlock();
00444                 processingNewCloud = false;
00445                 
00446                 ROS_DEBUG_STREAM("TOdomToMap:\n" << TOdomToMap);
00447 
00448                 // Publish odometry
00449                 if (odomPub.getNumSubscribers())
00450                         odomPub.publish(PointMatcher_ros::eigenMatrixToOdomMsg<float>(Ticp, mapFrame, stamp));
00451         
00452                 // Publish error on odometry
00453                 if (odomErrorPub.getNumSubscribers())
00454                         odomErrorPub.publish(PointMatcher_ros::eigenMatrixToOdomMsg<float>(TOdomToMap, mapFrame, stamp));
00455 
00456                 // Publish outliers
00457                 if (outlierPub.getNumSubscribers())
00458                 {
00459                         //DP outliers = PM::extractOutliers(transformation->compute(*newPointCloud, Ticp), *mapPointCloud, 0.6);
00460                         //outlierPub.publish(PointMatcher_ros::pointMatcherCloudToRosMsg<float>(outliers, mapFrame, mapCreationTime));
00461                 }
00462 
00463                 // check if news points should be added to the map
00464                 if (
00465                         mapping &&
00466                         ((estimatedOverlap < maxOverlapToMerge) || (icp.getInternalMap().features.cols() < minMapPointCount)) &&
00467                         #if BOOST_VERSION >= 104100
00468                         (!mapBuildingInProgress)
00469                         #else // BOOST_VERSION >= 104100
00470                         true
00471                         #endif // BOOST_VERSION >= 104100
00472                 )
00473                 {
00474                         // make sure we process the last available map
00475                         mapCreationTime = stamp;
00476                         #if BOOST_VERSION >= 104100
00477                         ROS_INFO("Adding new points to the map in background");
00478                         mapBuildingTask = MapBuildingTask(boost::bind(&Mapper::updateMap, this, newPointCloud.release(), Ticp, true));
00479                         mapBuildingFuture = mapBuildingTask.get_future();
00480                         mapBuildingThread = boost::thread(boost::move(boost::ref(mapBuildingTask)));
00481                         mapBuildingInProgress = true;
00482                         #else // BOOST_VERSION >= 104100
00483                         ROS_INFO("Adding new points to the map");
00484                         setMap(updateMap( newPointCloud.release(), Ticp, true));
00485                         #endif // BOOST_VERSION >= 104100
00486                 }
00487         }
00488         catch (PM::ConvergenceError error)
00489         {
00490                 ROS_ERROR_STREAM("ICP failed to converge: " << error.what());
00491                 return;
00492         }
00493         
00494         //Statistics about time and real-time capability
00495         int realTimeRatio = 100*t.elapsed() / (stamp.toSec()-lastPoinCloudTime.toSec());
00496         ROS_INFO_STREAM("[TIME] Total ICP took: " << t.elapsed() << " [s]");
00497         if(realTimeRatio < 80)
00498                 ROS_INFO_STREAM("[TIME] Real-time capability: " << realTimeRatio << "%");
00499         else
00500                 ROS_WARN_STREAM("[TIME] Real-time capability: " << realTimeRatio << "%");
00501 
00502         lastPoinCloudTime = stamp;
00503 }
00504 
00505 void Mapper::processNewMapIfAvailable()
00506 {
00507         #if BOOST_VERSION >= 104100
00508         if (mapBuildingInProgress && mapBuildingFuture.has_value())
00509         {
00510                 ROS_INFO_STREAM("New map available");
00511                 setMap(mapBuildingFuture.get());
00512                 mapBuildingInProgress = false;
00513         }
00514         #endif // BOOST_VERSION >= 104100
00515 }
00516 
00517 void Mapper::setMap(DP* newPointCloud)
00518 {
00519         // delete old map
00520         if (mapPointCloud)
00521                 delete mapPointCloud;
00522         
00523         // set new map
00524         mapPointCloud = newPointCloud;
00525         icp.setMap(*mapPointCloud);
00526         
00527         // Publish map point cloud
00528         // FIXME this crash when used without descriptor
00529         if (mapPub.getNumSubscribers())
00530                 mapPub.publish(PointMatcher_ros::pointMatcherCloudToRosMsg<float>(*mapPointCloud, mapFrame, mapCreationTime));
00531 }
00532 
00533 Mapper::DP* Mapper::updateMap(DP* newPointCloud, const PM::TransformationParameters Ticp, bool updateExisting)
00534 {
00535         timer t;
00536         
00537         // Correct new points using ICP result
00538         *newPointCloud = transformation->compute(*newPointCloud, Ticp); 
00539         
00540         // Preparation of cloud for inclusion in map
00541         mapPreFilters.apply(*newPointCloud);
00542         
00543         // Merge point clouds to map
00544         if (updateExisting)
00545                 newPointCloud->concatenate(*mapPointCloud);
00546 
00547         // Map maintenance
00548         if(newPointCloud->features.cols() > minMapPointCount)
00549                 mapPostFilters.apply(*newPointCloud);
00550         
00551         ROS_INFO_STREAM("[TIME] New map available (" << newPointCloud->features.cols() << " pts), update took " << t.elapsed() << " [s]");
00552         
00553         return newPointCloud;
00554 }
00555 
00556 void Mapper::waitForMapBuildingCompleted()
00557 {
00558         #if BOOST_VERSION >= 104100
00559         if (mapBuildingInProgress)
00560         {
00561                 // we wait for now, in future we should kill it
00562                 mapBuildingFuture.wait();
00563                 mapBuildingInProgress = false;
00564         }
00565         #endif // BOOST_VERSION >= 104100
00566 }
00567 
00568 void Mapper::publishLoop(double publishPeriod)
00569 {
00570         if(publishPeriod == 0)
00571                 return;
00572         ros::Rate r(1.0 / publishPeriod);
00573         while(ros::ok())
00574         {
00575                 publishTransform();
00576                 r.sleep();
00577         }
00578 }
00579 
00580 void Mapper::publishTransform()
00581 {
00582         if(processingNewCloud == false)
00583         {
00584                 publishLock.lock();
00585                 // Note: we use now as timestamp to refresh the tf and avoid other buffer to be empty
00586                 tfBroadcaster.sendTransform(PointMatcher_ros::eigenMatrixToStampedTransform<float>(TOdomToMap, mapFrame, odomFrame, ros::Time::now()));
00587                 publishLock.unlock();
00588         }
00589 }
00590 
00591 bool Mapper::getPointMap(map_msgs::GetPointMap::Request &req, map_msgs::GetPointMap::Response &res)
00592 {
00593         if (!mapPointCloud)
00594                 return false;
00595         
00596         // FIXME: do we need a mutex here?
00597         res.map = PointMatcher_ros::pointMatcherCloudToRosMsg<float>(*mapPointCloud, mapFrame, ros::Time::now());
00598         return true;
00599 }
00600 
00601 bool Mapper::saveMap(map_msgs::SaveMap::Request &req, map_msgs::SaveMap::Response &res)
00602 {
00603         if (!mapPointCloud)
00604                 return false;
00605         
00606         try
00607         {
00608                 mapPointCloud->save(req.filename.data);
00609         }
00610         catch (const std::runtime_error& e)
00611         {
00612                 ROS_ERROR_STREAM("Unable to save: " << e.what());
00613                 return false;
00614         }
00615         
00616         ROS_INFO_STREAM("Map saved at " <<  req.filename.data << "with " << mapPointCloud->features.cols() << " points.");
00617         return true;
00618 }
00619 
00620 bool Mapper::loadMap(ethzasl_icp_mapper::LoadMap::Request &req, ethzasl_icp_mapper::LoadMap::Response &res)
00621 {
00622         waitForMapBuildingCompleted();
00623         
00624         DP* cloud(new DP(DP::load(req.filename.data)));
00625 
00626         const int dim = cloud->features.rows();
00627         const int nbPts = cloud->features.cols();
00628         ROS_INFO_STREAM("Loading " << dim-1 << "D point cloud (" << req.filename.data << ") with " << nbPts << " points.");
00629 
00630         publishLock.lock();
00631         TOdomToMap = PM::TransformationParameters::Identity(dim,dim);
00632         publishLock.unlock();
00633 
00634         setMap(cloud);
00635         
00636         return true;
00637 }
00638 
00639 bool Mapper::reset(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
00640 {
00641         waitForMapBuildingCompleted();
00642         
00643         // note: no need for locking as we do ros::spin(), to update if we go for multi-threading
00644         publishLock.lock();
00645         TOdomToMap = PM::TransformationParameters::Identity(4,4);
00646         publishLock.unlock();
00647 
00648         icp.clearMap();
00649         
00650         return true;
00651 }
00652 
00653 bool Mapper::correctPose(ethzasl_icp_mapper::CorrectPose::Request &req, ethzasl_icp_mapper::CorrectPose::Response &res)
00654 {
00655         publishLock.lock();
00656         TOdomToMap = PointMatcher_ros::odomMsgToEigenMatrix<float>(req.odom);
00657         tfBroadcaster.sendTransform(PointMatcher_ros::eigenMatrixToStampedTransform<float>(TOdomToMap, mapFrame, odomFrame, ros::Time::now()));
00658         publishLock.unlock();
00659 
00660         return true;
00661 }
00662 
00663 bool Mapper::setMode(ethzasl_icp_mapper::SetMode::Request &req, ethzasl_icp_mapper::SetMode::Response &res)
00664 {
00665         // Impossible states
00666         if(req.localize == false && req.map == true)
00667                 return false;
00668 
00669         localizing = req.localize;
00670         mapping = req.map;
00671         
00672         return true;
00673 }
00674 
00675 bool Mapper::getMode(ethzasl_icp_mapper::GetMode::Request &req, ethzasl_icp_mapper::GetMode::Response &res)
00676 {
00677         res.localize = localizing;
00678         res.map = mapping;
00679         return true;
00680 }
00681 
00682 
00683 
00684 bool Mapper::getBoundedMap(ethzasl_icp_mapper::GetBoundedMap::Request &req, ethzasl_icp_mapper::GetBoundedMap::Response &res)
00685 {
00686         if (!mapPointCloud)
00687                 return false;
00688 
00689         const float max_x = req.topRightCorner.x;
00690         const float max_y = req.topRightCorner.y;
00691         const float max_z = req.topRightCorner.z;
00692 
00693         const float min_x = req.bottomLeftCorner.x;
00694         const float min_y = req.bottomLeftCorner.y;
00695         const float min_z = req.bottomLeftCorner.z;
00696 
00697         cerr << "min [" << min_x << ", " << min_y << ", " << min_z << "] " << endl;
00698         cerr << "max [" << max_x << ", " << max_y << ", " << max_z << "] " << endl;
00699 
00700 
00701 
00702         tf::StampedTransform stampedTr;
00703         
00704         Eigen::Affine3d eigenTr;
00705         tf::poseMsgToEigen(req.mapCenter, eigenTr);
00706         Eigen::MatrixXf T = eigenTr.matrix().inverse().cast<float>();
00707         //const Eigen::MatrixXf T = eigenTr.matrix().cast<float>();
00708 
00709         cerr << "T:" << endl << T << endl;
00710         T = transformation->correctParameters(T);
00711 
00712                 
00713         // FIXME: do we need a mutex here?
00714         const DP centeredPointCloud = transformation->compute(*mapPointCloud, T); 
00715         DP cutPointCloud = centeredPointCloud.createSimilarEmpty();
00716 
00717         cerr << centeredPointCloud.features.topLeftCorner(3, 10) << endl;
00718         cerr << T << endl;
00719 
00720         int newPtCount = 0;
00721         for(int i=0; i < centeredPointCloud.features.cols(); i++)
00722         {
00723                 const float x = centeredPointCloud.features(0,i);
00724                 const float y = centeredPointCloud.features(1,i);
00725                 const float z = centeredPointCloud.features(2,i);
00726                 
00727                 if(x < max_x && x > min_x &&
00728                          y < max_y && y > min_y &&
00729                    z < max_z && z > min_z       )
00730                 {
00731                         cutPointCloud.setColFrom(newPtCount, centeredPointCloud, i);
00732                         newPtCount++;   
00733                 }
00734         }
00735 
00736         cerr << "Extract " << newPtCount << " points from the map" << endl;
00737         
00738         cutPointCloud.conservativeResize(newPtCount);
00739         cutPointCloud = transformation->compute(cutPointCloud, T.inverse()); 
00740 
00741         
00742         // Send the resulting point cloud in ROS format
00743         res.boundedMap = PointMatcher_ros::pointMatcherCloudToRosMsg<float>(cutPointCloud, mapFrame, ros::Time::now());
00744         return true;
00745 }
00746 
00747 // Main function supporting the Mapper class
00748 int main(int argc, char **argv)
00749 {
00750         ros::init(argc, argv, "mapper");
00751         ros::NodeHandle n;
00752         ros::NodeHandle pn("~");
00753         Mapper mapper(n, pn);
00754         ros::spin();
00755         
00756         return 0;
00757 }


ethzasl_icp_mapper
Author(s): François Pomerleau and Stéphane Magnenat
autogenerated on Thu Jan 2 2014 11:16:21