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 <core/utils/lockptr.h>
00026 #include <core/threading/mutex_locker.h>
00027 #include <utils/system/fam.h>
00028 #include <navgraph/navgraph.h>
00029 #include <navgraph/yaml_navgraph.h>
00030
00031 #include <fawkes_msgs/NavGraph.h>
00032 #include <fawkes_msgs/NavGraphGetPairwiseCosts.h>
00033 #include <fawkes_msgs/NavGraphSearchPath.h>
00034
00035 #include <boost/filesystem.hpp>
00036
00037 #include <tf/transform_listener.h>
00038
00039 #define GET_CONFIG(privn, n, path, var) \
00040 if (! privn.getParam(path, var)) { \
00041 n.getParam(path, var); \
00042 }
00043
00044
00045 class TransformListenerExtended : public tf::TransformListener
00046 {
00047 public:
00048 TransformListenerExtended() : TransformListener() {}
00049
00050 bool transform_origin(const std::string& source_frame,
00051 const std::string& target_frame,
00052 tf::Stamped<tf::Pose>& stamped_out)
00053 {
00054 tf::Stamped<tf::Pose> ident(tf::Transform(tf::Quaternion(0, 0, 0, 1),
00055 tf::Vector3(0, 0, 0)),
00056 ros::Time(0,0), source_frame);
00057
00058 transformPose(target_frame, ident, stamped_out);
00059 return true;
00060 }
00061 };
00062
00063 class NavGraphRosNode : public fawkes::FamListener
00064 {
00065 public:
00066 NavGraphRosNode(ros::NodeHandle &n)
00067 : n(n)
00068 {
00069 ros::NodeHandle privn("~");
00070 GET_CONFIG(privn, n, "navgraph_file", cfg_navgraph_file_);
00071 GET_CONFIG(privn, n, "base_frame_id", cfg_base_frame_);
00072 GET_CONFIG(privn, n, "global_frame_id", cfg_global_frame_);
00073
00074 boost::filesystem::path p(cfg_navgraph_file_);
00075 p = boost::filesystem::absolute(p);
00076 cfg_navgraph_file_ = p.string();
00077 if (boost::filesystem::exists(p)) {
00078 navgraph = fawkes::LockPtr<fawkes::NavGraph>(fawkes::load_yaml_navgraph(cfg_navgraph_file_),
00079 true);
00080 ROS_INFO("[F-NavGraph] Loaded navgraph from '%s': %zu nodes, %zu edges",
00081 cfg_navgraph_file_.c_str(), navgraph->nodes().size(), navgraph->edges().size());
00082 } else {
00083 navgraph = fawkes::LockPtr<fawkes::NavGraph>(new fawkes::NavGraph("empty"), true);
00084 }
00085
00086 boost::filesystem::create_directories(p.parent_path());
00087 fam_ = new fawkes::FileAlterationMonitor();
00088 fam_->add_filter((std::string("^") + p.filename().string() + "$").c_str());
00089 printf("Watching dir %s\n", p.parent_path().string().c_str());
00090 fam_->watch_dir(p.parent_path().string().c_str());
00091
00092
00093 fam_->add_listener(this);
00094
00095 tf_listener = new TransformListenerExtended();
00096
00097 pub_navgraph_ = n.advertise<fawkes_msgs::NavGraph>("navgraph", 10, true);
00098 svs_search_path_ = n.advertiseService("navgraph/search_path",
00099 &NavGraphRosNode::svs_search_path_cb, this);
00100 svs_get_pwcosts_ = n.advertiseService("navgraph/get_pairwise_costs",
00101 &NavGraphRosNode::svs_get_pwcosts_cb, this);
00102
00103 fam_timer_ =
00104 n.createWallTimer(ros::WallDuration(1.0), &NavGraphRosNode::cb_fam_timer, this);
00105
00106 publish_graph();
00107 }
00108
00109 virtual ~NavGraphRosNode()
00110 {
00111 delete fam_;
00112 delete tf_listener;
00113 }
00114
00115 virtual void
00116 fam_event(const char *filename, unsigned int mask)
00117 {
00118
00119
00120
00121
00122
00123
00124 if (mask & FAM_DELETE) {
00125 ROS_INFO("[F-NavGraph] Navgraph file deleted, clearing");
00126 navgraph->clear();
00127 publish_graph();
00128 return;
00129 }
00130
00131 if (mask & (FAM_MODIFY | FAM_IGNORED)) {
00132 ROS_INFO("[F-NavGraph] NavGraph changed on disk, reloading");
00133
00134 try {
00135 fawkes::LockPtr<fawkes::NavGraph> new_graph =
00136 fawkes::LockPtr<fawkes::NavGraph>(fawkes::load_yaml_navgraph(cfg_navgraph_file_),
00137 true);
00138 ROS_INFO("[F-NavGraph] Re-loaded navgraph from '%s': %zu nodes, %zu edges",
00139 cfg_navgraph_file_.c_str(), new_graph->nodes().size(), new_graph->edges().size());
00140 **navgraph = **new_graph;
00141 publish_graph();
00142 } catch (fawkes::Exception &e) {
00143 ROS_WARN("[F-NavGraph] Loading new graph failed: %s", e.what());
00144 return;
00145 } catch (std::runtime_error &e) {
00146 ROS_WARN("[F-NavGraph] Loading new graph failed: %s", e.what());
00147 return;
00148 }
00149 }
00150 }
00151
00152 void
00153 cb_fam_timer(const ros::WallTimerEvent& event)
00154 {
00155 fam_->process_events();
00156 }
00157
00158 void
00159 convert_nodes(const std::vector<fawkes::NavGraphNode> &nodes,
00160 std::vector<fawkes_msgs::NavGraphNode> &out)
00161 {
00162 for (const fawkes::NavGraphNode &node : nodes) {
00163 fawkes_msgs::NavGraphNode ngn;
00164 ngn.name = node.name();
00165 ngn.has_orientation = node.has_property(fawkes::navgraph::PROP_ORIENTATION);
00166 ngn.pose.x = node.x();
00167 ngn.pose.y = node.y();
00168 if (ngn.has_orientation) {
00169 ngn.pose.theta = node.property_as_float(fawkes::navgraph::PROP_ORIENTATION);
00170 }
00171 ngn.unconnected = node.unconnected();
00172 const std::map<std::string, std::string> &props = node.properties();
00173 for (const auto p : props) {
00174 fawkes_msgs::NavGraphProperty ngp;
00175 ngp.key = p.first;
00176 ngp.value = p.second;
00177 ngn.properties.push_back(ngp);
00178 }
00179 out.push_back(ngn);
00180 }
00181 }
00182
00183 void
00184 publish_graph()
00185 {
00186 fawkes::MutexLocker lock(navgraph.objmutex_ptr());
00187
00188 fawkes_msgs::NavGraph ngm;
00189
00190 const std::vector<fawkes::NavGraphNode> &nodes = navgraph->nodes();
00191 convert_nodes(nodes, ngm.nodes);
00192
00193 const std::vector<fawkes::NavGraphEdge> &edges = navgraph->edges();
00194 for (const fawkes::NavGraphEdge &edge : edges) {
00195 fawkes_msgs::NavGraphEdge nge;
00196 nge.from_node = edge.from();
00197 nge.to_node = edge.to();
00198 nge.directed = edge.is_directed();
00199 const std::map<std::string, std::string> &props = edge.properties();
00200 for (const auto p : props) {
00201 fawkes_msgs::NavGraphProperty ngp;
00202 ngp.key = p.first;
00203 ngp.value = p.second;
00204 nge.properties.push_back(ngp);
00205 }
00206 ngm.edges.push_back(nge);
00207 }
00208
00209 pub_navgraph_.publish(ngm);
00210 }
00211
00212
00213 bool
00214 svs_search_path_cb(fawkes_msgs::NavGraphSearchPath::Request &req,
00215 fawkes_msgs::NavGraphSearchPath::Response &res)
00216 {
00217 fawkes::NavGraphNode from, to;
00218
00219
00220 if (req.from_node.empty()) {
00221 tf::Stamped<tf::Pose> pose;
00222 if (! tf_listener->transform_origin(cfg_base_frame_, cfg_global_frame_, pose)) {
00223 ROS_WARN("[F-NavGraph] Failed to compute pose, cannot generate plan");
00224
00225 res.ok = false;
00226 res.errmsg = "Failed to compute pose, cannot generate plan";
00227 return true;
00228 }
00229
00230 from =
00231 navgraph->closest_node(pose.getOrigin().x(), pose.getOrigin().y());
00232 if (! from.is_valid()) {
00233 res.ok = false;
00234 res.errmsg = "Failed to get closest node to pose";
00235 return true;
00236 }
00237
00238 fawkes_msgs::NavGraphNode free_start;
00239 free_start.name = "free-start";
00240 free_start.pose.x = pose.getOrigin().x();
00241 free_start.pose.y = pose.getOrigin().y();
00242 free_start.has_orientation = true;
00243 free_start.pose.theta = tf::getYaw(pose.getRotation());
00244 res.path.push_back(free_start);
00245 } else {
00246 from = navgraph->node(req.from_node);
00247 if (! from.is_valid()) {
00248 res.ok = false;
00249 res.errmsg = "Failed to find start node " + req.from_node;
00250 return true;
00251 }
00252 }
00253
00254 fawkes::NavGraphPath path;
00255
00256 if (! req.to_node.empty()) {
00257 path = navgraph->search_path(from.name(), req.to_node);
00258 } else {
00259 fawkes::NavGraphNode close_to_goal = navgraph->closest_node(req.to_pose.x, req.to_pose.y);
00260 path = navgraph->search_path(from, close_to_goal);
00261 if (! path.empty()) {
00262 fawkes::NavGraphNode free_target("free-target", req.to_pose.x, req.to_pose.y);
00263 if (std::isfinite(req.to_pose.theta)) {
00264 free_target.set_property("orientation", (float)req.to_pose.theta);
00265 }
00266 path.add_node(free_target, navgraph->cost(path.nodes().back(), free_target));
00267 }
00268 }
00269
00270
00271 convert_nodes(path.nodes(), res.path);
00272 res.cost = path.cost();
00273
00274 res.ok = true;
00275 return true;
00276 }
00277
00278 bool
00279 svs_get_pwcosts_cb(fawkes_msgs::NavGraphGetPairwiseCosts::Request &req,
00280 fawkes_msgs::NavGraphGetPairwiseCosts::Response &res)
00281 {
00282 for (unsigned int i = 0; i < req.nodes.size(); ++i) {
00283 for (unsigned int j = 0; j < req.nodes.size(); ++j) {
00284 if (i == j) continue;
00285
00286 fawkes::NavGraphNode from_node, to_node;
00287 try {
00288 from_node = navgraph->node(req.nodes[i]);
00289 to_node = navgraph->node(req.nodes[j]);
00290 } catch (fawkes::Exception &e) {
00291 res.ok = false;
00292 res.errmsg = "Failed to get path from '" + req.nodes[i] + "' to '" +
00293 req.nodes[j] + "': " + e.what_no_backtrace();
00294 res.path_costs.clear();
00295 return true;
00296 }
00297
00298 fawkes::NavGraphNode start_node, goal_node;
00299
00300 if (from_node.unconnected()) {
00301 start_node = navgraph->closest_node_to(from_node.name());
00302
00303
00304 } else {
00305 start_node = from_node;
00306 }
00307 if (to_node.unconnected()) {
00308 goal_node = navgraph->closest_node_to(to_node.name());
00309
00310
00311 } else {
00312 goal_node = to_node;
00313 }
00314 fawkes::NavGraphPath p = navgraph->search_path(start_node, goal_node);
00315 if (p.empty()) {
00316 res.ok = false;
00317 res.errmsg = "Failed to get path from '" + start_node.name() + "' to '" + goal_node.name() + "'";
00318 res.path_costs.clear();
00319 return true;
00320 }
00321 fawkes_msgs::NavGraphPathCost pc;
00322 pc.from_node = req.nodes[i];
00323 pc.to_node = req.nodes[j];
00324 pc.cost = p.cost();
00325 if (from_node.unconnected()) {
00326 pc.cost += navgraph->cost(from_node, start_node);
00327 }
00328 if (to_node.unconnected()) {
00329 pc.cost += navgraph->cost(goal_node, to_node);
00330 }
00331 res.path_costs.push_back(pc);
00332 }
00333 }
00334
00335 res.ok = true;
00336 return true;
00337 }
00338
00339
00340 private:
00341 ros::NodeHandle n;
00342
00343 std::string cfg_navgraph_file_;
00344 std::string cfg_base_frame_;
00345 std::string cfg_global_frame_;
00346
00347 fawkes::LockPtr<fawkes::NavGraph> navgraph;
00348 fawkes::FileAlterationMonitor *fam_;
00349
00350 ros::Publisher pub_navgraph_;
00351 ros::ServiceServer svs_search_path_;
00352 ros::ServiceServer svs_get_pwcosts_;
00353 ros::WallTimer fam_timer_;
00354
00355 TransformListenerExtended *tf_listener;
00356 };
00357
00358 int
00359 main(int argc, char **argv)
00360 {
00361 ros::init(argc, argv, "navgraph_node");
00362
00363 ros::NodeHandle n;
00364 NavGraphRosNode navgraph_node(n);
00365 ros::spin();
00366
00367 return 0;
00368 }