mrpt_graphslam_2d_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  std::string node_name = "mrpt_graphslam_2d";
41 
42  COutputLogger logger;
43  logger.setLoggerName(node_name);
44  logger.logFmt(LVL_WARN, "Initializing %s node...\n", node_name.c_str());
45 
46  ros::init(argc, argv, node_name);
47  ros::NodeHandle nh;
48 
49  ros::Rate loop_rate(10);
50 
51 
52  try {
53 
54  // Initialization
57  &logger, &options_checker, &nh);
58  graphslam_handler.readParams();
59  graphslam_handler.initEngine_ROS();
60  graphslam_handler.setupComm();
61 
62  // print the parameters just for verification
63  graphslam_handler.printParams();
64 
65  bool cont_exec = true;
66  while (ros::ok() && cont_exec) {
67  cont_exec = graphslam_handler.usePublishersBroadcasters();
68 
69  ros::spinOnce();
70  loop_rate.sleep();
71  }
72  }
73  catch (exception& e) {
75  "Finished with a (known) exception!"
76  << std::endl
77  << e.what()
78  << std::endl);
79  mrpt::system::pause();
80  return -1;
81  }
82  catch (...) {
84  "Finished with a (unknown) exception!"
85  << std::endl);
86  mrpt::system::pause();
87  return -1;
88  }
89 }
void readParams()
Read the problem configuration parameters.
bool usePublishersBroadcasters()
Provide feedback about the SLAM operation using ROS publilshers, update the registered frames using t...
void initEngine_ROS()
Initialize the CGraphslamEngine_* object.
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
int main(int argc, char **argv)
ROSCPP_DECL bool ok()
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...
#define ROS_ERROR_STREAM(args)
ROSCPP_DECL void spinOnce()


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