interact_mapper.cpp
Go to the documentation of this file.
00001 #include <interactive_markers/interactive_marker_server.h>
00002 #include <interactive_markers/menu_handler.h>
00003 
00004 #include "ros/ros.h"
00005 #include "ros/console.h"
00006 
00007 #include "pointmatcher/PointMatcher.h"
00008 #include "pointmatcher/Timer.h"
00009 
00010 #include "pointmatcher_ros/transform.h"
00011 #include "pointmatcher_ros/get_params_from_server.h"
00012 
00013 #include "nav_msgs/Odometry.h"
00014 #include "geometry_msgs/Pose.h"
00015 
00016 #include "tf/transform_broadcaster.h"
00017 #include "tf/transform_datatypes.h"
00018 #include "tf_conversions/tf_eigen.h"
00019 #include "tf/transform_listener.h"
00020 #include "eigen_conversions/eigen_msg.h"
00021 
00022 // Services
00023 #include "map_msgs/SaveMap.h"
00024 #include "ethzasl_icp_mapper/LoadMap.h"
00025 #include "ethzasl_icp_mapper/CorrectPose.h"
00026 #include "ethzasl_icp_mapper/SetMode.h"
00027 #include "ethzasl_icp_mapper/GetMode.h"
00028 #include "ethzasl_icp_mapper/GetBoundedMap.h"
00029 
00030 #include "boost/algorithm/string.hpp"
00031 #include "boost/filesystem.hpp"
00032 #include "boost/filesystem/path.hpp"
00033 #include "boost/filesystem/operations.hpp"
00034 #include "boost/lexical_cast.hpp"
00035 
00036 // ugly test depending on roscpp because tf_conversions is not properly versionized
00037 #if !ROS_VERSION_MINIMUM(1, 9, 30)
00038         #define transformTFToEigen TransformTFToEigen
00039         #define transformEigenToTF TransformEigenToTF
00040 #endif // !ROS_VERSION_MINIMUM(1, 9, 30)
00041 
00042 
00043 
00044 using namespace std;
00045 using namespace PointMatcherSupport;
00046 using namespace visualization_msgs;
00047 using namespace interactive_markers;
00048 namespace fs = ::boost::filesystem;
00049 
00050 class InteractMapper
00051 {
00052 public:
00053         typedef PointMatcher<float> PM;
00054 
00055         ros::NodeHandle& n;
00056         ros::NodeHandle& pn;
00057 
00058         int states_updatePose;
00059 
00060         // Parameters
00061         const string mapPath;
00062         const string baseFrame;
00063         const string odomFrame;
00064         const string mapFrame;
00065         
00066         // Publisher
00067         ros::Publisher test_mapPub;
00068 
00069         // Services
00070         ros::ServiceClient saveMapClient;
00071         ros::ServiceClient loadMapClient;
00072         ros::ServiceClient correctMapClient;
00073         ros::ServiceClient setModeClient;
00074         ros::ServiceClient getModeClient;
00075         // test
00076         ros::ServiceClient getBoundedMapClient;
00077 
00078         // Interative markers
00079         boost::shared_ptr<InteractiveMarkerServer> server;
00080         MenuHandler menu_handler;
00081         MenuHandler::EntryHandle h_localize;
00082         MenuHandler::EntryHandle h_map;
00083         MenuHandler::EntryHandle h_load;
00084         MenuHandler::EntryHandle h_save;
00085         MenuHandler::EntryHandle h_adjustPose;
00086         MenuHandler::EntryHandle h_getboundedMap;
00087 
00088         tf::TransformListener tfListener;
00089         geometry_msgs::Pose markerPose;
00090 
00091         InteractMapper(ros::NodeHandle& n, ros::NodeHandle& pn);
00092         ~InteractMapper();
00093 
00094 protected:
00095         void localizeCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback );
00096         void mapCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback );
00097         void saveMapCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback );
00098         void loadMapCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback );
00099         void adjustPoseCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback );
00100         void getBoundedMapCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback );
00101         
00102         void update_tf(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback );
00103 
00104 private:
00105         void makeMenuMarker(string name);
00106         void addRotAndTransCtrl(InteractiveMarker &int_marker, const double w, const double x, const double y, const double z, const std::string name);
00107 };
00108 
00109 
00110 InteractMapper::InteractMapper(ros::NodeHandle& n, ros::NodeHandle& pn):
00111         n(n),
00112         pn(pn),
00113         mapPath(getParam<string>("map_path", "/home/frank/.ros/3D_maps")),
00114         baseFrame(getParam<string>("base_frame", "base_link")),
00115         odomFrame(getParam<string>("odom_frame", "odom")),
00116         mapFrame(getParam<string>("map_frame", "map")),
00117   tfListener(ros::Duration(30))
00118 {
00119         ROS_INFO_STREAM("Waiting for a mapper node...");
00120         ros::service::waitForService("mapper/set_mode");
00121         ROS_INFO_STREAM("Mapper node found");
00122         
00123         states_updatePose = 0;
00124 
00125         // Publisher
00126         test_mapPub = n.advertise<sensor_msgs::PointCloud2>("debug_bounded_map", 2, true);
00127 
00128         // Service setups
00129         // TODO: remove private tags
00130         saveMapClient = n.serviceClient<map_msgs::SaveMap>("mapper/save_map");
00131         loadMapClient = n.serviceClient<ethzasl_icp_mapper::LoadMap>("mapper/load_map");
00132         correctMapClient = n.serviceClient<ethzasl_icp_mapper::CorrectPose>("mapper/correct_pose");
00133         setModeClient = n.serviceClient<ethzasl_icp_mapper::SetMode>("mapper/set_mode");
00134         getModeClient = n.serviceClient<ethzasl_icp_mapper::GetMode>("mapper/get_mode");
00135         getBoundedMapClient = n.serviceClient<ethzasl_icp_mapper::GetBoundedMap>("mapper/get_bounded_map");
00136 
00137         // Setup interactive maker
00138         server.reset( new InteractiveMarkerServer("MapControl","", false) );
00139         h_localize = menu_handler.insert( "Localize", boost::bind(&InteractMapper::localizeCallback, this, _1));
00140         h_map = menu_handler.insert( "Map", boost::bind(&InteractMapper::mapCallback, this, _1));
00141         h_save = menu_handler.insert( "Save map to VTK", boost::bind(&InteractMapper::saveMapCallback, this, _1));
00142         h_load = menu_handler.insert( "Load map from VTK");
00143 
00144         h_adjustPose = menu_handler.insert( "Correct map pose", boost::bind(&InteractMapper::adjustPoseCallback, this, _1));
00145         
00146         h_getboundedMap = menu_handler.insert( "Publish 3x3 meter map", boost::bind(&InteractMapper::getBoundedMapCallback, this, _1));
00147         
00148         // Fetch states of the mapper node
00149         ethzasl_icp_mapper::GetMode srv;
00150         getModeClient.call(srv);
00151         
00152         if(srv.response.localize)
00153                 menu_handler.setCheckState(h_localize, MenuHandler::CHECKED);
00154         else
00155                 menu_handler.setCheckState(h_localize, MenuHandler::UNCHECKED);
00156         if(srv.response.map)
00157                 menu_handler.setCheckState(h_map, MenuHandler::CHECKED);
00158         else
00159                 menu_handler.setCheckState(h_map, MenuHandler::UNCHECKED);
00160         
00161         menu_handler.setCheckState(h_adjustPose, MenuHandler::UNCHECKED);
00162 
00163         makeMenuMarker( "map_menu" );
00164   
00165         // Generate load menu based on map_path
00166         fs::path path(mapPath);
00167         if(!(fs::exists(path) && fs::is_directory(path)))
00168         {
00169                 ROS_ERROR_STREAM("Path " << mapPath << " does not exist");
00170         }
00171         else
00172         {
00173                 fs::directory_iterator end_iter;
00174                 for ( fs::directory_iterator dir_itr( path );
00175                                         dir_itr != end_iter;
00176                                         ++dir_itr )
00177                 {
00178                         try
00179                         {
00180                                 if ( fs::is_regular_file( dir_itr->status()) )
00181                                 {
00182                                         if(dir_itr->path().extension() == ".vtk")
00183                                         {
00184                                                 #if BOOST_FILESYSTEM_VERSION >= 3
00185                                                 menu_handler.insert(h_load, dir_itr->path().filename().string(), boost::bind(&InteractMapper::loadMapCallback, this, _1) );
00186                                                 #else
00187                                                 menu_handler.insert(h_load, dir_itr->path().filename(), boost::bind(&InteractMapper::loadMapCallback, this, _1) );
00188                                                 #endif
00189                                         }
00190                                 }
00191                         }
00192                         catch ( const std::exception & ex )
00193                         {
00194                                 std::cout << dir_itr->path().filename() << " " << ex.what() << std::endl;
00195                         }
00196                 }
00197         }
00198 
00199         menu_handler.apply( *server, "map_menu" );
00200         menu_handler.reApply( *server);
00201   server->applyChanges();
00202 
00203 }
00204 
00205 InteractMapper::~InteractMapper()
00206 {
00207         
00208 }
00209 
00210 
00211 //================================================
00212 // Callbacks
00213 void InteractMapper::localizeCallback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
00214 {
00215         ethzasl_icp_mapper::SetMode srv;
00216 
00217         MenuHandler::CheckState state_loc;
00218         MenuHandler::CheckState state_map;
00219         menu_handler.getCheckState(h_localize, state_loc);
00220         menu_handler.getCheckState(h_map, state_map);
00221 
00222         if(state_loc == MenuHandler::CHECKED)
00223         {
00224                 menu_handler.setCheckState(h_localize, MenuHandler::UNCHECKED);
00225                 menu_handler.setCheckState(h_map, MenuHandler::UNCHECKED);
00226                 srv.request.localize = false;
00227                 srv.request.map = false;
00228         }
00229         else
00230         {
00231                 menu_handler.setCheckState(h_localize, MenuHandler::CHECKED);
00232                 menu_handler.setCheckState(h_map, MenuHandler::UNCHECKED);
00233                 srv.request.localize = true;
00234                 srv.request.map = false;
00235         }
00236         
00237         menu_handler.reApply( *server );
00238         server->applyChanges();
00239         
00240         srv.request.applyChange = true;
00241         setModeClient.call(srv);
00242 }
00243 
00244 void InteractMapper::mapCallback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
00245 {
00246         ethzasl_icp_mapper::SetMode srv;
00247 
00248         MenuHandler::CheckState state_loc;
00249         MenuHandler::CheckState state_map;
00250         menu_handler.getCheckState(h_localize, state_loc);
00251         menu_handler.getCheckState(h_map, state_map);
00252 
00253         if(state_map == MenuHandler::CHECKED)
00254         {
00255                 menu_handler.setCheckState(h_map, MenuHandler::UNCHECKED);
00256                 srv.request.map = false;
00257                 if(state_loc == MenuHandler::CHECKED)
00258                         srv.request.localize = true;
00259                 else
00260                         srv.request.localize = false;
00261         }
00262         else
00263         {
00264                 menu_handler.setCheckState(h_localize, MenuHandler::CHECKED);
00265                 menu_handler.setCheckState(h_map, MenuHandler::CHECKED);
00266                 srv.request.localize = true;
00267                 srv.request.map = true;
00268         }
00269         
00270         menu_handler.reApply( *server );
00271         server->applyChanges();
00272         
00273         srv.request.applyChange = true;
00274         setModeClient.call(srv);
00275 }
00276 
00277 void InteractMapper::saveMapCallback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
00278 {
00279         map_msgs::SaveMap srv;
00280         srv.request.filename.data = mapPath + "/default_map.vtk";
00281         saveMapClient.call(srv);
00282 }
00283 
00284 void InteractMapper::loadMapCallback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
00285 {
00286         ethzasl_icp_mapper::SetMode srv_states;
00287         menu_handler.setCheckState(h_localize, MenuHandler::UNCHECKED);
00288         menu_handler.setCheckState(h_map, MenuHandler::UNCHECKED);
00289         srv_states.request.localize = false;
00290         srv_states.request.map = false;
00291         setModeClient.call(srv_states);
00292 
00293         
00294         MenuHandler::EntryHandle handle = feedback->menu_entry_id;
00295         string filename;
00296         menu_handler.getTitle(handle, filename);
00297 
00298         ROS_INFO_STREAM("Loading " << filename);
00299         
00300         ethzasl_icp_mapper::LoadMap srv;
00301         srv.request.filename.data = mapPath + "/" + filename;
00302         loadMapClient.call(srv);
00303 
00304         menu_handler.reApply( *server );
00305   server->applyChanges();
00306 }
00307 
00308 void InteractMapper::adjustPoseCallback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
00309 {
00310         MenuHandler::CheckState state;
00311         menu_handler.getCheckState(h_adjustPose, state);
00312         
00313         if(state == MenuHandler::UNCHECKED)
00314         {
00315                 cerr << "checked " << states_updatePose << endl;
00316 
00317                 if(states_updatePose == 0)
00318                         states_updatePose = 1;
00319                 else
00320                         states_updatePose = 0;
00321 
00322         }
00323 
00324         if(state == MenuHandler::CHECKED)
00325         {
00326                 cerr << "unchecked " << states_updatePose << endl;
00327 
00328                 if(states_updatePose == 1)
00329                         states_updatePose = 2;
00330                 else
00331                         states_updatePose = 0;
00332 
00333         }
00334         
00335         if(states_updatePose == 2)
00336         {
00337                 states_updatePose = 0;
00338 
00339                 ROS_INFO_STREAM("Updating giving pose");
00340 
00341                 nav_msgs::Odometry odom;
00342                 odom.pose.pose = feedback->pose;
00343                 markerPose = feedback->pose;
00344 
00345                 const PM::TransformationParameters TMarkerToMap =
00346                         PointMatcher_ros::odomMsgToEigenMatrix<float>(odom);
00347 
00348                 const ros::Time stamp = ros::Time::now();
00349 
00350                 tfListener.waitForTransform(mapFrame, baseFrame, stamp, ros::Duration(0.5));
00351                 const PM::TransformationParameters TBaseToMap = 
00352                         PointMatcher_ros::transformListenerToEigenMatrix<float>(
00353                                 tfListener,
00354                                 mapFrame,
00355                                 baseFrame,
00356                                 stamp
00357                         );
00358                         
00359                 tfListener.waitForTransform(odomFrame, baseFrame, stamp, ros::Duration(0.5));
00360                 const PM::TransformationParameters TBaseToOdom = 
00361                         PointMatcher_ros::transformListenerToEigenMatrix<float>(
00362                                 tfListener,
00363                                 odomFrame,
00364                                 baseFrame,
00365                                 stamp
00366                         );
00367                 
00368         //      const PM::TransformationParameters TOdomToMap = 
00369         //              PointMatcher_ros::transformListenerToEigenMatrix<float>(
00370         //                      tfListener,
00371         //                      mapFrame,
00372         //                      odomFrame,
00373         //                      ros::Time::now()
00374         //              );
00375         //      
00376                 const PM::TransformationParameters TCorr =
00377                          TMarkerToMap * TBaseToOdom.inverse();
00378 
00379                 //cout << "TMarkerToMap:\n" << TMarkerToMap << endl;
00380                 //cout << "TOdomToMap:\n" << TOdomToMap << endl;
00381                 //cout << "TCorr:\n" << TCorr << endl;
00382 
00383                 ethzasl_icp_mapper::CorrectPose srv;
00384 
00385                 srv.request.odom = PointMatcher_ros::eigenMatrixToOdomMsg<float>(TCorr, mapFrame, ros::Time::now());
00386 
00387                 correctMapClient.call(srv);
00388 
00389                 //InteractiveMarker int_marker;
00390                 //server->get("map_menu", int_marker);
00391                 //makeMenuMarker("map_menu");
00392                 
00393                 
00394                 menu_handler.reApply( *server );
00395                 server->applyChanges();
00396                 //ros::Duration(0.1).sleep();
00397         }
00398 
00399         if(state == MenuHandler::CHECKED)
00400                 menu_handler.setCheckState(h_adjustPose, MenuHandler::UNCHECKED);
00401         else
00402         {
00403                 menu_handler.setCheckState(h_adjustPose, MenuHandler::CHECKED);
00404                 
00405                 ethzasl_icp_mapper::SetMode srv_states;
00406                 menu_handler.setCheckState(h_localize, MenuHandler::UNCHECKED);
00407                 menu_handler.setCheckState(h_map, MenuHandler::UNCHECKED);
00408                 srv_states.request.localize = false;
00409                 srv_states.request.map = false;
00410                 setModeClient.call(srv_states);
00411         }
00412         
00413         menu_handler.reApply( *server );
00414   server->applyChanges();
00415 }
00416 
00417 void InteractMapper::update_tf(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
00418 {       
00419         MenuHandler::CheckState state;
00420         menu_handler.getCheckState(h_adjustPose, state);
00421         
00422 
00423         
00424 }
00425 
00426 void InteractMapper::getBoundedMapCallback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
00427 {
00428         ethzasl_icp_mapper::GetBoundedMap srv;
00429         srv.request.topRightCorner.x = 1.5;
00430         srv.request.topRightCorner.y = 1.5;
00431         srv.request.topRightCorner.z = 1.5;
00432 
00433         srv.request.bottomLeftCorner.x = -1.5;
00434         srv.request.bottomLeftCorner.y = -1.5;
00435         srv.request.bottomLeftCorner.z = -1.5;
00436 
00437         // FIXME: throw exeception: finish debug here...
00438         tf::StampedTransform stampedTr;
00439         ros::Time stamp = ros::Time::now();
00440         tfListener.waitForTransform(mapFrame, baseFrame, stamp, ros::Duration(2));
00441         tfListener.lookupTransform(mapFrame, baseFrame, stamp, stampedTr);
00442         
00443         Eigen::Affine3d eigenTr;
00444         tf::transformTFToEigen(stampedTr, eigenTr);
00445 
00446         srv.request.mapCenter.orientation.x = eigenTr.rotation().x();
00447         srv.request.mapCenter.orientation.z = eigenTr.rotation().y();
00448         srv.request.mapCenter.orientation.y = eigenTr.rotation().z();
00449         srv.request.mapCenter.orientation.w = eigenTr.rotation().w();
00450         srv.request.mapCenter.position.x = eigenTr.translation().x();
00451         srv.request.mapCenter.position.y = eigenTr.translation().y();
00452         srv.request.mapCenter.position.z = eigenTr.translation().z();
00453         getBoundedMapClient.call(srv);
00454 
00455         test_mapPub.publish(srv.response.boundedMap);
00456 
00457 }
00458 
00459 
00460 //================================================
00461 // Markers
00462 void InteractMapper::makeMenuMarker(string name)
00463 {
00464         InteractiveMarker int_marker;
00465 
00466         // Header
00467         int_marker.header.frame_id = mapFrame;
00468         int_marker.pose.position.x = 0.0;
00469         int_marker.pose.position.y = 0.0;
00470         int_marker.pose.position.z = 0.0;
00471 
00472         int_marker.scale = 1;
00473 
00474         // Information
00475         int_marker.name = name;
00476         //int_marker.description = "Move to the zone of interest";
00477         
00478         // Create 6 DoF control axis
00479         addRotAndTransCtrl(int_marker, 1, 1, 0, 0, "x");
00480         addRotAndTransCtrl(int_marker, 1, 0, 1, 0, "z");
00481         addRotAndTransCtrl(int_marker, 1, 0, 0, 1, "y");
00482 
00483         // Create a gray box to support a menu
00484         Marker grayBox;
00485   grayBox.type = Marker::SPHERE;
00486   grayBox.scale.x = 2.5;
00487   grayBox.scale.y = 2.5;
00488   grayBox.scale.z = 2.5;
00489 
00490   grayBox.color.r = 0.95;
00491   grayBox.color.g = 0.95;
00492   grayBox.color.b = 0.85;
00493   grayBox.color.a = 0.5;
00494 
00495         InteractiveMarkerControl control;
00496         control = InteractiveMarkerControl();
00497   control.markers.push_back(grayBox);
00498   control.interaction_mode = InteractiveMarkerControl::BUTTON;
00499         int_marker.controls.push_back(control);
00500 
00501         int_marker.scale = 2.5;
00502 
00503   server->insert( int_marker );
00504         server->setCallback(int_marker.name, boost::bind(&InteractMapper::update_tf, this, _1), visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE);
00505 }
00506 
00507 void InteractMapper::addRotAndTransCtrl(InteractiveMarker &int_marker, const double w, const double x, const double y, const double z, const std::string name)
00508 {
00509         InteractiveMarkerControl control;
00510         control.orientation.w = w;
00511         control.orientation.x = x;
00512         control.orientation.y = y;
00513         control.orientation.z = z;
00514         //control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
00515         control.interaction_mode = InteractiveMarkerControl::MOVE_ROTATE;
00516         control.name = "rotate_" + name;
00517 
00518   int_marker.controls.push_back( control );
00519 
00520 }
00521 
00522 // Main function supporting the InteractMapper class
00523 int main(int argc, char **argv)
00524 {
00525         ros::init(argc, argv, "interact_mapper");
00526         ros::NodeHandle n;
00527         ros::NodeHandle pn("~");
00528         InteractMapper interact(n, pn);
00529         
00530         tf::StampedTransform stampedTr;
00531 
00532         ros::Rate r(10);
00533         while(ros::ok())
00534         {
00535                 if(interact.states_updatePose != 1)
00536                 {
00537                         ros::Time stamp = ros::Time::now();
00538                         if (interact.tfListener.waitForTransform(interact.mapFrame, interact.baseFrame, stamp, ros::Duration(1)))
00539                         {
00540                                 interact.tfListener.lookupTransform(interact.mapFrame, interact.baseFrame, stamp, stampedTr);
00541 
00542                                 InteractiveMarker int_marker;
00543                                 interact.server->get("map_menu", int_marker);
00544                                 int_marker.pose.position.x = stampedTr.getOrigin().x();
00545                                 int_marker.pose.position.y = stampedTr.getOrigin().y();
00546                                 int_marker.pose.position.z = stampedTr.getOrigin().z();
00547                                 int_marker.pose.orientation.x = stampedTr.getRotation().x();
00548                                 int_marker.pose.orientation.y = stampedTr.getRotation().y();
00549                                 int_marker.pose.orientation.z = stampedTr.getRotation().z();
00550                                 int_marker.pose.orientation.w = stampedTr.getRotation().w();
00551 
00552                                 interact.server->insert(int_marker);
00553                                 interact.server->applyChanges();
00554                         }
00555                         else
00556                         {
00557                                 ROS_WARN_STREAM("Cannot find transformation from " << interact.mapFrame << " to " << interact.baseFrame);
00558                         }
00559                 }
00560                 
00561 
00562                 ros::spinOnce();
00563                 r.sleep();
00564         }
00565         
00566         return 0;
00567 }


ethzasl_icp_mapper
Author(s): François Pomerleau and Stéphane Magnenat
autogenerated on Tue Mar 3 2015 15:28:44