CGraphSlamEngine_ROS_impl.h
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 #pragma once
11 namespace mrpt { namespace graphslam {
12 
13 template<class GRAPH_t>
15  ros::NodeHandle* nh,
16  const std::string& config_file,
17  const std::string& rawlog_fname/* ="" */,
18  const std::string& fname_GT /* ="" */,
19  mrpt::graphslam::CWindowManager* win_manager /* = NULL */,
20  mrpt::graphslam::deciders::CNodeRegistrationDecider<GRAPH_t>* node_reg /* = NULL */,
21  mrpt::graphslam::deciders::CEdgeRegistrationDecider<GRAPH_t>* edge_reg /* = NULL */,
22  mrpt::graphslam::optimizers::CGraphSlamOptimizer<GRAPH_t>* optimizer /* = NULL */):
23  parent::CGraphSlamEngine(
24  config_file,
25  rawlog_fname,
26  fname_GT,
27  win_manager,
28  node_reg,
29  edge_reg,
30  optimizer),
31  m_nh(nh)
32 {
33  this->initClass();
34 }
35 
36 template<class GRAPH_t>
38  MRPT_LOG_DEBUG_STREAM("In Destructor: Deleting CGraphSlamEngine_ROS instance...");
39  ros::shutdown();
40 }
41 
42 template<class GRAPH_t>
44  using namespace mrpt::graphslam;
45  this->m_class_name = "CGraphSlamEngine_ROS";
46  this->setLoggerName(this->m_class_name);
47 
48  // http://wiki.ros.org/rospy/Overview/Publishers%20and%20Subscribers#queue_size:_publish.28.29_behavior_and_queuing
49  m_queue_size = 10;
50 
51  this->setupComm();
52 
53  // in case of ROS specific deciders/optimizers (they inherit from the CDeciderOrOptimizer_ROS interface)
54  // set the NodeHandle
55  {
57  dynamic_cast<CRegistrationDeciderOrOptimizer_ROS<GRAPH_t>*>(this->m_node_reg);
58 
59  if (dec_opt_ros) {
60  dec_opt_ros->setNodeHandle(m_nh);
61  }
62  }
63  {
65  dynamic_cast<CRegistrationDeciderOrOptimizer_ROS<GRAPH_t>*>(this->m_edge_reg);
66  if (dec_opt_ros) {
67  dec_opt_ros->setNodeHandle(m_nh);
68  }
69  }
70  {
72  dynamic_cast<CRegistrationDeciderOrOptimizer_ROS<GRAPH_t>*>(this->m_optimizer);
73  if (dec_opt_ros) {
74  dec_opt_ros->setNodeHandle(m_nh);
75  }
76  }
77 
78  this->readParams();
79 }
80 
81 template<class GRAPH_t>
83 
84  this->readROSParameters();
85 }
86 
87 template<class GRAPH_t>
89 
90 template<class GRAPH_t>
92  mrpt::obs::CActionCollection::Ptr& action,
93  mrpt::obs::CSensoryFrame::Ptr& observations,
94  mrpt::obs::CObservation::Ptr& observation,
95  size_t& rawlog_entry) {
96 
97  bool continue_exec = parent::_execGraphSlamStep(
98  action, observations, observation, rawlog_entry);
100 
101  return continue_exec;
102 }
103 
104 template<class GRAPH_t>
106 
107 template<class GRAPH_t>
109  MRPT_LOG_INFO_STREAM(
110  "Setting up subscribers, publishers, services...");
111 
112  // setup subscribers, publishers, services...
113  this->setupSubs();
114  this->setupPubs();
115  this->setupSrvs();
116 
117 }
118 
119 template<class GRAPH_t>
121  MRPT_LOG_DEBUG_STREAM("Setting up subscribers...");
122 
123 }
124 
125 template<class GRAPH_t>
127  MRPT_LOG_DEBUG_STREAM("Setting up publishers...");
128 
129 }
130 
131 template<class GRAPH_t>
133  MRPT_LOG_DEBUG_STREAM("Setting up services...");
134 
135 }
136 
137 
138 } } // end of namespaces
139 
CGraphSlamEngine_ROS(ros::NodeHandle *nh, const std::string &config_file, const std::string &rawlog_fname="", const std::string &fname_GT="", mrpt::graphslam::CWindowManager *win_manager=NULL, mrpt::graphslam::deciders::CNodeRegistrationDecider< GRAPH_t > *node_reg=NULL, mrpt::graphslam::deciders::CEdgeRegistrationDecider< GRAPH_t > *edge_reg=NULL, mrpt::graphslam::optimizers::CGraphSlamOptimizer< GRAPH_t > *optimizer=NULL)
void readParams()
Read the problem configuration parameters.
virtual bool _execGraphSlamStep(mrpt::obs::CActionCollection::Ptr &action, mrpt::obs::CSensoryFrame::Ptr &observations, mrpt::obs::CObservation::Ptr &observation, size_t &rawlog_entry)
virtual void usePublishersBroadcasters()
Provide feedback about the SLAM operation.
ROSCPP_DECL void shutdown()
Interface class that all ROS-specific deciders/optimizers can inherit from.
void initClass()
Initialize object instance.
void readROSParameters()
Read configuration parameters from the ROS parameter server.
void setupComm()
Wrapper method around the protected setup* class methods.


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