CMapMerger.h

Go to the documentation of this file.

/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file        |
   | See: http://www.mrpt.org/Authors - All rights reserved.                   |
   | Released under BSD License. See details in http://www.mrpt.org/License    |
   +---------------------------------------------------------------------------+ */

#pragma once

#include "mrpt_graphslam_2d/CConnectionManager.h"
#include "mrpt_graphslam_2d/TNeighborAgentMapProps.h"
#include "mrpt_graphslam_2d/misc/common.h"

#include <ros/ros.h>
#include <nav_msgs/OccupancyGrid.h>
#include <nav_msgs/Path.h>
#include <mrpt_msgs/GraphSlamAgents.h>
#include <mrpt/maps/COccupancyGridMap2D.h>
#include <mrpt/maps/CSimplePointsMap.h>
#include <mrpt/slam/CGridMapAligner.h>
#include <mrpt_bridge/map.h>
#include <mrpt/system/filesystem.h>
#include <mrpt/system/os.h>
#include <mrpt/math/utils.h>
#include <mrpt/graphslam/misc/CWindowManager.h>
#include <mrpt/opengl/CAxis.h>
#include <mrpt/opengl/CGridPlaneXY.h>
#include <mrpt/opengl/CSetOfLines.h>

#include <mrpt/version.h>
#if MRPT_VERSION>=0x199
#include <mrpt/system/COutputLogger.h>
#include <mrpt/img/TColorManager.h>
using namespace mrpt::system;
using namespace mrpt::img;
#else
#include <mrpt/utils/COutputLogger.h>
#include <mrpt/utils/TColorManager.h>
using namespace mrpt::utils;
#endif



#include <iterator>

const mrpt::poses::CPose3D EMPTY_POSE;

namespace mrpt { namespace graphslam {

class CMapMerger
{
public:
        typedef std::map<TNeighborAgentMapProps*, COccupancyGridMap2D::Ptr> maps_t;
        typedef std::map<TNeighborAgentMapProps*, bool> neighbor_to_is_used_t;
        typedef std::map<TNeighborAgentMapProps*, mrpt::poses::CPose2D> neighbor_to_rel_pose_t;

        typedef std::map<
                TNeighborAgentMapProps*,
                mrpt::opengl::CSetOfLines::Ptr> trajectories_t;
        typedef std::vector<TNeighborAgentMapProps*> neighbors_t;
        CMapMerger(
                        mrpt::utils::COutputLogger* logger_in,
                        ros::NodeHandle* nh_in);
        ~CMapMerger();
        void mergeMaps();
        bool updateState();
private:
        void monitorKeystrokes(mrpt::graphslam::CWindowObserver* win_observer);
        void initWindowVisuals(mrpt::graphslam::CWindowManager* win_manager);
        mrpt::graphslam::CWindowManager* initWindowVisuals();

        neighbors_t m_neighbors;
        std::map<TNeighborAgentMapProps*, CWindowManager*> m_neighbors_to_windows;
        mrpt::utils::COutputLogger* m_logger;
        ros::NodeHandle* m_nh;
        mrpt::graphslam::detail::CConnectionManager m_conn_manager;

        std::string m_global_ns;
        std::string m_options_ns;
        std::string m_feedback_ns;
        size_t m_queue_size;
        mrpt::slam::CGridMapAligner::TConfigParams m_alignment_options;

        std::string quit_keypress1;
        std::string quit_keypress2;
        std::string map_merge_keypress;

        bool save_map_merging_results;

        mrpt::graphslam::CWindowManager* m_fused_map_win_manager;

}; // end of CMapMerger

} } // end of namespaces