CGraphSlamHandler_ROS_impl.h
Go to the documentation of this file.
00001 /* +---------------------------------------------------------------------------+
00002    |                     Mobile Robot Programming Toolkit (MRPT)               |
00003    |                          http://www.mrpt.org/                             |
00004    |                                                                           |
00005    | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file        |
00006    | See: http://www.mrpt.org/Authors - All rights reserved.                   |
00007    | Released under BSD License. See details in http://www.mrpt.org/License    |
00008    +---------------------------------------------------------------------------+ */
00009 #ifndef CGRAPHSLAMHANDLER_ROS_IMPL_H
00010 #define CGRAPHSLAMHANDLER_ROS_IMPL_H
00011 
00012 namespace mrpt { namespace graphslam { namespace apps {
00013 
00014 using mrpt::utils::LVL_DEBUG;
00015 using mrpt::utils::LVL_INFO;
00016 using mrpt::utils::LVL_WARN;
00017 using mrpt::utils::LVL_ERROR;
00018 
00019 // static member variables
00020 template<class GRAPH_T>
00021 const std::string CGraphSlamHandler_ROS<GRAPH_T>::sep_header(40, '=');
00022 
00023 template<class GRAPH_T>
00024 const std::string CGraphSlamHandler_ROS<GRAPH_T>::sep_subheader(20, '-');
00025 
00026 // Ctor
00027 template<class GRAPH_T>
00028 CGraphSlamHandler_ROS<GRAPH_T>::CGraphSlamHandler_ROS(
00029         mrpt::utils::COutputLogger* logger,
00030         TUserOptionsChecker<GRAPH_T>* options_checker,
00031         ros::NodeHandle* nh_in):
00032   m_nh(nh_in),
00033   parent_t(logger, options_checker, /*enable_visuals=*/ false)
00034 {
00035   using namespace mrpt::obs;
00036 
00037   ASSERT_(m_nh);
00038 
00039   // TODO - does this affect?
00040   // Previous value = 0;
00041   m_queue_size = 1;
00042 
00043   // variables initialization/assignment
00044   m_pub_seq = 0;
00045   m_stats_pub_seq = 0;
00046   this->resetReceivedFlags();
00047 
00048   // measurements initialization
00049   m_mrpt_odom = CObservationOdometry::Create();
00050   m_mrpt_odom->hasEncodersInfo = false;
00051   m_mrpt_odom->hasVelocities = false;
00052   m_first_time_in_sniff_odom = true;
00053 
00054   m_measurement_cnt = 0;
00055 
00056   // Thu Nov 3 23:36:49 EET 2016, Nikos Koukis
00057   // WARNING: ROS Server Parameters have not been read yet. Make sure you know
00058   // what to initialize at this stage!
00059 }
00060 
00061 template<class GRAPH_T>
00062 CGraphSlamHandler_ROS<GRAPH_T>::~CGraphSlamHandler_ROS() { }
00063 
00064 template<class GRAPH_T>
00065 void CGraphSlamHandler_ROS<GRAPH_T>::readParams() {
00066   this->readROSParameters();
00067 
00068   ASSERT_(!this->m_ini_fname.empty());
00069   parent_t::readConfigFname(this->m_ini_fname);
00070 
00071 }
00072 
00073 template<class GRAPH_T>
00074 void CGraphSlamHandler_ROS<GRAPH_T>::readROSParameters() {
00075   using namespace mrpt::utils;
00076 
00077   // misc
00078   {
00079                 std::string ns = "misc/";
00080 
00081                 // enable/disable visuals
00082                 bool m_disable_MRPT_visuals;
00083                 m_nh->param<bool>(ns + "disable_MRPT_visuals", m_disable_MRPT_visuals, false);
00084                 this->m_enable_visuals = !m_disable_MRPT_visuals;
00085 
00086                 // verbosity level
00087                 int lvl;
00088                 m_nh->param<int>(ns + "verbosity",
00089                                 lvl,
00090                                 static_cast<int>(LVL_INFO));
00091                 m_min_logging_level = static_cast<VerbosityLevel>(lvl);
00092                 this->m_logger->setMinLoggingLevel(m_min_logging_level);
00093   }
00094   // deciders, optimizer
00095   {
00096                 std::string ns = "deciders_optimizers/";
00097                 m_nh->param<std::string>(ns + "NRD", m_node_reg, "CFixedIntervalsNRD");
00098                 m_nh->param<std::string>(ns + "ERD", m_edge_reg, "CICPCriteriaERD");
00099                 m_nh->param<std::string>(ns + "GSO", m_optimizer, "CLevMarqGSO");
00100   }
00101   // filenames
00102   {
00103                 std::string ns = "files/";
00104 
00105                 // configuration file - mandatory
00106                 std::string config_param_path = ns + "config";
00107                 bool found_config = m_nh->getParam(ns + "config", this->m_ini_fname);
00108                 ASSERTMSG_(found_config,
00109                                 mrpt::format(
00110                                 "Configuration file was not set. Set %s and try again.\nExiting...",
00111                                 config_param_path.c_str()));
00112 
00113                 // ground-truth file
00114                 m_nh->getParam(ns + "ground_truth", this->m_gt_fname);
00115   }
00116 
00117   // TF Frame IDs
00118   // names of the frames of the corresponding robot parts
00119   {
00120                 std::string ns = "frame_IDs/";
00121 
00122                 m_nh->param<std::string>(ns + "anchor_frame" , m_anchor_frame_id, "map");
00123                 m_nh->param<std::string>(ns + "base_link_frame" , m_base_link_frame_id, "base_link");
00124                 m_nh->param<std::string>(ns + "odometry_frame" , m_odom_frame_id, "odom");
00125 
00126   }
00127 
00128   // ASSERT that the given user options are valid
00129   // Fill the TuserOptionsChecker related structures
00130   this->m_options_checker->createDeciderOptimizerMappings();
00131   this->m_options_checker->populateDeciderOptimizerProperties();
00132   this->verifyUserInput();
00133 
00134   this->m_logger->logFmt(LVL_DEBUG,
00135                 "Successfully read parameters from ROS Parameter Server");
00136 
00137   // Visuals initialization
00138   if (this->m_enable_visuals) {
00139                 this->initVisualization();
00140   }
00141 } // end of readROSParameters
00142 
00143 template<class GRAPH_T>
00144 void CGraphSlamHandler_ROS<GRAPH_T>::readStaticTFs() { }
00145 
00146 template<class GRAPH_T>
00147 void CGraphSlamHandler_ROS<GRAPH_T>::initEngine_ROS() { 
00148 
00149         this->m_logger->logFmt(LVL_WARN,
00150                         "Initializing CGraphSlamEngine_ROS instance...");
00151         this->m_engine = new CGraphSlamEngine_ROS<GRAPH_T>(
00152                         m_nh,
00153                         this->m_ini_fname,
00154                         /*rawlog_fname=*/ "",
00155                         this->m_gt_fname,
00156                         this->m_win_manager,
00157                         this->m_options_checker->node_regs_map[m_node_reg](),
00158                         this->m_options_checker->edge_regs_map[m_edge_reg](),
00159                         this->m_options_checker->optimizers_map[m_optimizer]());
00160         this->m_logger->logFmt(LVL_WARN,
00161                         "Successfully initialized CGraphSlamEngine_ROS instance.");
00162 
00163 }
00164 
00165 template<class GRAPH_T>
00166 void CGraphSlamHandler_ROS<GRAPH_T>::initEngine_MR() {
00167 
00168   this->m_options_checker->node_regs_map[m_node_reg]();
00169   this->m_options_checker->edge_regs_map[m_edge_reg]();
00170   this->m_options_checker->optimizers_map[m_optimizer]();
00171 
00172   this->m_logger->logFmt(LVL_WARN,
00173           "Initializing CGraphSlamEngine_MR instance...");
00174   this->m_engine = new CGraphSlamEngine_MR<GRAPH_T>(
00175           m_nh,
00176           this->m_ini_fname,
00177           /*rawlog_fname=*/ "",
00178           this->m_gt_fname,
00179           this->m_win_manager,
00180           this->m_options_checker->node_regs_map[m_node_reg](),
00181           this->m_options_checker->edge_regs_map[m_edge_reg](),
00182           this->m_options_checker->optimizers_map[m_optimizer]());
00183   this->m_logger->logFmt(LVL_WARN,
00184           "Successfully initialized CGraphSlamEngine_MR instance.");
00185 
00186 }
00187 
00188 template<class GRAPH_T>
00189 void CGraphSlamHandler_ROS<GRAPH_T>::getROSParameters(std::string* str_out) {
00190   using namespace mrpt::utils;
00191   using namespace std;
00192 
00193   ASSERT_(str_out);
00194 
00195   stringstream ss("");
00196 
00197 
00198   ss << "ROS Parameters: " << endl;
00199   ss << sep_header << endl;
00200   ss << endl;
00201 
00202   ss << "Deciders / Optimizers = " << endl;
00203   ss << sep_subheader << endl;
00204   ss << "Node Registration Decider = " << m_node_reg << endl;
00205   ss << "Edge Registration Decider = " << m_edge_reg << endl;
00206   ss << "GraphSLAM Optimizer       = " << m_optimizer << endl;
00207   ss << endl;
00208 
00209   ss << "Filenames: " << endl;
00210   ss << sep_subheader << endl;
00211   ss << "Configuration .ini file   = " << this->m_ini_fname << endl;
00212   ss << "Ground truth filename     = " << (!this->m_gt_fname.empty()
00213           ? this->m_gt_fname : "NONE")
00214         << endl;
00215   ss << endl;
00216 
00217   ss << "Miscellaneous: " << endl;
00218   ss << sep_subheader << endl;
00219   ss << "Enable MRPT visuals?      = " <<
00220         (this->m_enable_visuals? "TRUE" : "FALSE")
00221         << endl;
00222   ss << "Logging verbosity Level   = " << 
00223         COutputLogger::logging_levels_to_names[m_min_logging_level] << endl;;
00224   ss << endl;
00225 
00226   *str_out = ss.str();
00227 }
00228 
00229 template<class GRAPH_T>
00230 void CGraphSlamHandler_ROS<GRAPH_T>::getParamsAsString(std::string* str_out) {
00231   ASSERT_(str_out);
00232 
00233   // ros parameters
00234   std::string ros_params("");
00235   this->getROSParameters(&ros_params);
00236   std::string mrpt_params("");
00237   parent_t::getParamsAsString(&mrpt_params);
00238 
00239   *str_out += ros_params;
00240   *str_out += "\n\n";
00241   *str_out += mrpt_params;
00242 
00243   // various parameters
00244 }
00245 
00246 template<class GRAPH_T>
00247 std::string CGraphSlamHandler_ROS<GRAPH_T>::getParamsAsString() {
00248   std::string params;
00249   this->getParamsAsString(&params);
00250   return params;
00251 }
00252 
00253 template<class GRAPH_T>
00254 void CGraphSlamHandler_ROS<GRAPH_T>::printParams() {
00255   using namespace std;
00256   parent_t::printParams();
00257   cout << this->getParamsAsString() << endl;
00258 
00259 
00260 }
00261 
00262 template<class GRAPH_T>
00263 void CGraphSlamHandler_ROS<GRAPH_T>::verifyUserInput() {
00264   this->m_logger->logFmt(LVL_DEBUG, "Verifying user input...");
00265 
00266 
00267   // verify the NRD, ERD, GSO parameters
00268   bool node_success, edge_success, optimizer_success;
00269   bool failed = false;
00270 
00271   node_success =
00272         this->m_options_checker->checkRegistrationDeciderExists(
00273                 m_node_reg, "node");
00274   edge_success =
00275         this->m_options_checker->checkRegistrationDeciderExists(
00276                 m_edge_reg, "edge");
00277   optimizer_success =
00278         this->m_options_checker->checkOptimizerExists(
00279                 m_optimizer);
00280 
00281   if (!node_success) {
00282         this->m_logger->logFmt(LVL_ERROR,
00283                 "\nNode Registration Decider \"%s\" is not available",
00284                 m_node_reg.c_str());
00285         this->m_options_checker->dumpRegistrarsToConsole("node");
00286         failed = true;
00287   }
00288   if (!edge_success) {
00289         this->m_logger->logFmt(LVL_ERROR,
00290                 "\nEdge Registration Decider \"%s\" is not available.",
00291                 m_edge_reg.c_str());
00292         this->m_options_checker->dumpRegistrarsToConsole("edge");
00293         failed = true;
00294   }
00295   if (!optimizer_success) {
00296         this->m_logger->logFmt(LVL_ERROR,
00297                 "\ngraphSLAM Optimizser \"%s\" is not available.",
00298                 m_optimizer.c_str());
00299         this->m_options_checker->dumpOptimizersToConsole();
00300         failed = true;
00301   }
00302   ASSERT_(!failed)
00303 
00304         // verify that the path to the files is correct
00305         // .ini file
00306         bool ini_exists = system::fileExists(this->m_ini_fname);
00307   ASSERTMSG_(ini_exists,
00308           format(
00309                 "\n.ini configuration file \"%s\"doesn't exist. "
00310                 "Please specify a valid pathname.\nExiting...\n",
00311                 this->m_ini_fname.c_str()));
00312   // ground-truth file
00313   if (!this->m_gt_fname.empty()) {
00314         bool gt_exists = system::fileExists(this->m_gt_fname);
00315         ASSERTMSG_(gt_exists,
00316                 format(
00317                   "\nGround truth file \"%s\"doesn't exist."
00318                   "Either unset the corresponding ROS parameter or specify a valid pathname.\n"
00319                   "Exiting...\n",
00320                   this->m_gt_fname.c_str()));
00321   }
00322 
00323 } // end of verifyUserInput
00324 
00325 template<class GRAPH_T>
00326 void CGraphSlamHandler_ROS<GRAPH_T>::setupComm() {
00327 
00328   this->m_logger->logFmt(LVL_INFO,
00329           "Setting up ROS-related subscribers, publishers, services...");
00330 
00331   // setup subscribers, publishers, services...
00332   this->setupSubs();
00333   this->setupPubs();
00334   this->setupSrvs();
00335 
00336   // fetch the static geometrical transformations
00337   //this->readStaticTFs();
00338 
00339 } // end of setupComm
00340 
00341 template<class GRAPH_T>
00342 void CGraphSlamHandler_ROS<GRAPH_T>::setupSubs() {
00343   this->m_logger->logFmt(LVL_INFO, "Setting up the subscribers...");
00344 
00345   // setup the names
00346   std::string ns = "input/";
00347 
00348   m_odom_topic = ns + "odom";
00349   m_laser_scan_topic = ns + "laser_scan";
00350 
00351   // odometry
00352   m_odom_sub = m_nh->subscribe<nav_msgs::Odometry>(
00353           m_odom_topic,
00354           m_queue_size,
00355           &self_t::sniffOdom, this);
00356 
00357   // laser_scans
00358   m_laser_scan_sub = m_nh->subscribe<sensor_msgs::LaserScan>(
00359           m_laser_scan_topic,
00360           m_queue_size,
00361           &self_t::sniffLaserScan, this);
00362 
00363   // camera
00364   // TODO
00365 
00366   // 3D point clouds
00367   // TODO
00368 
00369 } // end of setupSubs
00370 
00371 template<class GRAPH_T>
00372 void CGraphSlamHandler_ROS<GRAPH_T>::setupPubs() {
00373   this->m_logger->logFmt(LVL_INFO, "Setting up the publishers...");
00374 
00375   // setup the names
00376   std::string ns = "feedback/";
00377 
00378   m_curr_robot_pos_topic = ns + "robot_position";
00379   m_robot_trajectory_topic = ns + "robot_trajectory";
00380   m_robot_tr_poses_topic = ns + "robot_tr_poses";
00381   m_odom_trajectory_topic = ns + "odom_trajectory";
00382   m_gridmap_topic = ns + "gridmap";
00383   m_stats_topic = ns + "graphslam_stats";
00384 
00385   // setup the publishers
00386 
00387   // agent estimated position
00388   m_curr_robot_pos_pub = m_nh->advertise<geometry_msgs::PoseStamped>(
00389           m_curr_robot_pos_topic,
00390           m_queue_size);
00391   m_robot_trajectory_pub = m_nh->advertise<nav_msgs::Path>(
00392           m_robot_trajectory_topic,
00393           m_queue_size);
00394   m_robot_tr_poses_pub = m_nh->advertise<geometry_msgs::PoseArray>(
00395           m_robot_tr_poses_topic,
00396           m_queue_size);
00397 
00398   // odometry nav_msgs::Path
00399   m_odom_path.header.seq = 0;
00400   m_odom_path.header.stamp = ros::Time::now();
00401   m_odom_path.header.frame_id = m_anchor_frame_id;
00402 
00403   m_odom_trajectory_pub = m_nh->advertise<nav_msgs::Path>(
00404           m_odom_trajectory_topic,
00405           m_queue_size);
00406 
00407   // generated gridmap
00408   m_gridmap_pub = m_nh->advertise<nav_msgs::OccupancyGrid>(
00409           m_gridmap_topic,
00410           m_queue_size,
00411           /*latch=*/true);
00412 
00413   m_stats_pub = m_nh->advertise<mrpt_msgs::GraphSlamStats>(
00414           m_stats_topic,
00415           m_queue_size,
00416           /*latch=*/true);
00417 
00418 } // end of setupPubs
00419 
00420 template<class GRAPH_T>
00421 void CGraphSlamHandler_ROS<GRAPH_T>::setupSrvs() {
00422   this->m_logger->logFmt(LVL_INFO, "Setting up the services...");
00423 
00424   // TODO Error statistics
00425 
00426 } // end of setupSrvs
00427 
00428 template<class GRAPH_T>
00429 bool CGraphSlamHandler_ROS<GRAPH_T>::usePublishersBroadcasters() {
00430   using namespace mrpt::utils;
00431   using namespace std;
00432 
00433   MRPT_START;
00434   bool ret_val = true;
00435 
00436   ros::Time timestamp = ros::Time::now();
00437 
00438   // current MRPT robot pose
00439   pose_t mrpt_pose = this->m_engine->getCurrentRobotPosEstimation();
00440 
00441   //
00442   // convert pose_t to corresponding geometry_msg::TransformStamped
00443   // anchor frame <=> base_link
00444   //
00445         geometry_msgs::TransformStamped anchor_base_link_transform;
00446         anchor_base_link_transform.header.stamp = ros::Time::now();
00447         anchor_base_link_transform.header.frame_id = m_anchor_frame_id;
00448         anchor_base_link_transform.child_frame_id = m_base_link_frame_id;
00449 
00450         // translation
00451         anchor_base_link_transform.transform.translation.x = mrpt_pose.x();
00452         anchor_base_link_transform.transform.translation.y = mrpt_pose.y();
00453         anchor_base_link_transform.transform.translation.z = 0;
00454 
00455         // rotation
00456         anchor_base_link_transform.transform.rotation =
00457                 tf::createQuaternionMsgFromYaw(mrpt_pose.phi());
00458 
00459   // TODO - potential error in the rotation, investigate this
00460         m_broadcaster.sendTransform(anchor_base_link_transform);
00461 
00462   // anchor frame <=> odom frame
00463   //
00464   // make sure that we have received odometry information in the first
00465   // place...
00466   // the corresponding field would be initialized
00467   if (!m_anchor_odom_transform.child_frame_id.empty()) {
00468         m_broadcaster.sendTransform(m_anchor_odom_transform);
00469   }
00470 
00471   // set an arrow indicating the current orientation of the robot
00472   {
00473                 geometry_msgs::PoseStamped geom_pose;
00474                 geom_pose.header.stamp = timestamp;
00475                 geom_pose.header.seq = m_pub_seq;
00476                 geom_pose.header.frame_id = m_anchor_frame_id; // with regards to base_link...
00477 
00478                 // position
00479                 mrpt_bridge::convert(mrpt_pose, geom_pose.pose);
00480                 m_curr_robot_pos_pub.publish(geom_pose);
00481   }
00482 
00483   // robot trajectory
00484   // publish the trajectory of the robot
00485   {
00486                 this->m_logger->logFmt(LVL_DEBUG,
00487                                 "Publishing the current robot trajectory");
00488                 typename GRAPH_T::global_poses_t graph_poses;
00489                 this->m_engine->getRobotEstimatedTrajectory(&graph_poses);
00490 
00491                 nav_msgs::Path path;
00492 
00493                 // set the header
00494                 path.header.stamp = timestamp;
00495                 path.header.seq = m_pub_seq;
00496                 path.header.frame_id = m_anchor_frame_id;
00497 
00498                 //
00499                 // fill in the pose as well as the nav_msgs::Path at the same time.
00500                 //
00501 
00502                 geometry_msgs::PoseArray geom_poses;
00503                 geom_poses.header.stamp = timestamp;
00504                 geom_poses.header.frame_id = m_anchor_frame_id;
00505 
00506                 for (auto n_cit = graph_poses.begin();
00507                                 n_cit != graph_poses.end();
00508                                 ++n_cit) {
00509 
00510                 geometry_msgs::PoseStamped geom_pose_stamped;
00511                 geometry_msgs::Pose geom_pose;
00512 
00513                 // grab the pose - convert to geometry_msgs::Pose format
00514                 const pose_t& mrpt_pose = n_cit->second;
00515                 mrpt_bridge::convert(mrpt_pose, geom_pose);
00516                 geom_poses.poses.push_back(geom_pose);
00517                 geom_pose_stamped.pose = geom_pose;
00518 
00519                 // edit the header
00520                 geom_pose_stamped.header.stamp = timestamp;
00521                 geom_pose_stamped.header.seq = m_pub_seq;
00522                 geom_pose_stamped.header.frame_id = m_anchor_frame_id;
00523 
00524                 path.poses.push_back(geom_pose_stamped);
00525                 }
00526 
00527                 // publish only on new node addition
00528                 if (this->m_engine->getGraph().nodeCount() > m_graph_nodes_last_size) {
00529                 m_robot_tr_poses_pub.publish(geom_poses);
00530                 m_robot_trajectory_pub.publish(path);
00531                 }
00532   }
00533 
00534   // Odometry trajectory - nav_msgs::Path
00535   m_odom_trajectory_pub.publish(m_odom_path);
00536 
00537   // generated gridmap
00538   // publish only on new node addition
00539   if (this->m_engine->getGraph().nodeCount() > m_graph_nodes_last_size) {
00540                 std_msgs::Header h;
00541                 mrpt::system::TTimeStamp mrpt_time;
00542                 mrpt::maps::COccupancyGridMap2DPtr mrpt_gridmap =
00543                 mrpt::maps::COccupancyGridMap2D::Create();
00544                 this->m_engine->getMap(mrpt_gridmap, &mrpt_time);
00545 
00546                 // timestamp
00547                 mrpt_bridge::convert(mrpt_time, h.stamp);
00548                 h.seq = m_pub_seq;
00549                 h.frame_id = m_anchor_frame_id;
00550 
00551                 // nav gridmap
00552                 nav_msgs::OccupancyGrid nav_gridmap;
00553                 mrpt_bridge::convert(*mrpt_gridmap, nav_gridmap, h);
00554                 m_gridmap_pub.publish(nav_gridmap);
00555   }
00556 
00557   // GraphSlamStats publishing
00558   {
00559         mrpt_msgs::GraphSlamStats stats;
00560         stats.header.seq = m_stats_pub_seq++;
00561 
00562         map<string, int>  node_stats;
00563         map<string, int>  edge_stats;
00564         vector<double> def_energy_vec;
00565         mrpt::system::TTimeStamp mrpt_time;
00566 
00567         this->m_engine->getGraphSlamStats(&node_stats,
00568                         &edge_stats,
00569                         &mrpt_time);
00570 
00571         // node/edge count
00572         stats.nodes_total = node_stats["nodes_total"];
00573         stats.edges_total = edge_stats["edges_total"];
00574         if (edge_stats.find("ICP2D") != edge_stats.end()) {
00575           stats.edges_ICP2D = edge_stats["ICP2D"];
00576         }
00577         if (edge_stats.find("ICP3D") != edge_stats.end()) {
00578           stats.edges_ICP3D = edge_stats["ICP3D"];
00579         }
00580         if (edge_stats.find("Odometry") != edge_stats.end()) {
00581           stats.edges_odom = edge_stats["Odometry"];
00582         }
00583         stats.loop_closures = edge_stats["loop_closures"];
00584 
00585         // SLAM evaluation metric
00586         this->m_engine->getDeformationEnergyVector(
00587                         &stats.slam_evaluation_metric);
00588 
00589         mrpt_bridge::convert(mrpt_time, stats.header.stamp);
00590 
00591         m_stats_pub.publish(stats);
00592 
00593   }
00594 
00595 
00596 
00597   // update the last known size
00598   m_graph_nodes_last_size = this->m_engine->getGraph().nodeCount();
00599   m_pub_seq++;
00600 
00601   if (this->m_enable_visuals) {
00602                 ret_val = this->queryObserverForEvents();
00603   }
00604 
00605   return ret_val;
00606   MRPT_END;
00607 } // end of usePublishersBroadcasters
00608 
00609 template<class GRAPH_T>
00610 void CGraphSlamHandler_ROS<GRAPH_T>::sniffLaserScan(
00611                 const sensor_msgs::LaserScan::ConstPtr& ros_laser_scan) {
00612   using namespace std;
00613   using namespace mrpt::utils;
00614   using namespace mrpt::obs;
00615 
00616   this->m_logger->logFmt(
00617                 LVL_DEBUG,
00618                 "sniffLaserScan: Received a LaserScan msg. Converting it to MRPT format...");
00619 
00620   // build the CObservation2DRangeScan
00621   m_mrpt_laser_scan = CObservation2DRangeScan::Create();
00622   mrpt::poses::CPose3D rel_pose; // pose is 0.
00623   mrpt_bridge::convert(*ros_laser_scan, rel_pose, *m_mrpt_laser_scan);
00624 
00625   m_received_laser_scan = true;
00626   this->processObservation(m_mrpt_laser_scan);
00627 } // end of sniffLaserScan
00628 
00629 template<class GRAPH_T>
00630 void CGraphSlamHandler_ROS<GRAPH_T>::sniffOdom(
00631                 const nav_msgs::Odometry::ConstPtr& ros_odom) {
00632   using namespace std;
00633   using namespace mrpt::utils;
00634   using namespace mrpt::obs;
00635   using namespace mrpt::poses;
00636 
00637   this->m_logger->logFmt(
00638                 LVL_DEBUG,
00639                 "sniffOdom: Received an odometry msg. Converting it to MRPT format...");
00640 
00641   // update the odometry frame with regards to the anchor
00642   {
00643                 // header
00644                 m_anchor_odom_transform.header.frame_id = m_anchor_frame_id;
00645                 m_anchor_odom_transform.header.stamp = ros_odom->header.stamp;
00646                 m_anchor_odom_transform.header.seq = ros_odom->header.seq;
00647 
00648                 m_anchor_odom_transform.child_frame_id = m_odom_frame_id;
00649 
00650                 //
00651                 // copy ros_odom ==> m_anchor_odom
00652                 //
00653 
00654                 // translation
00655                 m_anchor_odom_transform.transform.translation.x =
00656                 ros_odom->pose.pose.position.x;
00657                 m_anchor_odom_transform.transform.translation.y =
00658                 ros_odom->pose.pose.position.y;
00659                 m_anchor_odom_transform.transform.translation.z =
00660                         ros_odom->pose.pose.position.z;
00661 
00662                 // quaternion
00663                 m_anchor_odom_transform.transform.rotation =
00664                         ros_odom->pose.pose.orientation;
00665   }
00666 
00667   // build and fill an MRPT CObservationOdometry instance for manipulation from
00668   // the main algorithm
00669   mrpt_bridge::convert(
00670                 /* src = */ ros_odom->header.stamp,
00671                 /* dst = */ m_mrpt_odom->timestamp);
00672   mrpt_bridge::convert(
00673                 /* src = */ ros_odom->pose.pose,
00674                 /* dst = */ m_mrpt_odom->odometry);
00675 
00676   // if this is the first call odometry should be 0. Decrement by the
00677   // corresponding offset
00678   if (m_first_time_in_sniff_odom) {
00679                 m_input_odometry_offset = m_mrpt_odom->odometry;
00680                 m_first_time_in_sniff_odom = false;
00681   }
00682   // decrement by the (constant) offset
00683   m_mrpt_odom->odometry =
00684                 m_mrpt_odom->odometry - m_input_odometry_offset;
00685 
00686   // add to the overall odometry path
00687   {
00688                 geometry_msgs::PoseStamped pose_stamped;
00689                 pose_stamped.header = ros_odom->header;
00690 
00691         // just for convenience - convert the MRPT pose back to PoseStamped
00692                 mrpt_bridge::convert(
00693                                 /* src = */ m_mrpt_odom->odometry,
00694                                 /* des = */ pose_stamped.pose);
00695         m_odom_path.poses.push_back(pose_stamped);
00696   }
00697 
00698   // print the odometry -  for debugging reasons...
00699   stringstream ss;
00700   ss << "Odometry - MRPT format:\t" << m_mrpt_odom->odometry << endl;
00701   this->m_logger->logFmt(LVL_DEBUG, "%s", ss.str().c_str());
00702 
00703   m_received_odom = true;
00704   this->processObservation(m_mrpt_odom);
00705 } // end of sniffOdom
00706 
00707 template<class GRAPH_T>
00708 void CGraphSlamHandler_ROS<GRAPH_T>::sniffCameraImage() {
00709   THROW_EXCEPTION("Method is not implemented yet.");
00710 
00711 }
00712 template<class GRAPH_T>
00713 void CGraphSlamHandler_ROS<GRAPH_T>::sniff3DPointCloud() {
00714   THROW_EXCEPTION("Method is not implemented yet.");
00715 
00716 }
00717 template<class GRAPH_T>
00718 void CGraphSlamHandler_ROS<GRAPH_T>::processObservation(
00719                 mrpt::obs::CObservationPtr& observ) {
00720   using namespace mrpt::utils;
00721   using namespace std;
00722 
00723   this->_process(observ);
00724   this->resetReceivedFlags();
00725 
00726 }
00727 
00728 template<class GRAPH_T>
00729 void CGraphSlamHandler_ROS<GRAPH_T>::_process(
00730                 mrpt::obs::CObservationPtr& observ) {
00731   using namespace mrpt::utils;
00732 
00733   //this->m_logger->logFmt(LVL_DEBUG, "Calling execGraphSlamStep...");
00734 
00735         // TODO - use the exit code of execGraphSlamStep to exit??
00736   if (!this->m_engine->isPaused()) {
00737                 this->m_engine->execGraphSlamStep(observ, m_measurement_cnt);
00738                 m_measurement_cnt++;
00739   }
00740 }
00741 
00742 template<class GRAPH_T>
00743 void CGraphSlamHandler_ROS<GRAPH_T>::resetReceivedFlags() {
00744   m_received_odom = false;
00745   m_received_laser_scan = false;
00746   m_received_camera = false;
00747   m_received_point_cloud = false;
00748 }
00749 
00750 } } } // end of namespaces
00751 
00752 #endif /* end of include guard: CGRAPHSLAMHANDLER_ROS_IMPL_H */


mrpt_graphslam_2d
Author(s): Nikos Koukis
autogenerated on Sun Sep 17 2017 03:02:04