fawkes_navgraph_node.cpp
Go to the documentation of this file.
00001 /***************************************************************************
00002  *  navgraph_node.cpp - navgraph access from ROS
00003  *
00004  *  Created: Thu Mar 09 20:28:10 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 <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                                                                      /* recursive mutex */ 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"), /* recursive mutex */ 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                 //printf("Watching file %s\n", cfg_navgraph_file_.c_str());
00092                 //fam_->watch_file(cfg_navgraph_file_.c_str());
00093                 fam_->add_listener(this);
00094 
00095                 tf_listener = new TransformListenerExtended();
00096                 
00097                 pub_navgraph_ = n.advertise<fawkes_msgs::NavGraph>("navgraph", 10, /* latching */ 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                 // The file will be ignored from now onwards, re-register
00119                 // if (mask & FAM_IGNORED) {
00120                 //      boost::filesystem::path p(cfg_navgraph_file_);
00121                 //      fam_->watch_dir(p.parent_path().string().c_str());
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                                                                           /* recursive mutex */ 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                 // translate path into result
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                                         //ROS_WARN("[F-NavGraph] From node %s is UNCONNECTED, starting instead from %s",
00303                                         //         from_node.name().c_str(), start_node.name().c_str());
00304                                 } else {
00305                                         start_node = from_node;
00306                                 }
00307                                 if (to_node.unconnected()) {
00308                                         goal_node = navgraph->closest_node_to(to_node.name());
00309                                         //ROS_WARN("[F-NavGraph] To node %s is UNCONNECTED, ending instead at %s",
00310                                         //         to_node.name().c_str(), goal_node.name().c_str());
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 }


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