mrpt_graphslam_2d_node.cpp
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 
00010 // MRPT headers
00011 #include <mrpt/utils/COutputLogger.h>
00012 #include <mrpt/graphslam/CGraphSlamEngine.h>
00013 #include <mrpt/system/string_utils.h>
00014 
00015 #include <cstdlib>
00016 #include <cstring>
00017 
00018 // ROS headers
00019 #include "mrpt_graphslam_2d/CGraphSlamHandler_ROS.h"
00020 
00021 using namespace mrpt;
00022 using namespace mrpt::utils;
00023 using namespace mrpt::poses;
00024 using namespace mrpt::obs;
00025 using namespace mrpt::system;
00026 using namespace mrpt::graphs;
00027 using namespace mrpt::math;
00028 using namespace mrpt::opengl;
00029 using namespace mrpt::utils;
00030 using namespace mrpt::graphslam;
00031 using namespace mrpt::graphslam::deciders;
00032 using namespace mrpt::graphslam::optimizers;
00033 using namespace mrpt::graphslam::apps;
00034 
00035 using namespace std;
00036 
00038 int main(int argc, char **argv)
00039 {
00040         std::string node_name = "mrpt_graphslam_2d";
00041 
00042         COutputLogger logger;
00043         logger.setLoggerName(node_name);
00044         logger.logFmt(LVL_WARN, "Initializing %s node...\n", node_name.c_str());
00045 
00046   ros::init(argc, argv, node_name);
00047         ros::NodeHandle nh;
00048 
00049         ros::Rate loop_rate(10);
00050 
00051 
00052         try {
00053 
00054                 // Initialization
00055                 TUserOptionsChecker_ROS<CNetworkOfPoses2DInf> options_checker;
00056                 CGraphSlamHandler_ROS<CNetworkOfPoses2DInf> graphslam_handler(
00057                                 &logger, &options_checker, &nh);
00058                 graphslam_handler.readParams();
00059                 graphslam_handler.initEngine_ROS();
00060                 graphslam_handler.setupComm();
00061 
00062                 // print the parameters just for verification
00063                 graphslam_handler.printParams();
00064 
00065                 bool cont_exec = true;
00066                 while (ros::ok() && cont_exec) {
00067                         cont_exec = graphslam_handler.usePublishersBroadcasters();
00068 
00069                         ros::spinOnce();
00070                         loop_rate.sleep();
00071                 }
00072         }
00073         catch (exception& e) {
00074                 ROS_ERROR_STREAM(
00075                                 "Finished with a (known) exception!"
00076                                 << std::endl
00077                                 << e.what()
00078                                 << std::endl);
00079                 mrpt::system::pause();
00080                 return -1;
00081         }
00082         catch (...) {
00083                 ROS_ERROR_STREAM(
00084                                 "Finished with a (unknown) exception!"
00085                                 << std::endl);
00086                 mrpt::system::pause();
00087                 return -1;
00088         }
00089 }


mrpt_graphslam_2d
Author(s): Nikos Koukis
autogenerated on Thu Jun 6 2019 21:40:26