CGraphSlamEngine_MR.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 
12 #include <ros/callback_queue.h>
13 
19 
20 #include <mrpt_msgs/NodeIDWithLaserScan.h>
21 #include <mrpt_msgs/NodeIDWithPose_vec.h>
22 #include <mrpt_msgs/NetworkOfPoses.h>
23 #include <mrpt_msgs/GetCMGraph.h> // service
24 #include <mrpt_bridge/network_of_poses.h>
25 #include <mrpt_bridge/laser_scan.h>
26 #include <sensor_msgs/LaserScan.h>
27 #include <std_msgs/String.h>
28 #include <mrpt/poses/CPosePDFGaussian.h>
29 #include <mrpt/poses/CPosePDFSOG.h>
30 
31 #include <mrpt/math/utils.h>
32 #include <mrpt/system/os.h>
33 #include <mrpt/slam/CGridMapAligner.h>
34 #include <mrpt/graphs/TMRSlamNodeAnnotations.h>
35 #include <mrpt/graphslam/misc/TUncertaintyPath.h>
36 #include <mrpt/graphslam/misc/TNodeProps.h>
37 
38 #include <mrpt/version.h>
39 #if MRPT_VERSION>=0x199
40 #include <mrpt/img/TColorManager.h>
41 #include <mrpt/img/TColor.h>
42 #include <mrpt/graphs/TNodeID.h>
43 #include <mrpt/config/CConfigFileBase.h>
44 #include <mrpt/system/COutputLogger.h>
45 #include <mrpt/containers/stl_containers_utils.h>
46 using namespace mrpt::img;
47 using namespace mrpt::graphs;
48 using namespace mrpt::config;
49 using namespace mrpt::system;
50 using namespace mrpt::containers;
51 #else
52 #include <mrpt/utils/TColorManager.h>
53 using namespace mrpt::utils;
54 #endif
55 
56 
57 #include <set>
58 #include <iterator>
59 #include <algorithm>
60 
61 namespace mrpt { namespace graphslam {
62 
66 template<class GRAPH_T>
68 {
69 public:
72  typedef typename GRAPH_T::constraint_t constraint_t;
73  typedef typename constraint_t::type_value pose_t;
74  typedef std::pair<
75  TNodeID,
76  mrpt::obs::CObservation2DRangeScan::Ptr> MRPT_NodeIDWithLaserScan;
77  typedef std::map<
78  TNodeID,
79  mrpt::obs::CObservation2DRangeScan::Ptr> nodes_to_scans2D_t;
80  typedef std::vector<std::vector<TNodeID>> partitions_t;
81  typedef typename mrpt::graphs::detail::THypothesis<GRAPH_T> hypot_t;
82  typedef std::vector<hypot_t> hypots_t;
83  typedef std::vector<hypot_t*> hypotsp_t;
84  typedef typename GRAPH_T::global_pose_t global_pose_t;
85  typedef typename mrpt::graphslam::detail::TNodeProps<GRAPH_T> node_props_t;
86  typedef mrpt::graphslam::TUncertaintyPath<GRAPH_T> path_t;
87  typedef std::vector<path_t> paths_t;
89 
91  ros::NodeHandle* nh,
92  const std::string& config_file,
93  const std::string& rawlog_fname="",
94  const std::string& fname_GT="",
95  mrpt::graphslam::CWindowManager* win_manager=NULL,
96  mrpt::graphslam::deciders::CNodeRegistrationDecider<GRAPH_T>* node_reg=NULL,
97  mrpt::graphslam::deciders::CEdgeRegistrationDecider<GRAPH_T>* edge_reg=NULL,
98  mrpt::graphslam::optimizers::CGraphSlamOptimizer<GRAPH_T>* optimizer=NULL
99  );
100 
102 
103  bool _execGraphSlamStep(
104  mrpt::obs::CActionCollection::Ptr& action,
105  mrpt::obs::CSensoryFrame::Ptr& observations,
106  mrpt::obs::CObservation::Ptr& observation,
107  size_t& rawlog_entry);
108 
109  void initClass();
110 
118  CGraphSlamEngine_MR<GRAPH_T>& engine_in,
119  const mrpt_msgs::GraphSlamAgent& agent_in);
122 
125  void setupComm();
129  void setupSubs();
132  void setupSrvs();
138  void fetchUpdatedNodesList(
139  const mrpt_msgs::NodeIDWithPose_vec::ConstPtr& nodes);
141  void fetchLastRegdIDScan(
142  const mrpt_msgs::NodeIDWithLaserScan::ConstPtr& last_regd_id_scan);
159  void getCachedNodes(
160  std::vector<TNodeID>* nodeIDs=NULL,
161  std::map<
162  TNodeID,
163  node_props_t>* nodes_params=NULL,
164  bool only_unused=true) const;
167  void fillOptPaths(
168  const std::set<TNodeID>& nodeIDs,
169  paths_t* opt_paths) const;
173  void computeGridMap() const;
174  const mrpt::maps::COccupancyGridMap2D::Ptr& getGridMap() const;
177  void getGridMap(mrpt::maps::COccupancyGridMap2D::Ptr& map) const;
185  bool hasNewData() const;
186  std::string getAgentNs() const { return this->agent.topic_namespace.data; }
187  void resetFlags() const;
189  const TNeighborAgentProps& other) const {
190  return (this->agent == other.agent);
191  }
192  bool operator<(
193  const TNeighborAgentProps& other) const {
194  return (this->agent < other.agent);
195  }
199  const sensor_msgs::LaserScan* getLaserScanByNodeID(
200  const TNodeID nodeID) const;
201 
204 
208  const mrpt_msgs::GraphSlamAgent agent;
217  bool hasNewNodesBatch(int new_batch_size);
218 
223  void setTColor(const TColor& color_in) { color = color_in; }
226  TColor color;
227 
231  std::set<TNodeID> nodeIDs_set;
233  typename GRAPH_T::global_poses_t poses;
235  std::vector<mrpt_msgs::NodeIDWithLaserScan> ros_scans;
240  std::map<TNodeID, bool> nodeID_to_is_integrated;
247 
258 
259  std::string cm_graph_service;
264  mutable mrpt::maps::COccupancyGridMap2D::Ptr gridmap_cached;
265 
266  mutable bool has_new_nodes;
267  mutable bool has_new_scans;
268 
273 
282  std::pair<
283  TNodeID,
284  mrpt::poses::CPose2D> last_integrated_pair_neighbor_frame;
285 
286  };
287  typedef std::vector<TNeighborAgentProps*> neighbors_t;
288 
289  const neighbors_t& getVecOfNeighborAgentProps() const {
290  return m_neighbors;
291  }
298  bool isOwnNodeID(
299  const TNodeID nodeID,
300  const global_pose_t* pose_out=NULL) const;
301 
302 
303 private:
307  bool addNodeBatchesFromAllNeighbors();
318  bool addNodeBatchFromNeighbor(TNeighborAgentProps* neighbor);
328  bool findTFsWithAllNeighbors();
337  bool findTFWithNeighbor(TNeighborAgentProps* neighbor);
344  bool getNeighborByAgentID(
345  const std::string& agent_ID_str,
346  TNeighborAgentProps*& neighbor) const;
358  bool pubUpdatedNodesList();
368  bool pubLastRegdIDScan();
369 
370  void usePublishersBroadcasters();
371 
372  void setupSubs();
373  void setupPubs();
374  void setupSrvs();
375 
378  bool getCMGraph(
379  mrpt_msgs::GetCMGraph::Request& req,
380  mrpt_msgs::GetCMGraph::Response& res);
381 
382  void readParams();
383  void readROSParameters();
384  void printParams() const;
385  mrpt::poses::CPose3D getLSPoseForGridMapVisualization(
386  const TNodeID nodeID) const;
387  void setObjectPropsFromNodeID(
388  const TNodeID nodeID,
389  mrpt::opengl::CSetOfObjects::Ptr& viz_object);
394  void monitorNodeRegistration(
395  bool registered=false,
396  std::string class_name="Class");
400  void getAllOwnNodes(
401  std::set<TNodeID>* nodes_set) const;
402  void getNodeIDsOfEstimatedTrajectory(
403  std::set<TNodeID>* nodes_set) const;
404  void getRobotEstimatedTrajectory(
405  typename GRAPH_T::global_poses_t* graph_poses) const;
406 
407 
413  neighbors_t m_neighbors;
420  std::map<TNeighborAgentProps*, bool> m_neighbor_to_found_initial_tf;
421 
438 
449  std::string m_mr_ns;
462 
466  std::string m_cm_graph_service;
467 
472 
475 
476 
483 
492 
504 
509 
512  mrpt::slam::CGridMapAligner::TConfigParams m_alignment_options;
513 
515 
517  std::string m_sec_mr_slam_params;
518 
519  struct TOptions : CLoadableOptions {
520 
521  typedef self_t engine_mr_t;
522 
523  TOptions(const engine_mr_t& engine_in);
524  ~TOptions();
525  void loadFromConfigFile(
526  const CConfigFileBase& source,
527  const std::string& section);
528 #if MRPT_VERSION>=0x199
529  void dumpToTextStream(std::ostream& out) const;
530 #else
531  void dumpToTextStream(mrpt::utils::CStream& out) const;
532 #endif
533 
534 
565 
567  const engine_mr_t& engine;
568 
569  } m_opts;
570 
571 };
572 
573 
574 } } // end of namespaces
575 
576 // pseudo-split decleration from implementation
578 
CGraphSlamEngine_MR< GRAPH_T > self_t
std::string m_list_neighbors_topic
Name of topic at which we publish information about the agents that we can currently communicate with...
ros::AsyncSpinner cm_graph_async_spinner
AsyncSpinner that is used to query the CM-Graph service in case a new request arrives.
std::vector< TNeighborAgentProps * > neighbors_t
ros::Publisher m_last_regd_id_scan_pub
Publisher of the laserScan + the corresponding (last) registered node.
bool m_registered_multiple_nodes
Indicates whether multiple nodes were just registered. Used for checking correct node registration in...
std::set< TNodeID > nodeIDs_set
NodeIDs that I have received from this graphSLAM agent.
ros::NodeHandle * nh
NodeHandle passed by the calling CGraphSlamEngine_MR class.
std::vector< mrpt_msgs::NodeIDWithLaserScan > ros_scans
ROS LaserScans that I have received from this graphSLAM agent.
std::pair< TNodeID, mrpt::obs::CObservation2DRangeScan::Ptr > MRPT_NodeIDWithLaserScan
TColor color
Unique color of current TNeighborAgentProps instance.
mrpt::graphslam::TUncertaintyPath< GRAPH_T > path_t
std::map< TNeighborAgentProps *, bool > m_neighbor_to_found_initial_tf
Map from neighbor instance to transformation from own to neighbor&#39;s graph.
mrpt::graphs::detail::THypothesis< GRAPH_T > hypot_t
size_t m_graph_nodes_last_size
Last known size of the m_nodes map.
mrpt::poses::CPose2D tf_self_to_neighbor_first_integrated_pose
CPose2D connecting own graph origin to neighbor first received and integrated node. If this pose is 0 then it is not yet computed.
bool operator==(const TNeighborAgentProps &other) const
std::map< TNodeID, bool > nodeID_to_is_integrated
Have I already integrated this node in my graph?
const neighbors_t & getVecOfNeighborAgentProps() const
CGraphSlamEngine_ROS< GRAPH_T > parent_t
mrpt::slam::CGridMapAligner::TConfigParams m_alignment_options
Parameters used during the alignment operation.
size_t inter_group_node_count_thresh
Lowest number of nodes that should exist in a group of nodes before evaluating it. These nodes are fetched by the other running graphSLAM agents.
mrpt::graphslam::detail::CConnectionManager m_conn_manager
CConnectionManager instance.
void setTColor(const TColor &color_in)
Set the color related to the current neighbor agent.
ros::NodeHandle * m_nh
NodeHandle pointer - inherited by the parent. Redefined here for convenience.
mrpt::graphslam::deciders::CEdgeRegistrationDecider_MR< GRAPH_T > edge_reg_mr_t
int num_last_regd_nodes
Max number of last registered NodeIDs + corresponding positions to publish.
mrpt::graphslam::detail::TNodeProps< GRAPH_T > node_props_t
bool operator<(const TNeighborAgentProps &other) const
std::map< TNodeID, mrpt::obs::CObservation2DRangeScan::Ptr > nodes_to_scans2D_t
bool conservative_find_initial_tfs_to_neighbors
Be conservative when it comes to deciding the initial transformation of own graph with regards to gra...
std::string m_mr_ns
Condensed Measurements topic namespace.
int nodes_integration_batch_size
After an inter-graph transformation is found between own graph and a neighbor&#39;s map, newly fetched neighbor&#39;s nodes are added in batches.
mrpt::maps::COccupancyGridMap2D::Ptr gridmap_cached
Pointer to the gridmap object of the neighbor.
std::vector< std::vector< TNodeID > > partitions_t
ros::Publisher m_last_regd_nodes_pub
Publisher of the last registered nodeIDs and positions.
neighbors_t m_neighbors
GraphSlamAgent instance pointer to TNeighborAgentProps.
size_t inter_group_node_count_thresh_minadv
Minimum advised limit of inter_group_node_count_thresh.
double m_offset_y_nrd
Display the Deciders/Optimizers with which we are running as well as the namespace of the current age...
GRAPH_T::global_poses_t poses
Poses that I have received from this graphSLAM agent.
std::string m_cm_graph_service
Name of the server which is to be called when other agent wants to have a subgraph of certain nodes r...
std::pair< TNodeID, mrpt::poses::CPose2D > last_integrated_pair_neighbor_frame
Last nodeID/CPose2D of neighbor that was integrated - Pose is taken with regards to neighbor&#39;s frame ...
mrpt::graphslam::CGraphSlamEngine derived class for executing multi-robot graphSLAM ...
std::string m_last_regd_id_scan_topic
Name of the topic that the last registered laser scan (+ corresponding nodeID) is published at...
size_t m_nodes_to_laser_scans2D_last_size
Last known size of the m_nodes_to_laser_scans2D map.
Struct responsible for holding properties (nodeIDs, node positions, LaserScans) that have been regist...
std::string m_last_regd_nodes_topic
Name of the topic that the last X registered nodes + positions are going to be published at...
Class template that provides a wrapper around the MRPT CGraphSlamEngine class template and implements...
Class responsible of handling the network communication between SLAM agents in the Multi-Robot graphS...


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