CConnectionManager.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 <ros/ros.h>
#include <multimaster_msgs_fkie/DiscoverMasters.h>
#include <mrpt_msgs/GraphSlamAgent.h>
#include <mrpt_msgs/GraphSlamAgents.h>
#include <mrpt_bridge/mrpt_bridge.h>

#include <mrpt/utils/COutputLogger.h>
#include <mrpt/system/datetime.h>
#include <mrpt/system/os.h>
#include <mrpt/system/string_utils.h>
#include <mrpt/math/utils.h>

#include <algorithm>
#include <iterator>
#include <iostream>
#include <string>
#include <vector>

#include <cstdlib>

namespace mrpt { namespace graphslam { namespace detail {

class CConnectionManager
{
public:
        //typedef std::vector<mrpt_msgs::GraphSlamAgent>::iterator agents_it;
        //typedef std::vector<mrpt_msgs::GraphSlamAgent>::const_iterator agents_cit;
        typedef mrpt_msgs::GraphSlamAgents::_list_type::iterator agents_it;
        typedef mrpt_msgs::GraphSlamAgents::_list_type::const_iterator agents_cit;

        CConnectionManager(
                        mrpt::utils::COutputLogger* logger,
                        ros::NodeHandle* nh_in);
        ~CConnectionManager();
        void getNearbySlamAgents(mrpt_msgs::GraphSlamAgents* agents_vec,
                        bool ignore_self=true);
        const mrpt_msgs::GraphSlamAgents&  getNearbySlamAgents();
        const mrpt_msgs::GraphSlamAgents&  getNearbySlamAgentsCached() const;

        void setupComm();
        const std::string& getTrimmedNs() const;

private:
        std::string own_ns;
        void updateNearbySlamAgents();
        void setupSubs();
        void setupPubs();
        void setupSrvs();
        static bool convert(
                        const multimaster_msgs_fkie::ROSMaster& ros_master,
                        mrpt_msgs::GraphSlamAgent* slam_agent);
        static void convert(
                        const mrpt_msgs::GraphSlamAgent& slam_agent,
                        multimaster_msgs_fkie::ROSMaster* ros_master);
        static std::string extractHostnameOrIP(const std::string& str,
                        unsigned short* agent_port=NULL);

        mrpt::utils::COutputLogger* m_logger;
        ros::NodeHandle* m_nh;

        ros::ServiceClient m_DiscoverMasters_client;
        mrpt_msgs::GraphSlamAgents m_nearby_slam_agents;

        bool has_setup_comm;

};

} } } // end of namespaces

bool operator==(
                const multimaster_msgs_fkie::ROSMaster& master1,
                const multimaster_msgs_fkie::ROSMaster& master2);
bool operator!=(
                const multimaster_msgs_fkie::ROSMaster& master1,
                const multimaster_msgs_fkie::ROSMaster& master2);
bool operator==(
                const mrpt_msgs::GraphSlamAgent& agent1,
                const mrpt_msgs::GraphSlamAgent& agent2);
bool operator!=(
                const mrpt_msgs::GraphSlamAgent& agent1,
                const mrpt_msgs::GraphSlamAgent& agent2);
bool operator<(
                const mrpt_msgs::GraphSlamAgent& agent1,
                const mrpt_msgs::GraphSlamAgent& agent2);
bool operator==(
                const multimaster_msgs_fkie::ROSMaster& master,
                const mrpt_msgs::GraphSlamAgent& agent);
bool operator==(
                const mrpt_msgs::GraphSlamAgent& agent,
                const multimaster_msgs_fkie::ROSMaster& master);
bool operator!=(
                const multimaster_msgs_fkie::ROSMaster& master,
                const mrpt_msgs::GraphSlamAgent& agent);
bool operator!=(
                const mrpt_msgs::GraphSlamAgent& agent,
                const multimaster_msgs_fkie::ROSMaster& master);