map_maintainer.cpp
Go to the documentation of this file.
00001 #include <fstream>
00002 
00003 #include <boost/version.hpp>
00004 
00005 #include "ros/ros.h"
00006 #include "ros/console.h"
00007 #include "pointmatcher/PointMatcher.h"
00008 #include "pointmatcher/Timer.h"
00009 
00010 #include "pointmatcher_ros/point_cloud.h"
00011 #include "pointmatcher_ros/transform.h"
00012 #include "pointmatcher_ros/get_params_from_server.h"
00013 #include "pointmatcher_ros/ros_logger.h"
00014 
00015 #include "nav_msgs/Odometry.h"
00016 #include "tf/transform_broadcaster.h"
00017 #include "tf_conversions/tf_eigen.h"
00018 #include "tf/transform_listener.h"
00019 
00020 #include <message_filters/subscriber.h>
00021 #include <message_filters/synchronizer.h>
00022 #include <message_filters/sync_policies/exact_time.h>
00023 
00024 #include <interactive_markers/interactive_marker_server.h>
00025 #include <interactive_markers/menu_handler.h>
00026 
00027 #include "std_msgs/String.h"
00028 #include "std_srvs/Empty.h"
00029 
00030 using namespace std;
00031 using namespace PointMatcherSupport;
00032 using namespace visualization_msgs;
00033 using namespace interactive_markers;
00034 
00035 class MapMaintener
00036 {
00037         typedef PointMatcher<float> PM;
00038         typedef PM::DataPoints DP;
00039         typedef PM::TransformationParameters TP;
00040         
00041         ros::NodeHandle& n;
00042         ros::NodeHandle& pn;
00043 
00044         message_filters::Subscriber<sensor_msgs::PointCloud2> cloud_sub;
00045         message_filters::Subscriber<nav_msgs::Odometry> odom_sub;
00046 
00047         typedef message_filters::sync_policies::ExactTime<sensor_msgs::PointCloud2, nav_msgs::Odometry> MySyncPolicy;
00048         message_filters::Synchronizer<MySyncPolicy> sync;
00049 
00050         ros::Subscriber scanSub;
00051         ros::Subscriber cloudSub;
00052         ros::Publisher mapPub;
00053         
00054         // FIXME which service do we need?
00055         //ros::ServiceServer getPointMapSrv;
00056         //ros::ServiceServer saveMapSrv;
00057         //ros::ServiceServer resetSrv;
00058 
00059         PM::ICP icp;
00060 
00061         PM::DataPointsFilters inputFilters;
00062         PM::DataPointsFilters mapPreFilters;
00063         PM::DataPointsFilters mapPostFilters;
00064         unique_ptr<PM::Transformation> transformation;
00065         
00066         PM::DataPoints mapPointCloud;
00067         
00068         // Parameters
00069         string vtkFinalMapName; 
00070         int inputQueueSize; 
00071         const float size_x;
00072         const float size_y;
00073         const float size_z;
00074 
00075         // Interative markers
00076         boost::shared_ptr<InteractiveMarkerServer> server;
00077         MenuHandler menu_handler;
00078         bool mappingActive;
00079         bool singleScan;
00080         MenuHandler::EntryHandle h_single;
00081         MenuHandler::EntryHandle h_start; 
00082         MenuHandler::EntryHandle h_pause;
00083         MenuHandler::EntryHandle h_stop;
00084         MenuHandler::EntryHandle h_save;
00085 
00086         // tf
00087         tf::TransformListener tfListener;
00088 
00089 public:
00090         boost::mutex publishLock;
00091         PM::TransformationParameters TObjectToMap;
00092         // Parameters
00093         string objectFrame;
00094         string mapFrame;
00095         
00096 public:
00097         MapMaintener(ros::NodeHandle& n, ros::NodeHandle& pn);
00098         ~MapMaintener();
00099         
00100 protected:
00101         void gotCloud(const sensor_msgs::PointCloud2ConstPtr& cloudMsgIn, const nav_msgs::OdometryConstPtr& odom);
00102         void processCloud(DP cloud, const TP TScannerToMap);
00103         
00104         void makeMenuMarker( std::string name );
00105         void addRotAndTransCtrl(InteractiveMarker &int_marker, const double w, const double x, const double y, const double z, const std::string name);
00106         void update_tf(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback );
00107 
00108 
00109         void singleScanCallback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback );
00110         void startMapCallback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback );
00111         void pauseMapCallback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback );
00112         void stopMapCallback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback );
00113         void saveMapCallback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback );
00114 
00115         //bool getPointMap(map_msgs::GetPointMap::Request &req, map_msgs::GetPointMap::Response &res);
00116         //bool saveMap(map_msgs::SaveMap::Request &req, map_msgs::SaveMap::Response &res);
00117         //bool reset(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res);
00118 };
00119 
00120 MapMaintener::MapMaintener(ros::NodeHandle& n, ros::NodeHandle& pn):
00121         n(n),
00122         pn(pn),
00123         cloud_sub(n, "cloud_in", 1),
00124         odom_sub(n, "icp_odom", 1),
00125         sync(MySyncPolicy(1), cloud_sub, odom_sub),
00126         transformation(PM::get().REG(Transformation).create("RigidTransformation")),
00127         vtkFinalMapName(getParam<string>("vtkFinalMapName", "finalMap_maintained.vtk")),
00128         size_x(getParam<double>("size_x", 1.0)),
00129         size_y(getParam<double>("size_y", 1.0)),
00130         size_z(getParam<double>("size_z", 1.0)),
00131         mappingActive(false),
00132         singleScan(false),
00133   tfListener(ros::Duration(30)),
00134         TObjectToMap(PM::TransformationParameters::Identity(4,4)),
00135         objectFrame(getParam<string>("object_frame", "map")),
00136         mapFrame(getParam<string>("map_frame", "map"))
00137 {
00138         
00139         // load configs
00140         string configFileName;
00141         if (ros::param::get("~icpConfig", configFileName))
00142         {
00143                 ifstream ifs(configFileName.c_str());
00144                 if (ifs.good())
00145                 {
00146                         icp.loadFromYaml(ifs);
00147                 }
00148                 else
00149                 {
00150                         ROS_ERROR_STREAM("Cannot load ICP config from YAML file " << configFileName);
00151                         icp.setDefault();
00152                 }
00153         }
00154         else
00155         {
00156                 ROS_INFO_STREAM("No ICP config file given, using default");
00157                 icp.setDefault();
00158         }
00159 
00160         if (ros::param::get("~inputFiltersConfig", configFileName))
00161         {
00162                 ifstream ifs(configFileName.c_str());
00163                 if (ifs.good())
00164                 {
00165                         inputFilters = PM::DataPointsFilters(ifs);
00166                 }
00167                 else
00168                 {
00169                         ROS_ERROR_STREAM("Cannot load input filters config from YAML file " << configFileName);
00170                 }
00171         }
00172         else
00173         {
00174                 ROS_INFO_STREAM("No input filters config file given, not using these filters");
00175         }
00176 
00177         if (ros::param::get("~mapPreFiltersConfig", configFileName))
00178         {
00179                 ifstream ifs(configFileName.c_str());
00180                 if (ifs.good())
00181                 {
00182                         mapPreFilters = PM::DataPointsFilters(ifs);
00183 
00184                         // Add the bounding box
00185                         PM::Parameters params;
00186                         params["removeInside"] = toParam(0);
00187                         params["xMin"] = toParam(-size_x/2);
00188                         params["xMax"] = toParam(size_x/2);
00189                         params["yMin"] = toParam(-size_y/2);
00190                         params["yMax"] = toParam(size_y/2);
00191                         params["zMin"] = toParam(-size_z/2);
00192                         params["zMax"] = toParam(size_z/2);
00193 
00194                         PM::DataPointsFilter* box = 
00195                                 PM::get().DataPointsFilterRegistrar.create("BoundingBoxDataPointsFilter", params);
00196                         mapPreFilters.push_back(box);   
00197 
00198                 }
00199                 else
00200                 {
00201                         ROS_ERROR_STREAM("Cannot load map pre-filters config from YAML file " << configFileName);
00202                 }
00203         }
00204         else
00205         {
00206                 ROS_INFO_STREAM("No map pre-filters config file given, not using these filters");
00207         }
00208 
00209         if (ros::param::get("~mapPostFiltersConfig", configFileName))
00210         {
00211                 ifstream ifs(configFileName.c_str());
00212                 if (ifs.good())
00213                 {
00214                         mapPostFilters = PM::DataPointsFilters(ifs);
00215                 }
00216                 else
00217                 {
00218                         ROS_ERROR_STREAM("Cannot load map post-filters config from YAML file " << configFileName);
00219                 }
00220         }
00221         else
00222         {
00223                 ROS_INFO_STREAM("No map post-filters config file given, not using these filters");
00224         }
00225         
00226         // topics and services initialization
00227         sync.registerCallback(boost::bind(&MapMaintener::gotCloud, this, _1, _2));
00228 
00229         //cloudSub = n.subscribe("cloud_in", inputQueueSize, &MapMaintener::gotCloud, this);
00230         mapPub = n.advertise<sensor_msgs::PointCloud2>("object_map", 2, true);
00231         
00232         //getPointMapSrv = n.advertiseService("dynamic_point_map", &MapMaintener::getPointMap, this);
00233         //saveMapSrv = pn.advertiseService("save_map", &MapMaintener::saveMap, this);
00234         //resetSrv = pn.advertiseService("reset", &MapMaintener::reset, this);
00235 
00236         // Setup interactive maker
00237   server.reset( new InteractiveMarkerServer("MapCenter","", false) );
00238 
00239         h_single = menu_handler.insert( "Record single scan", boost::bind(&MapMaintener::singleScanCallback, this, _1));
00240         h_start = menu_handler.insert( "Start object mapping", boost::bind(&MapMaintener::startMapCallback, this, _1));
00241         h_pause = menu_handler.insert( "Pause object mapping", boost::bind(&MapMaintener::pauseMapCallback, this, _1));
00242   h_stop = menu_handler.insert( "Stop/reset object mapping", boost::bind(&MapMaintener::stopMapCallback, this, _1));
00243         menu_handler.setVisible(h_stop, false);
00244         h_save = menu_handler.insert( "Save object to VTK", boost::bind(&MapMaintener::saveMapCallback, this, _1));
00245 
00246   makeMenuMarker( "object_menu" );
00247   menu_handler.apply( *server, "object_menu" );
00248         menu_handler.reApply( *server);
00249   server->applyChanges();
00250         
00251 }
00252 
00253 MapMaintener::~MapMaintener()
00254 {
00255         server.reset();
00256 
00257         // save point cloud
00258         if (mapPointCloud.features.cols())
00259         {
00260                 mapPointCloud.save(vtkFinalMapName);
00261         }
00262 }
00263 
00264 
00265 // Callback
00266 void MapMaintener::gotCloud(const sensor_msgs::PointCloud2ConstPtr& cloudMsgIn, const nav_msgs::OdometryConstPtr& odom)
00267 {
00268         // IMPORTANT:  We need to receive the point clouds in local coordinates (scanner or robot)
00269 
00270         if(mappingActive || singleScan)
00271         {
00272                 // Convert pointcloud2 to DataPoint
00273                 DP cloud(PointMatcher_ros::rosMsgToPointMatcherCloud<float>(*cloudMsgIn));
00274                 // Convert odometry msg to TransformationParameters
00275                 TP TScannerToMap =      PointMatcher_ros::odomMsgToEigenMatrix<float>(*odom);
00276                 processCloud(cloud, TScannerToMap);
00277         }
00278 
00279         singleScan = false;
00280 }
00281 
00282 // Point cloud processing
00283 void MapMaintener::processCloud(DP newPointCloud, const TP TScannerToMap)
00284 {
00285         string reason;
00286         timer t;
00287         
00288         // Convert point cloud
00289         const size_t goodCount(newPointCloud.features.cols());
00290         if (goodCount == 0)
00291         {
00292                 ROS_ERROR("I found no good points in the cloud");
00293                 return;
00294         }
00295         
00296         ROS_INFO("Processing new point cloud");
00297         {
00298                 timer t; // Print how long take the algo
00299                 
00300                 // Apply filters to incoming cloud, in scanner coordinates
00301                 inputFilters.apply(newPointCloud);
00302                 
00303                 ROS_INFO_STREAM("Input filters took " << t.elapsed() << " [s]");
00304         }
00305         
00306         
00307         // Correct new points using ICP result and move them in their own frame
00308         //cout << "TObjectToMap: " << endl << TObjectToMap << endl;
00309         //cout << "TScannerToMap: " << endl << TScannerToMap << endl;
00310         //cout << "concat: " << endl << TObjectToMap.inverse() * TScannerToMap << endl;
00311         newPointCloud = transformation->compute(newPointCloud, transformation->correctParameters(TObjectToMap.inverse() * TScannerToMap)); 
00312         
00313         // Preparation of cloud for inclusion in map
00314         mapPreFilters.apply(newPointCloud);
00315         
00316         // FIXME put that as parameter
00317         if(newPointCloud.features.cols() < 400)
00318                 return;
00319 
00320         // Generate first map
00321         if(!mapPointCloud.features.rows())      
00322         {
00323                 mapPointCloud = newPointCloud;
00324                 return;
00325         }
00326         
00327 
00328         // Check dimension
00329         if (newPointCloud.features.rows() != mapPointCloud.features.rows())
00330         {
00331                 ROS_ERROR_STREAM("Dimensionality missmatch: incoming cloud is " << newPointCloud.features.rows()-1 << " while map is " << mapPointCloud.features.rows()-1);
00332                 return;
00333         }
00334 
00335 
00336         PM::TransformationParameters Tcorr;
00337         try
00338         {
00339                 Tcorr = icp(newPointCloud, mapPointCloud);
00340                 TObjectToMap = TObjectToMap * Tcorr.inverse();
00341         }
00342         catch (PM::ConvergenceError error)
00343         {
00344                 ROS_WARN_STREAM("ICP failed to converge: " << error.what());
00345                 return;
00346         }
00347 
00348 
00349         const double estimatedOverlap = icp.errorMinimizer->getOverlap();
00350         if(estimatedOverlap < 0.40)
00351         {
00352                 ROS_WARN_STREAM("Estimated overlap too small: " << estimatedOverlap);
00353                 return;
00354         }
00355 
00356         ROS_DEBUG_STREAM("Tcorr:\n" << Tcorr);
00357 
00358         cout << "Tcorr: " << endl << Tcorr << endl;
00359         newPointCloud = transformation->compute(newPointCloud, Tcorr); 
00360         // Merge point clouds to map
00361         mapPointCloud.concatenate(newPointCloud);
00362 
00363         // Map maintenance
00364         mapPostFilters.apply(mapPointCloud);
00365         
00366         ROS_INFO_STREAM("New map available (" << mapPointCloud.features.cols() << " pts), update took " << t.elapsed() << " [s]");
00367 
00368         // Publish map point cloud
00369         // FIXME this crash when used without descriptor
00370         if (mapPub.getNumSubscribers())
00371                 mapPub.publish(PointMatcher_ros::pointMatcherCloudToRosMsg<float>(mapPointCloud, objectFrame, ros::Time::now()));
00372 
00373         ROS_INFO_STREAM("Total map merging: " << t.elapsed() << " [s]");
00374 
00375         //ros::Rate r(2);
00376         //r.sleep();
00377 }
00378 
00379 
00380 
00381 // SERVICES definitions
00382 //bool MapMaintener::getPointMap(map_msgs::GetPointMap::Request &req, map_msgs::GetPointMap::Response &res)
00383 //{
00384 //      if (!mapPointCloud)
00385 //              return false;
00386 //      
00387 //      res.map = PointMatcher_ros::pointMatcherCloudToRosMsg<float>(*mapPointCloud, mapFrame, ros::Time::now());
00388 //      return true;
00389 //}
00390 //
00391 //bool MapMaintener::saveMap(map_msgs::SaveMap::Request &req, map_msgs::SaveMap::Response &res)
00392 //{
00393 //      if (!mapPointCloud)
00394 //              return false;
00395 //      
00396 //      try
00397 //      {
00398 //              mapPointCloud->save(req.filename.data);
00399 //      }
00400 //      catch (const std::runtime_error& e)
00401 //      {
00402 //              return false;
00403 //      }
00404 //      
00405 //      return true;
00406 //}
00407 //
00408 //bool MapMaintener::reset(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
00409 //{
00410 //      // note: no need for locking as we do ros::spin(), to update if we go for multi-threading
00411 //      TOdomToMap = PM::TransformationParameters::Identity(4,4);
00412 //      icp.clearMap();
00413 //      return true;
00414 //}
00415 
00416 //================================================
00417 // Markers
00418 void MapMaintener::makeMenuMarker( std::string name )
00419 {
00420         InteractiveMarker int_marker;
00421 
00422         // Header
00423         // TODO: adapt that for real system
00424         int_marker.header.frame_id = mapFrame;
00425         int_marker.pose.position.x = 0.0;
00426         int_marker.pose.position.y = 0.0;
00427         int_marker.pose.position.z = 0.0;
00428 
00429         int_marker.scale = 1;
00430 
00431         // Information
00432         int_marker.name = name;
00433         //int_marker.description = "Move to the zone of interest";
00434         
00435         // Create 6 DoF control axis
00436         addRotAndTransCtrl(int_marker, 1, 1, 0, 0, "x");
00437         addRotAndTransCtrl(int_marker, 1, 0, 1, 0, "z");
00438         addRotAndTransCtrl(int_marker, 1, 0, 0, 1, "y");
00439 
00440         // Create a gray box to support a menu
00441         Marker grayBox;
00442   grayBox.type = Marker::CUBE;
00443   grayBox.scale.x = size_x;
00444   grayBox.scale.y = size_y;
00445   grayBox.scale.z = size_z;
00446   grayBox.color.r = 0.5;
00447   grayBox.color.g = 0.5;
00448   grayBox.color.b = 0.5;
00449   grayBox.color.a = 0.5;
00450 
00451         InteractiveMarkerControl control;
00452         control = InteractiveMarkerControl();
00453   control.markers.push_back(grayBox);
00454   control.interaction_mode = InteractiveMarkerControl::BUTTON;
00455         int_marker.controls.push_back(control);
00456         int_marker.scale = std::max(std::max(size_x, size_y), size_z);
00457 
00458   server->insert( int_marker );
00459         server->setCallback(int_marker.name, boost::bind(&MapMaintener::update_tf, this, _1), visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE);
00460 }
00461 
00462 void MapMaintener::addRotAndTransCtrl(InteractiveMarker &int_marker, const double w, const double x, const double y, const double z, const std::string name)
00463 {
00464         InteractiveMarkerControl control;
00465         control.orientation.w = w;
00466         control.orientation.x = x;
00467         control.orientation.y = y;
00468         control.orientation.z = z;
00469         //control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
00470         control.interaction_mode = InteractiveMarkerControl::MOVE_ROTATE;
00471         control.name = "rotate_" + name;
00472 
00473   int_marker.controls.push_back( control );
00474 
00475 }
00476 
00477 // Marker callback
00478 void MapMaintener::update_tf(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
00479 {
00480         //InteractiveMarker int_marker;
00481         //server->get("object_menu", int_marker);
00482 
00483         if(mapPointCloud.features.cols() == 0)
00484         {       
00485                 nav_msgs::Odometry odom;
00486                 odom.pose.pose = feedback->pose;
00487                 publishLock.lock();
00488                 TObjectToMap =  PointMatcher_ros::odomMsgToEigenMatrix<float>(odom);
00489                 publishLock.unlock();
00490                 
00491                 //int_marker.description = "Move to the zone of interest";
00492                 menu_handler.reApply( *server);
00493                 server->applyChanges();
00494         }
00495         else
00496         {
00497                 //int_marker.description = "Reset the map to apply change";
00498                 menu_handler.reApply( *server);
00499                 server->applyChanges();
00500         }
00501 }
00502 
00503 void MapMaintener::singleScanCallback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
00504 {
00505                 singleScan = true;
00506 }
00507 
00508 void MapMaintener::startMapCallback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
00509 {
00510         menu_handler.setVisible(h_start, false);
00511         menu_handler.setVisible(h_pause, true);
00512         menu_handler.setVisible(h_stop, true);
00513         menu_handler.reApply( *server);
00514   server->applyChanges();
00515         mappingActive = true;
00516 }
00517 
00518 void MapMaintener::pauseMapCallback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
00519 {
00520         menu_handler.setVisible(h_start, true);
00521         menu_handler.setVisible(h_pause, false);
00522         menu_handler.setVisible(h_stop, true);
00523         menu_handler.reApply( *server);
00524   server->applyChanges();
00525         mappingActive = false;
00526 }
00527 
00528 void MapMaintener::stopMapCallback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
00529 {
00530         menu_handler.setVisible(h_start, true);
00531         menu_handler.setVisible(h_pause, false);
00532         menu_handler.setVisible(h_stop, false);
00533         menu_handler.reApply( *server);
00534   server->applyChanges();
00535         mappingActive = false;
00536 
00537         mapPointCloud = PM::DataPoints();
00538         update_tf(feedback);
00539 }
00540 
00541 void MapMaintener::saveMapCallback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
00542 {
00543         // save point cloud
00544         if (mapPointCloud.features.cols())
00545         {
00546                 mapPointCloud.save(vtkFinalMapName);
00547         }
00548 }
00549 
00550 // Main function supporting the MapMaintener class
00551 int main(int argc, char **argv)
00552 {
00553         ros::init(argc, argv, "map_maintainer");
00554         ros::NodeHandle n;
00555         ros::NodeHandle pn("~");
00556         MapMaintener map(n, pn);
00557 
00558         tf::TransformBroadcaster tfBroadcaster;
00559 
00560         ros::Rate r(50);
00561         while(ros::ok())
00562         {
00563                 map.publishLock.lock();
00564                 tfBroadcaster.sendTransform(PointMatcher_ros::eigenMatrixToStampedTransform<float>(map.TObjectToMap,  map.mapFrame, map.objectFrame, ros::Time::now()));
00565                 map.publishLock.unlock();
00566                 ros::spinOnce();
00567                 r.sleep();
00568         }
00569         
00570         return 0;
00571 }


ethzasl_icp_mapper
Author(s): François Pomerleau and Stéphane Magnenat
autogenerated on Mon Oct 6 2014 10:28:23