24 #include <core/exception.h> 25 #include <navgraph/navgraph.h> 26 #include <navgraph/navgraph_node.h> 27 #include <navgraph/navgraph_edge.h> 28 #include <navgraph/yaml_navgraph.h> 30 #include <fawkes_msgs/NavGraph.h> 32 #include <boost/filesystem.hpp> 34 #define GET_CONFIG(privn, n, path, var) \ 35 if (! privn.getParam(path, var)) { \ 36 n.getParam(path, var); \ 49 throw std::runtime_error(
"No navgraph file given");
53 p = boost::filesystem::absolute(p);
55 ROS_INFO(
"Using navgraph path: %s", p.string().c_str());
56 if (boost::filesystem::exists(p)) {
57 boost::filesystem::remove(p);
59 boost::filesystem::create_directories(p.parent_path());
72 if (boost::filesystem::exists(p)) {
73 boost::filesystem::remove(p);
86 ROS_INFO(
"No navgraph received, yet. Re-subscribing to avoid stale latched publisher");
97 ROS_INFO(
"Initial navgraph received, disabling re-connect timer");
102 fawkes::NavGraph
n(
"received");
103 for (
size_t i = 0; i < msg->nodes.size(); ++i) {
104 const fawkes_msgs::NavGraphNode &msg_node = msg->nodes[i];
106 std::map<std::string, std::string> properties;
107 for (
size_t j = 0; j < msg_node.properties.size(); ++j) {
108 properties[msg_node.properties[j].key] = msg_node.properties[j].value;
110 fawkes::NavGraphNode node(msg_node.name, msg_node.pose.x, msg_node.pose.y, properties);
111 node.set_unconnected(msg_node.unconnected);
116 for (
size_t i = 0; i < msg->edges.size(); ++i) {
117 const fawkes_msgs::NavGraphEdge &msg_edge = msg->edges[i];
118 std::map<std::string, std::string> properties;
119 for (
size_t j = 0; j < msg_edge.properties.size(); ++j) {
120 properties[msg_edge.properties[j].key] = msg_edge.properties[j].value;
125 fawkes::NavGraphEdge edge(msg_edge.from_node, msg_edge.to_node,
126 properties, msg_edge.directed);
129 n.calc_reachability(
true);
147 ros::init(argc, argv,
"navgraph_retriever");
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
#define GET_CONFIG(privn, n, path, var)
ROSCPP_DECL void spin(Spinner &spinner)
void cb_sub_init_timer(const ros::WallTimerEvent &event)
WallTimer createWallTimer(WallDuration period, void(T::*callback)(const WallTimerEvent &), T *obj, bool oneshot=false, bool autostart=true) const
void cb_navgraph(const fawkes_msgs::NavGraph::ConstPtr &msg)
NavGraphRosRetriever(ros::NodeHandle &n)
virtual ~NavGraphRosRetriever()
ros::Subscriber sub_navgraph_
std::string cfg_navgraph_file_
int main(int argc, char **argv)
ros::WallTimer sub_init_timer_