21 #include "rospub_thread.h" 23 #include <core/threading/mutex_locker.h> 24 #include <fawkes_msgs/NavGraph.h> 25 #include <fawkes_msgs/NavGraphEdge.h> 26 #include <fawkes_msgs/NavGraphNode.h> 40 :
Thread(
"NavGraphROSPubThread",
Thread::OPMODE_WAITFORWAKEUP)
55 pub_ =
rosnode->advertise<fawkes_msgs::NavGraph>(
"navgraph", 10,
true);
56 svs_search_path_ =
rosnode->advertiseService(
"navgraph/search_path",
57 &NavGraphROSPubThread::svs_search_path_cb,
59 svs_get_pwcosts_ =
rosnode->advertiseService(
"navgraph/get_pairwise_costs",
60 &NavGraphROSPubThread::svs_get_pwcosts_cb,
71 navgraph->remove_change_listener(
this);
73 svs_search_path_.shutdown();
74 svs_get_pwcosts_.shutdown();
90 }
catch (std::runtime_error &e) {
96 NavGraphROSPubThread::convert_nodes(
const std::vector<fawkes::NavGraphNode> &nodes,
97 std::vector<fawkes_msgs::NavGraphNode> & out)
100 fawkes_msgs::NavGraphNode ngn;
101 ngn.name = node.name();
102 ngn.has_orientation = node.has_property(navgraph::PROP_ORIENTATION);
103 ngn.pose.x = node.x();
104 ngn.pose.y = node.y();
105 if (ngn.has_orientation) {
106 ngn.pose.theta = node.property_as_float(navgraph::PROP_ORIENTATION);
108 ngn.unconnected = node.unconnected();
109 const std::map<std::string, std::string> &props = node.properties();
110 for (
const auto p : props) {
111 fawkes_msgs::NavGraphProperty ngp;
113 ngp.value = p.second;
114 ngn.properties.push_back(ngp);
121 NavGraphROSPubThread::publish_graph()
125 fawkes_msgs::NavGraph ngm;
127 const std::vector<NavGraphNode> &nodes =
navgraph->nodes();
128 convert_nodes(nodes, ngm.nodes);
130 const std::vector<NavGraphEdge> &edges =
navgraph->edges();
132 fawkes_msgs::NavGraphEdge nge;
133 nge.from_node = edge.from();
134 nge.to_node = edge.to();
135 nge.directed = edge.is_directed();
136 const std::map<std::string, std::string> &props = edge.properties();
137 for (
const auto p : props) {
138 fawkes_msgs::NavGraphProperty ngp;
140 ngp.value = p.second;
141 nge.properties.push_back(ngp);
143 ngm.edges.push_back(nge);
150 NavGraphROSPubThread::svs_search_path_cb(fawkes_msgs::NavGraphSearchPath::Request & req,
151 fawkes_msgs::NavGraphSearchPath::Response &res)
155 if (req.from_node.empty()) {
161 res.errmsg =
"Failed to compute pose, cannot generate plan";
165 from =
navgraph->closest_node(pose.getOrigin().x(), pose.getOrigin().y());
168 res.errmsg =
"Failed to get closest node to pose";
172 fawkes_msgs::NavGraphNode free_start;
173 free_start.name =
"free-start";
174 free_start.pose.x = pose.getOrigin().x();
175 free_start.pose.y = pose.getOrigin().y();
176 free_start.has_orientation =
true;
177 free_start.pose.theta = tf::get_yaw(pose.getRotation());
178 res.path.push_back(free_start);
180 from =
navgraph->node(req.from_node);
183 res.errmsg =
"Failed to find start node " + req.from_node;
190 if (!req.to_node.empty()) {
194 path =
navgraph->search_path(from, close_to_goal);
196 NavGraphNode free_target(
"free-target", req.to_pose.x, req.to_pose.y);
197 if (std::isfinite(req.to_pose.theta)) {
198 free_target.set_property(
"orientation", (
float)req.to_pose.theta);
205 convert_nodes(path.
nodes(), res.path);
206 res.cost = path.
cost();
213 NavGraphROSPubThread::svs_get_pwcosts_cb(fawkes_msgs::NavGraphGetPairwiseCosts::Request & req,
214 fawkes_msgs::NavGraphGetPairwiseCosts::Response &res)
216 for (
unsigned int i = 0; i < req.nodes.size(); ++i) {
217 for (
unsigned int j = 0; j < req.nodes.size(); ++j) {
223 from_node =
navgraph->node(req.nodes[i]);
224 to_node =
navgraph->node(req.nodes[j]);
227 res.errmsg =
"Failed to get path from '" + req.nodes[i] +
"' to '" + req.nodes[j]
229 res.path_costs.clear();
236 start_node =
navgraph->closest_node_to(from_node.
name());
240 start_node = from_node;
253 "Failed to get path from '" + start_node.
name() +
"' to '" + goal_node.
name() +
"'";
254 res.path_costs.clear();
257 fawkes_msgs::NavGraphPathCost pc;
258 pc.from_node = req.nodes[i];
259 pc.to_node = req.nodes[j];
262 pc.cost +=
navgraph->cost(from_node, start_node);
265 pc.cost +=
navgraph->cost(goal_node, to_node);
267 res.path_costs.push_back(pc);
NavGraphROSPubThread()
Constructor.
fawkes::LockPtr< NavGraph > navgraph
NavGraph instance shared in framework.
virtual void init()
Initialize the thread.
Fawkes library namespace.
Class representing a path for a NavGraph.
bool is_valid() const
Check if node is valid, i.e.
Thread class encapsulation of pthreads.
Logger * logger
This is the Logger member used to access the logger.
float cost() const
Get cost of path from start to to end.
virtual void finalize()
Finalize the thread.
Base class for exceptions in Fawkes.
const std::string & name() const
Get name of node.
const std::vector< NavGraphNode > & nodes() const
Get nodes along the path.
const char * name() const
Get name of thread.
virtual const char * what_no_backtrace() const
Get primary string (does not implicitly print the back trace).
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
bool empty() const
Check if path is empty.
virtual void loop()
Code to execute in the thread.
bool unconnected() const
Check if this node shall be unconnected.
virtual ~NavGraphROSPubThread()
Destructor.
LockPtr< ros::NodeHandle > rosnode
Central ROS node handle.
void add_node(const NavGraphNode &node, float cost_from_end=0)
Add a node to the path.
Configuration * config
This is the Configuration member used to access the configuration.
virtual void graph_changed()
Function called if the graph has been changed.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.