mrpt_graphslam_2d_mr_node.cpp
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 
10 // MRPT headers
11 #include <mrpt/utils/COutputLogger.h>
12 #include <mrpt/graphslam/CGraphSlamEngine.h>
13 #include <mrpt/system/string_utils.h>
14 
15 #include <cstdlib>
16 #include <cstring>
17 
18 // ROS headers
20 
21 using namespace mrpt;
22 using namespace mrpt::utils;
23 using namespace mrpt::poses;
24 using namespace mrpt::obs;
25 using namespace mrpt::system;
26 using namespace mrpt::graphs;
27 using namespace mrpt::math;
28 using namespace mrpt::opengl;
29 using namespace mrpt::utils;
30 using namespace mrpt::graphslam;
31 using namespace mrpt::graphslam::deciders;
32 using namespace mrpt::graphslam::optimizers;
33 using namespace mrpt::graphslam::apps;
34 
35 using namespace std;
36 
38 int main(int argc, char **argv)
39 {
40  COutputLogger logger;
41 
42  try {
43  std::string node_name = "mrpt_graphslam_2d_mr";
44 
45  ros::init(argc, argv, node_name);
46  ros::NodeHandle nh;
47 
48  node_name = node_name + nh.getNamespace();
49  logger.setLoggerName(node_name);
50  logger.logFmt(LVL_WARN, "Initialized %s node...\n", node_name.c_str());
51 
52  ros::Rate loop_rate(10);
53 
54  // Initialization
57  &logger, &options_checker, &nh);
58  graphslam_handler.readParams();
59  graphslam_handler.initEngine_MR();
60  graphslam_handler.setupComm();
61 
62  std::string ns = nh.getNamespace();
63  // overwite default results directory due to the multi-robot nature
64  graphslam_handler.setResultsDirName(std::string(ns.begin()+2, ns.end()));
65 
66  // print the parameters just for verification
67  graphslam_handler.printParams();
68 
69  bool cont_exec = true;
70  while (ros::ok() && cont_exec) {
71  cont_exec = graphslam_handler.usePublishersBroadcasters();
72 
73  ros::spinOnce();
74  loop_rate.sleep();
75 
76  }
77  }
78  catch (exception& e) {
79  cout << "Known error!" << endl;
80  logger.logFmt(LVL_ERROR, "Caught exception: %s", e.what());
81  mrpt::system::pause();
82  return -1;
83  }
84  catch (...) {
85  cout << "Unknown error!" << endl;
86  logger.logFmt(LVL_ERROR, "Finished with unknown exception. Exiting\n.");
87  mrpt::system::pause();
88  return -1;
89  }
90 }
void readParams()
Read the problem configuration parameters.
bool usePublishersBroadcasters()
Provide feedback about the SLAM operation using ROS publilshers, update the registered frames using t...
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ROSCPP_DECL bool ok()
const std::string & getNamespace() const
int main(int argc, char **argv)
void setupComm()
Wrapper method around the protected setup* class methods.
bool sleep()
void printParams()
Print in a compact manner the overall problem configuration parameters.
Manage variables, ROS parameters and everything else related to the graphslam-engine ROS wrapper...
ROSCPP_DECL void spinOnce()


mrpt_graphslam_2d
Author(s): Nikos Koukis
autogenerated on Thu Jun 6 2019 19:37:48