fawkes_navgraph_retriever.cpp
Go to the documentation of this file.
00001 /***************************************************************************
00002  *  navgraph_retriever.cpp - retrieve navgraph and write to file
00003  *
00004  *  Created: Mon May 15 13:39:21 2017
00005  *  Copyright  2017  Tim Niemueller [www.niemueller.de]
00006  ****************************************************************************/
00007 
00008 /*  This program is free software; you can redistribute it and/or modify
00009  *  it under the terms of the GNU General Public License as published by
00010  *  the Free Software Foundation; either version 2 of the License, or
00011  *  (at your option) any later version.
00012  *
00013  *  This program is distributed in the hope that it will be useful,
00014  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00015  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00016  *  GNU Library General Public License for more details.
00017  *
00018  *  Read the full text in the LICENSE.GPL file in the doc directory.
00019  */
00020 
00021 #include <ros/ros.h>
00022 
00023 // from Fawkes
00024 #include <core/exception.h>
00025 #include <navgraph/navgraph.h>
00026 #include <navgraph/navgraph_node.h>
00027 #include <navgraph/navgraph_edge.h>
00028 #include <navgraph/yaml_navgraph.h>
00029 
00030 #include <fawkes_msgs/NavGraph.h>
00031 
00032 #include <boost/filesystem.hpp>
00033 
00034 #define GET_CONFIG(privn, n, path, var)   \
00035         if (! privn.getParam(path, var)) {      \
00036                 n.getParam(path, var);                \
00037         }
00038 
00039 class NavGraphRosRetriever
00040 {
00041  public:
00042         NavGraphRosRetriever(ros::NodeHandle &n)
00043                 : n(n)
00044         {
00045                 ros::NodeHandle privn("~");
00046                 GET_CONFIG(privn, n, "navgraph_file", cfg_navgraph_file_);
00047 
00048                 if (cfg_navgraph_file_.empty()) {
00049                         throw std::runtime_error("No navgraph file given");
00050                 }
00051 
00052                 boost::filesystem::path p(cfg_navgraph_file_);
00053                 p = boost::filesystem::absolute(p);
00054                 cfg_navgraph_file_ = p.string();
00055                 ROS_INFO("Using navgraph path: %s", p.string().c_str());
00056                 if (boost::filesystem::exists(p)) {
00057                         boost::filesystem::remove(p);
00058                 }
00059                 boost::filesystem::create_directories(p.parent_path());
00060 
00061                 sub_has_received_ = false;
00062                 sub_navgraph_ = n.subscribe<fawkes_msgs::NavGraph>("navgraph", 10, &NavGraphRosRetriever::cb_navgraph, this);
00063 
00064                 sub_init_timer_ =
00065                         n.createWallTimer(ros::WallDuration(5.0), &NavGraphRosRetriever::cb_sub_init_timer, this);
00066         }
00067 
00068         virtual ~NavGraphRosRetriever()
00069         {
00070                 sub_navgraph_.shutdown();
00071                 boost::filesystem::path p(cfg_navgraph_file_);
00072                 if (boost::filesystem::exists(p)) {
00073                         boost::filesystem::remove(p);
00074                 }
00075         }
00076 
00077         /* This timer is a workaround for some ROS issues that fails (not so
00078          * infrequently as one would hope) to retrieve the initial message
00079          * from a latched publisher.
00080          *
00081          */
00082         void
00083         cb_sub_init_timer(const ros::WallTimerEvent& event)
00084         {
00085                 if (! sub_has_received_) {
00086                         ROS_INFO("No navgraph received, yet. Re-subscribing to avoid stale latched publisher");
00087                         sub_navgraph_.shutdown();
00088                         sub_navgraph_ =
00089                                 n.subscribe<fawkes_msgs::NavGraph>("navgraph", 10, &NavGraphRosRetriever::cb_navgraph, this);
00090                 }
00091         }
00092         
00093         void
00094         cb_navgraph(const fawkes_msgs::NavGraph::ConstPtr& msg)
00095         {
00096                 if (! sub_has_received_) {
00097                         ROS_INFO("Initial navgraph received, disabling re-connect timer");
00098                         sub_has_received_ = true;
00099                         sub_init_timer_.stop();
00100                 }
00101 
00102                 fawkes::NavGraph n("received");
00103                 for (size_t i = 0; i < msg->nodes.size(); ++i) {
00104                         const fawkes_msgs::NavGraphNode &msg_node = msg->nodes[i];
00105                         //ROS_INFO("Adding Node %s", msg_node.name.c_str());
00106                         std::map<std::string, std::string> properties;
00107                         for (size_t j = 0; j < msg_node.properties.size(); ++j) {
00108                                 properties[msg_node.properties[j].key] = msg_node.properties[j].value;
00109                         }
00110                         fawkes::NavGraphNode node(msg_node.name, msg_node.pose.x, msg_node.pose.y, properties);
00111                         node.set_unconnected(msg_node.unconnected);
00112                         // has_orientation flag irrelevant, just means props have orientation field
00113                         n.add_node(node);
00114                 }
00115                 
00116                 for (size_t i = 0; i < msg->edges.size(); ++i) {
00117                         const fawkes_msgs::NavGraphEdge &msg_edge = msg->edges[i];
00118                         std::map<std::string, std::string> properties;
00119                         for (size_t j = 0; j < msg_edge.properties.size(); ++j) {
00120                                 properties[msg_edge.properties[j].key] = msg_edge.properties[j].value;
00121                         }
00122                         //ROS_INFO("Adding Edge %s %s %s", msg_edge.from_node.c_str(),
00123                         //         msg_edge.directed ? "->" : "--",
00124                         //         msg_edge.to_node.c_str());
00125                         fawkes::NavGraphEdge edge(msg_edge.from_node, msg_edge.to_node,
00126                                                   properties, msg_edge.directed);
00127                         n.add_edge(edge);
00128                 }
00129                 n.calc_reachability(/* allow multigraph */ true);
00130                 ROS_INFO("Writing YAML navgraph '%s'", cfg_navgraph_file_.c_str());
00131                 fawkes::save_yaml_navgraph(cfg_navgraph_file_, &n);
00132         }
00133 
00134  private:
00135         ros::NodeHandle    n;
00136 
00137         std::string cfg_navgraph_file_;
00138 
00139   ros::Subscriber sub_navgraph_;
00140         ros::WallTimer  sub_init_timer_;
00141         bool            sub_has_received_;
00142 };
00143 
00144 int
00145 main(int argc, char **argv)
00146 {
00147         ros::init(argc, argv, "navgraph_retriever");
00148 
00149         ros::NodeHandle n;
00150         NavGraphRosRetriever navgraph_retriever(n);
00151   ros::spin();
00152 
00153         return 0;
00154 }


rcll_fawkes_sim
Author(s): Tim Niemueller
autogenerated on Thu Jun 6 2019 17:48:58