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 <fkie_multimaster_msgs/DiscoverMasters.h>
#include <mrpt_msgs/GraphSlamAgent.h>
#include <mrpt_msgs/GraphSlamAgents.h>

#include <mrpt/system/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::system::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 fkie_multimaster_msgs::ROSMaster& ros_master,
                mrpt_msgs::GraphSlamAgent* slam_agent);
        static void convert(
                const mrpt_msgs::GraphSlamAgent& slam_agent,
                fkie_multimaster_msgs::ROSMaster* ros_master);
        static std::string extractHostnameOrIP(
                const std::string& str, unsigned short* agent_port = NULL);

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

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

        bool has_setup_comm;
};

}  // namespace detail
}  // namespace graphslam
}  // namespace mrpt

bool operator==(
        const fkie_multimaster_msgs::ROSMaster& master1,
        const fkie_multimaster_msgs::ROSMaster& master2);
bool operator!=(
        const fkie_multimaster_msgs::ROSMaster& master1,
        const fkie_multimaster_msgs::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 fkie_multimaster_msgs::ROSMaster& master,
        const mrpt_msgs::GraphSlamAgent& agent);
bool operator==(
        const mrpt_msgs::GraphSlamAgent& agent,
        const fkie_multimaster_msgs::ROSMaster& master);
bool operator!=(
        const fkie_multimaster_msgs::ROSMaster& master,
        const mrpt_msgs::GraphSlamAgent& agent);
bool operator!=(
        const mrpt_msgs::GraphSlamAgent& agent,
        const fkie_multimaster_msgs::ROSMaster& master);