CConnectionManager.cpp
Go to the documentation of this file.
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 #include "mrpt_graphslam_2d/CConnectionManager.h"
00011 
00012 using namespace mrpt::graphslam::detail;
00013 using namespace std;
00014 using namespace mrpt::utils;
00015 using namespace mrpt::system;
00016 using namespace mrpt::math;
00017 using namespace multimaster_msgs_fkie;
00018 
00019 bool operator==(
00020                 const multimaster_msgs_fkie::ROSMaster& master1,
00021                 const multimaster_msgs_fkie::ROSMaster& master2) {
00022         return master1.uri == master2.uri;
00023 }
00024 bool operator!=(
00025                 const multimaster_msgs_fkie::ROSMaster& master1,
00026                 const multimaster_msgs_fkie::ROSMaster& master2) {
00027         return !(master1 == master2);
00028 }
00029 
00030 bool operator==(
00031                 const mrpt_msgs::GraphSlamAgent& agent1,
00032                 const mrpt_msgs::GraphSlamAgent& agent2) {
00033         return (
00034                         agent1.agent_ID == agent2.agent_ID &&
00035                         agent1.topic_namespace.data == agent2.topic_namespace.data);
00036 }
00038 
00039 bool operator!=(
00040                 const mrpt_msgs::GraphSlamAgent& agent1,
00041                 const mrpt_msgs::GraphSlamAgent& agent2) {
00042         return !(agent1 == agent2);
00043 }
00044 
00045 bool operator<(
00046                 const mrpt_msgs::GraphSlamAgent& agent1,
00047                 const mrpt_msgs::GraphSlamAgent& agent2) {
00048         return agent1.agent_ID < agent2.agent_ID;
00049 }
00051 
00052 bool operator==(
00053                 const multimaster_msgs_fkie::ROSMaster& master,
00054                 const mrpt_msgs::GraphSlamAgent& agent) {
00055         return (master.name == agent.name.data);
00056 }
00057 
00058 bool operator!=(
00059                 const multimaster_msgs_fkie::ROSMaster& master,
00060                 const mrpt_msgs::GraphSlamAgent& agent) {
00061         return !(master == agent);
00062 }
00063 
00064 bool operator==(
00065                 const mrpt_msgs::GraphSlamAgent& agent,
00066                 const multimaster_msgs_fkie::ROSMaster& master) {
00067         return (master == agent);
00068 }
00069 bool operator!=(
00070                 const mrpt_msgs::GraphSlamAgent& agent,
00071                 const multimaster_msgs_fkie::ROSMaster& master) {
00072         return (master != agent);
00073 }
00074 
00076 
00077 CConnectionManager::CConnectionManager(
00078                 mrpt::utils::COutputLogger* logger,
00079                 ros::NodeHandle* nh_in):
00080         m_logger(logger),
00081         m_nh(nh_in),
00082         has_setup_comm(false)
00083 {
00084 
00085         ASSERT_(m_logger);
00086         ASSERT_(m_nh);
00087 
00088         {
00089                 std::string own_ns_tmp = m_nh->getNamespace();
00090                 // ignore starting "/" characters
00091                 own_ns = std::string(
00092                                 own_ns_tmp.begin() + own_ns_tmp.find_first_not_of(" /"),
00093                                 own_ns_tmp.end());
00094         }
00095 
00096         // keep this call below the topic names initializations
00097         this->setupComm();
00098 
00099 }
00100 
00101 CConnectionManager::~CConnectionManager() { }
00102 
00103 const std::string& CConnectionManager::getTrimmedNs() const {
00104         return own_ns;
00105 }
00106 
00107 void CConnectionManager::getNearbySlamAgents(
00108                 mrpt_msgs::GraphSlamAgents* agents_vec,
00109                 bool ignore_self/*= true */) {
00110 
00111 
00112         ASSERTMSG_(agents_vec, "Invalid pointer to vector of GraphSlam Agents.");
00113         this->updateNearbySlamAgents();
00114         *agents_vec = m_nearby_slam_agents;
00115 
00116         if (ignore_self) {
00117                 // remove the GraphSlamAgent instance whose topic namespace coincedes with
00118                 // the namespace that the CConnectionManager instance is running under.
00119                 auto search = [this](const mrpt_msgs::GraphSlamAgent& agent) {
00120                         return (agent.topic_namespace.data == this->own_ns);
00121                 };
00122                 agents_it it = find_if(
00123                                 agents_vec->list.begin(),
00124                                 agents_vec->list.end(), search);
00125 
00126                 // TODO - fix the following
00127                 // this agent should always exist
00128                 // TODO - well, sometimes it doesn't, investigate this
00129                 // UPDATE: Even when /master_discovery node is up the agents vector might
00130                 // be empty.
00131                 //ASSERT_(it != agents_vec->list.end());
00132                 if (it != agents_vec->list.end()) {
00133                         agents_vec->list.erase(it);
00134                 }
00135                 else {
00136                 }
00137         }
00138 }
00139 
00140 const mrpt_msgs::GraphSlamAgents&
00141 CConnectionManager::getNearbySlamAgentsCached() const {
00142         return m_nearby_slam_agents;
00143 }
00144 
00145 const mrpt_msgs::GraphSlamAgents&
00146 CConnectionManager::getNearbySlamAgents() {
00147         this->updateNearbySlamAgents();
00148         return this->getNearbySlamAgentsCached();
00149 } // end of getNearbySlamAgents
00150 
00151 void CConnectionManager::updateNearbySlamAgents() {
00152         using ::operator==;
00153         using namespace mrpt::utils;
00154         using namespace mrpt::math;
00155         ASSERT_(has_setup_comm);
00156 
00157         DiscoverMasters srv;
00158 
00159         // ask for the agents in the neighborhood
00160         m_DiscoverMasters_client.call(srv);
00161         std::vector<ROSMaster>* masters = &(srv.response.masters);
00162 
00163         // convert RosMaster(s) to mrpt_msgs::GraphSlamAgent(s)
00164         for (std::vector<ROSMaster>::const_iterator
00165                         masters_it = masters->begin();
00166                         masters_it != masters->end();
00167                         ++masters_it) {
00168 
00169                 // 3 cases:
00170                 // In RosMasters     AND     In mrpt_msgs::GraphSlamAgents => update relevant fields
00171                 // In RosMasters     AND NOT In mrpt_msgs::GraphSlamAgents => add it to mrpt_msgs::GraphSlamAgents
00172                 // NOT In RosMasters AND     In mrpt_msgs::GraphSlamAgents => Do nothing.
00173 
00174                 // have I already registered the current agent?
00175                 auto search = [masters_it](const mrpt_msgs::GraphSlamAgent& agent) {
00176                         return agent == *masters_it;
00177                 };
00178                 agents_it it = find_if(
00179                                 m_nearby_slam_agents.list.begin(),
00180                                 m_nearby_slam_agents.list.end(), search);
00181 
00182                 if (it != m_nearby_slam_agents.list.end()) { // found, update relevant fields
00183                         // update timestamp
00184                         it->last_seen_time.data = ros::Time(masters_it->timestamp);
00185                 }
00186                 else { // not found, try to insert it.
00187                         mrpt_msgs::GraphSlamAgent new_agent;
00188                         bool is_agent = this->convert(*masters_it, &new_agent);
00189                         if (is_agent) {
00190                                 m_nearby_slam_agents.list.push_back(new_agent);
00191                         };
00192                 }
00193 
00194         } // for all ROSMaster(s)
00195 
00196 } // end of updateNearbySlamAgents
00197 
00198 
00199 void CConnectionManager::setupComm() { 
00200 
00201         this->setupSubs();
00202         this->setupPubs();
00203         this->setupSrvs();
00204 
00205         has_setup_comm = true;
00206 } // end of setupComm
00207 
00208 void CConnectionManager::setupSubs() { }
00209 void CConnectionManager::setupPubs() { }
00210 void CConnectionManager::setupSrvs() {
00211         // call to the querier should be made after the
00212         // multimaster_msgs_fkie::DiscoverMaster service is up and running
00213         m_DiscoverMasters_client =
00214                 m_nh->serviceClient<multimaster_msgs_fkie::DiscoverMasters>(
00215                                 "/master_discovery/list_masters");
00216 
00217         //ASSERT_(m_DiscoverMasters_client.isValid());
00218 }
00219 
00220 bool CConnectionManager::convert(
00221                 const multimaster_msgs_fkie::ROSMaster& ros_master,
00222                 mrpt_msgs::GraphSlamAgent* slam_agent) {
00223         ASSERT_(slam_agent);
00224         bool agent_namespace_found = false;
00225 
00226         slam_agent->name.data = ros_master.name;
00227         slam_agent->is_online.data = static_cast<bool>(ros_master.online);
00228 
00229         // ip_address, hostname, port
00230         std::string ip_addr = CConnectionManager::extractHostnameOrIP(ros_master.monitoruri);
00231         slam_agent->ip_addr.data = ip_addr;
00232         std::string hostname = CConnectionManager::extractHostnameOrIP(
00233                         ros_master.uri, &slam_agent->port);
00234         slam_agent->hostname.data = hostname;
00235 
00236         // agent_ID - last field of the IP address
00237         vector<string> tokens;
00238         mrpt::system::tokenize(ip_addr, ".", tokens);
00239         slam_agent->agent_ID = atoi(tokens.rbegin()->c_str());
00240 
00241         // robot topic namespace
00242         {
00243                 //stringstream ss("");
00244                 //ss << slam_agent->name.data  << "_" << slam_agent->agent_ID;
00245                 //slam_agent->topic_namespace.data = ss.str().c_str();
00246                 slam_agent->topic_namespace.data = slam_agent->name.data;
00247 
00248                 // assert that there exists a subtopic namespace named feedback under this.
00249                 ros::master::V_TopicInfo topics;
00250                 bool got_topics = ros::master::getTopics(topics);
00251                 ASSERTMSG_(got_topics, "Unable to fetch topics. Exiting.");
00252 
00253                 // get the namespaces under the current topic_namespace
00254                 const std::string& topic_ns = "/" + slam_agent->topic_namespace.data;
00255                 // TODO - What if this topic changes? from the configuration file
00256                 const std::string& feedback_ns =
00257                         "/" + slam_agent->topic_namespace.data + "/" + "feedback";
00258 
00259                 auto search = [&feedback_ns](
00260                                 const ros::master::TopicInfo& topic) {
00261                         return (strStarts(topic.name, feedback_ns));
00262                 };
00263                 ros::master::V_TopicInfo::const_iterator cit = find_if(
00264                                 topics.begin(), topics.end(), search);
00265                 if (cit != topics.end()) {
00266                         agent_namespace_found = true;
00267                 }
00268         }
00269 
00270         // timestamp
00271         slam_agent->last_seen_time.data = ros::Time(ros_master.timestamp);
00272         return agent_namespace_found;
00273 
00274 } // end of convert
00275 
00276 void CConnectionManager::convert(
00277                 const mrpt_msgs::GraphSlamAgent& slam_agent,
00278                 multimaster_msgs_fkie::ROSMaster* ros_master) {
00279         ASSERT_(ros_master);
00280 
00281         ros_master->name = slam_agent.name.data;
00282         {
00283                 stringstream ss("");
00284                 ss << "http://" << slam_agent.ip_addr <<  ":" << slam_agent.port;
00285                 ros_master->uri = ss.str();
00286         }
00287         ros_master->online = slam_agent.is_online.data;
00288         ros_master->discoverer_name = "/master_discovery";
00289 
00290         // TODO - timestamp
00291 
00292 }
00293 
00294 std::string CConnectionManager::extractHostnameOrIP(const std::string& str,
00295                 unsigned short* agent_port/*=NULL*/) {
00296         // example for monitoruri: http://nickkouk-ubuntu:11311/
00297         std::string s = std::string(str.begin()+7, str.end());
00298 
00299         vector<string> tokens;
00300         mrpt::system::tokenize(s, ":", tokens);
00301 
00302         if (agent_port) {
00303                 *agent_port = static_cast<unsigned short>(atoi(tokens[1].c_str()));
00304         }
00305 
00306         return tokens[0];
00307 }
00308 


mrpt_graphslam_2d
Author(s): Nikos Koukis
autogenerated on Thu Jun 6 2019 21:40:26