mrpt_graphslam_2d_mr_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         COutputLogger logger;
00041 
00042         try {
00043                 std::string node_name = "mrpt_graphslam_2d_mr";
00044 
00045         ros::init(argc, argv, node_name);
00046                 ros::NodeHandle nh;
00047 
00048                 node_name = node_name + nh.getNamespace();
00049                 logger.setLoggerName(node_name);
00050                 logger.logFmt(LVL_WARN, "Initialized %s node...\n", node_name.c_str());
00051 
00052                 ros::Rate loop_rate(10);
00053 
00054                 // Initialization
00055                 TUserOptionsChecker_ROS<CNetworkOfPoses2DInf_NA> options_checker;
00056                 CGraphSlamHandler_ROS<CNetworkOfPoses2DInf_NA> graphslam_handler(
00057                                 &logger, &options_checker, &nh);
00058                 graphslam_handler.readParams();
00059                 graphslam_handler.initEngine_MR();
00060                 graphslam_handler.setupComm();
00061 
00062                 std::string ns = nh.getNamespace();
00063                 // overwite default results directory due to the multi-robot nature
00064                 graphslam_handler.setResultsDirName(std::string(ns.begin()+2, ns.end()));
00065 
00066                 // print the parameters just for verification
00067                 graphslam_handler.printParams();
00068 
00069                 bool cont_exec = true;
00070                 while (ros::ok() && cont_exec) {
00071                         cont_exec = graphslam_handler.usePublishersBroadcasters();
00072 
00073                         ros::spinOnce();
00074                         loop_rate.sleep();
00075 
00076                 }
00077         }
00078         catch (exception& e) {
00079                 cout << "Known error!" << endl;
00080                 logger.logFmt(LVL_ERROR, "Caught exception: %s", e.what());
00081                 mrpt::system::pause();
00082                 return -1;
00083         }
00084         catch (...) {
00085                 cout << "Unknown error!" << endl;
00086                 logger.logFmt(LVL_ERROR, "Finished with unknown exception. Exiting\n.");
00087                 mrpt::system::pause();
00088                 return -1;
00089         }
00090 }


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