00001 /* +---------------------------------------------------------------------------+ 00002 | Mobile Robot Programming Toolkit (MRPT) | 00003 | http://www.mrpt.org/ | 00004 | | 00005 | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file | 00006 | See: http://www.mrpt.org/Authors - All rights reserved. | 00007 | Released under BSD License. See details in http://www.mrpt.org/License | 00008 +---------------------------------------------------------------------------+ */ 00009 00010 #ifndef CMAPMERGER_H 00011 #define CMAPMERGER_H 00012 00013 #include "mrpt_graphslam_2d/CConnectionManager.h" 00014 #include "mrpt_graphslam_2d/TNeighborAgentMapProps.h" 00015 #include "mrpt_graphslam_2d/misc/common.h" 00016 00017 #include <ros/ros.h> 00018 #include <nav_msgs/OccupancyGrid.h> 00019 #include <nav_msgs/Path.h> 00020 #include <mrpt_msgs/GraphSlamAgents.h> 00021 #include <mrpt/utils/COutputLogger.h> 00022 #include <mrpt/maps/COccupancyGridMap2D.h> 00023 #include <mrpt/maps/CSimplePointsMap.h> 00024 #include <mrpt/slam/CGridMapAligner.h> 00025 #include <mrpt_bridge/map.h> 00026 #include <mrpt/system/filesystem.h> 00027 #include <mrpt/system/os.h> 00028 #include <mrpt/math/utils.h> 00029 #include <mrpt/graphslam/misc/CWindowManager.h> 00030 #include <mrpt/opengl/CAxis.h> 00031 #include <mrpt/opengl/CGridPlaneXY.h> 00032 #include <mrpt/utils/TColorManager.h> 00033 #include <mrpt/opengl/CSetOfLines.h> 00034 00035 #include <iterator> 00036 00037 const mrpt::poses::CPose3D EMPTY_POSE; 00038 00039 namespace mrpt { namespace graphslam { 00040 00047 class CMapMerger 00048 { 00049 public: 00050 typedef std::map<TNeighborAgentMapProps*, COccupancyGridMap2DPtr> maps_t; 00051 typedef std::map<TNeighborAgentMapProps*, bool> neighbor_to_is_used_t; 00052 typedef std::map<TNeighborAgentMapProps*, mrpt::poses::CPose2D> neighbor_to_rel_pose_t; 00053 00055 typedef std::map< 00056 TNeighborAgentMapProps*, 00057 mrpt::opengl::CSetOfLinesPtr> trajectories_t; 00058 typedef std::vector<TNeighborAgentMapProps*> neighbors_t; 00059 CMapMerger( 00060 mrpt::utils::COutputLogger* logger_in, 00061 ros::NodeHandle* nh_in); 00062 ~CMapMerger(); 00063 void mergeMaps(); 00068 bool updateState(); 00069 private: 00073 void monitorKeystrokes(mrpt::graphslam::CWindowObserver* win_observer); 00074 void initWindowVisuals(mrpt::graphslam::CWindowManager* win_manager); 00075 mrpt::graphslam::CWindowManager* initWindowVisuals(); 00076 00080 neighbors_t m_neighbors; 00081 std::map<TNeighborAgentMapProps*, CWindowManager*> m_neighbors_to_windows; 00082 mrpt::utils::COutputLogger* m_logger; 00083 ros::NodeHandle* m_nh; 00084 mrpt::graphslam::detail::CConnectionManager m_conn_manager; 00085 00088 std::string m_global_ns; 00092 std::string m_options_ns; 00093 std::string m_feedback_ns; 00094 size_t m_queue_size; 00095 mrpt::slam::CGridMapAligner::TConfigParams m_alignment_options; 00096 00097 std::string quit_keypress1; 00098 std::string quit_keypress2; 00099 std::string map_merge_keypress; 00100 00101 bool save_map_merging_results; 00102 00103 mrpt::graphslam::CWindowManager* m_fused_map_win_manager; 00104 00105 }; // end of CMapMerger 00106 00107 } } // end of namespaces 00108 00109 #endif /* end of include guard: CMAPMERGER_H */