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.
virtual void init()
Initialize the thread.
virtual ~NavGraphROSPubThread()
Destructor.
virtual void loop()
Code to execute in the thread.
virtual void finalize()
Finalize the thread.
virtual void graph_changed() noexcept
Function called if the graph has been changed.
Configuration * config
This is the Configuration member used to access the configuration.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
Base class for exceptions in Fawkes.
virtual const char * what() const noexcept
Get primary string.
virtual const char * what_no_backtrace() const noexcept
Get primary string (does not implicitly print the back trace).
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
Logger * logger
This is the Logger member used to access the logger.
fawkes::LockPtr< NavGraph > navgraph
NavGraph instance shared in framework.
bool unconnected() const
Check if this node shall be unconnected.
bool is_valid() const
Check if node is valid, i.e.
const std::string & name() const
Get name of node.
Class representing a path for a NavGraph.
bool empty() const
Check if path is empty.
const std::vector< NavGraphNode > & nodes() const
Get nodes along the path.
float cost() const
Get cost of path from start to to end.
void add_node(const NavGraphNode &node, float cost_from_end=0)
Add a node to the path.
LockPtr< ros::NodeHandle > rosnode
Central ROS node handle.
Thread class encapsulation of pthreads.
const char * name() const
Get name of thread.
Fawkes library namespace.