CMapMerger.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 
11 #pragma once
12 
16 
17 #include <ros/ros.h>
18 #include <nav_msgs/OccupancyGrid.h>
19 #include <nav_msgs/Path.h>
20 #include <mrpt_msgs/GraphSlamAgents.h>
21 #include <mrpt/maps/COccupancyGridMap2D.h>
22 #include <mrpt/maps/CSimplePointsMap.h>
23 #include <mrpt/slam/CGridMapAligner.h>
24 #include <mrpt/ros1bridge/map.h>
25 #include <mrpt/system/filesystem.h>
26 #include <mrpt/system/os.h>
27 #include <mrpt/math/utils.h>
28 #include <mrpt/graphslam/misc/CWindowManager.h>
29 #include <mrpt/opengl/CAxis.h>
30 #include <mrpt/opengl/CGridPlaneXY.h>
31 #include <mrpt/opengl/CSetOfLines.h>
32 #include <mrpt/system/COutputLogger.h>
33 #include <mrpt/img/TColorManager.h>
34 
35 #include <iterator>
36 
37 const mrpt::poses::CPose3D EMPTY_POSE;
38 
39 using namespace mrpt::system;
40 using namespace mrpt::img;
41 using namespace mrpt::maps;
42 using namespace mrpt::obs;
43 
44 namespace mrpt
45 {
46 namespace graphslam
47 {
55 {
56  public:
57  typedef std::map<TNeighborAgentMapProps*, COccupancyGridMap2D::Ptr> maps_t;
58  typedef std::map<TNeighborAgentMapProps*, bool> neighbor_to_is_used_t;
59  typedef std::map<TNeighborAgentMapProps*, mrpt::poses::CPose2D>
61 
63  typedef std::map<TNeighborAgentMapProps*, mrpt::opengl::CSetOfLines::Ptr>
65  typedef std::vector<TNeighborAgentMapProps*> neighbors_t;
66  CMapMerger(mrpt::system::COutputLogger* logger_in, ros::NodeHandle* nh_in);
67  ~CMapMerger();
68  void mergeMaps();
73  bool updateState();
74 
75  private:
79  void monitorKeystrokes(mrpt::graphslam::CWindowObserver* win_observer);
80  void initWindowVisuals(mrpt::graphslam::CWindowManager* win_manager);
81  mrpt::graphslam::CWindowManager* initWindowVisuals();
82 
86  neighbors_t m_neighbors;
87  std::map<TNeighborAgentMapProps*, CWindowManager*> m_neighbors_to_windows;
88  mrpt::system::COutputLogger* m_logger;
91 
95  std::string m_global_ns;
99  std::string m_options_ns;
100  std::string m_feedback_ns;
101  size_t m_queue_size;
102  mrpt::slam::CGridMapAligner::TConfigParams m_alignment_options;
103 
104  std::string quit_keypress1;
105  std::string quit_keypress2;
106  std::string map_merge_keypress;
107 
109 
110  mrpt::graphslam::CWindowManager* m_fused_map_win_manager;
111 
112 }; // end of CMapMerger
113 
114 } // namespace graphslam
115 } // namespace mrpt
std::vector< TNeighborAgentMapProps * > neighbors_t
Definition: CMapMerger.h:65
Class responsible of the execution of the map_merger_node.
Definition: CMapMerger.h:54
std::map< TNeighborAgentMapProps *, CWindowManager * > m_neighbors_to_windows
Definition: CMapMerger.h:87
const mrpt::poses::CPose3D EMPTY_POSE
Definition: CMapMerger.h:37
std::string map_merge_keypress
Definition: CMapMerger.h:106
mrpt::slam::CGridMapAligner::TConfigParams m_alignment_options
Definition: CMapMerger.h:102
mrpt::graphslam::detail::CConnectionManager m_conn_manager
Definition: CMapMerger.h:90
std::map< TNeighborAgentMapProps *, COccupancyGridMap2D::Ptr > maps_t
Definition: CMapMerger.h:57
std::string m_global_ns
Topic namespace under which current node is going to be publishing.
Definition: CMapMerger.h:95
mrpt::graphslam::CWindowManager * m_fused_map_win_manager
Definition: CMapMerger.h:110
mrpt::system::COutputLogger * m_logger
Definition: CMapMerger.h:88
neighbors_t m_neighbors
CConnectionManager instance for fetching the running graphSLAM agents.
Definition: CMapMerger.h:86
std::map< TNeighborAgentMapProps *, mrpt::opengl::CSetOfLines::Ptr > trajectories_t
Robot trajectory visual object type.
Definition: CMapMerger.h:64
ros::NodeHandle * m_nh
Definition: CMapMerger.h:89
std::map< TNeighborAgentMapProps *, mrpt::poses::CPose2D > neighbor_to_rel_pose_t
Definition: CMapMerger.h:60
std::map< TNeighborAgentMapProps *, bool > neighbor_to_is_used_t
Definition: CMapMerger.h:58
std::string m_options_ns
Topic namespace under which, options that are used during the map alignment procedure are fetched fro...
Definition: CMapMerger.h:99
Class responsible of handling the network communication between SLAM agents in the Multi-Robot graphS...


mrpt_graphslam_2d
Author(s): Nikos Koukis
autogenerated on Sun Jun 26 2022 02:12:26