Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021 #include <ros/ros.h>
00022
00023
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
00078
00079
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
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
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
00123
00124
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( 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 }