22 #include "navgraph_thread.h" 24 #include <core/utils/lockptr.h> 25 #include <navgraph/constraints/constraint_repo.h> 26 #include <navgraph/yaml_navgraph.h> 28 #include <utils/math/angle.h> 42 :
Thread(
"NavGraphThread",
Thread::OPMODE_WAITFORWAKEUP),
46 #ifdef HAVE_VISUALIZATION 51 #ifdef HAVE_VISUALIZATION 54 :
Thread(
"NavGraphThread",
Thread::OPMODE_WAITFORWAKEUP),
84 cfg_abort_on_error_ =
config->
get_bool(
"/navgraph/abort_on_error");
85 #ifdef HAVE_VISUALIZATION 86 cfg_visual_interval_ =
config->
get_float(
"/navgraph/visualization_interval");
88 cfg_monitor_file_ =
false;
94 cfg_allow_multi_graph_ =
false;
96 cfg_allow_multi_graph_ =
config->
get_bool(
"/navgraph/allow_multi_graph");
100 cfg_enable_path_execution_ =
true;
102 cfg_enable_path_execution_ =
config->
get_bool(
"/navgraph/path_execution");
116 throw Exception(
"Navgraph tolerances may no longer be set in the config");
119 if (cfg_enable_path_execution_) {
125 if (!cfg_graph_file_.empty()) {
126 if (cfg_graph_file_[0] !=
'/') {
127 cfg_graph_file_ = std::string(CONFDIR) +
"/" + cfg_graph_file_;
129 graph_ = load_graph(cfg_graph_file_);
135 throw Exception(
"Graph must specify travel tolerance");
138 throw Exception(
"Graph must specify target tolerance");
141 throw Exception(
"Graph must specify orientation tolerance");
144 throw Exception(
"Graph must specify shortcut tolerance");
153 "Using target orientation time %f from graph file",
154 cfg_target_ori_time_);
158 if (cfg_log_graph_) {
162 if (cfg_monitor_file_) {
174 exec_active_ =
false;
175 target_reached_ =
false;
176 target_ori_reached_ =
false;
177 target_rotating_ =
false;
180 constrained_plan_ =
false;
186 #ifdef HAVE_VISUALIZATION 200 delete path_planned_at_;
201 delete target_reached_at_;
203 #ifdef HAVE_VISUALIZATION 204 delete visualized_at_;
210 if (cfg_enable_path_execution_) {
220 #ifdef HAVE_VISUALIZATION 222 vt_->set_constraint_repo(constraint_repo_);
223 vt_->set_graph(graph_);
232 bool needs_write =
false;
233 while (cfg_enable_path_execution_ && !pp_nav_if_->
msgq_empty()) {
243 exec_active_ =
false;
246 "Received stop message for non-active command " 247 "(got %u, running %u)",
249 pp_nav_if_->
msgid());
255 name(),
"cartesian goto (x,y,ori) = (%f,%f,%f)", msg->
x(), msg->
y(), msg->
orientation());
270 if (generate_plan(msg->
place())) {
293 if (cfg_monitor_file_) {
297 if (cfg_enable_path_execution_ && exec_active_) {
304 }
else if (target_reached_) {
310 exec_active_ =
false;
313 }
else if (target_ori_reached_) {
314 if ((now - target_reached_at_) >= target_time_) {
319 }
else if (!target_rotating_ && (now - target_reached_at_) >= target_time_) {
323 target_rotating_ =
true;
329 }
else if (target_rotating_ && node_ori_reached()) {
336 if (target_time_ == 0)
337 target_time_ = cfg_target_ori_time_;
339 target_ori_reached_ =
true;
340 target_reached_at_->
stamp();
343 }
else if (node_reached()) {
346 if (traversal_.
last()) {
351 if (target_time_ == 0)
352 target_time_ = cfg_target_time_;
354 target_reached_ =
true;
355 target_reached_at_->
stamp();
357 }
else if (traversal_.
next()) {
362 "Sending next goal %s after node reached",
371 }
else if ((shortcut_to = shortcut_possible()) > 0) {
373 "Shortcut possible, jumping from '%s' to '%s'",
375 traversal_.
path().
nodes()[shortcut_to].name().c_str());
391 bool new_plan =
false;
393 if (traversal_.
remaining() > 2 && (now - path_planned_at_) > cfg_replan_interval_) {
394 *path_planned_at_ = now;
395 constraint_repo_.
lock();
396 if (constraint_repo_->
compute() || constraint_repo_->
modified(
true)) {
405 constraint_repo_.
unlock();
408 if (!new_plan && (now - cmd_sent_at_) > cfg_resend_interval_) {
420 #ifdef HAVE_VISUALIZATION 423 if (now - visualized_at_ >= cfg_visual_interval_) {
424 *visualized_at_ = now;
425 constraint_repo_.
lock();
426 if (constraint_repo_->
compute() || constraint_repo_->
modified(
false)) {
429 constraint_repo_.
unlock();
434 if (cfg_enable_path_execution_ && needs_write) {
440 NavGraphThread::load_graph(std::string filename)
442 std::ifstream inf(filename);
443 std::string firstword;
447 if (firstword ==
"%YAML") {
457 NavGraphThread::generate_plan(
const std::string &goal_name)
461 if (cfg_enable_path_execution_) {
463 pp_nav_if_->
set_error_code(NavigatorInterface::ERROR_PATH_GEN_FAIL);
471 logger->
log_error(
name(),
"Failed to generate path to %s: goal is unknown", goal_name.c_str());
472 if (cfg_enable_path_execution_) {
474 pp_nav_if_->
set_error_code(NavigatorInterface::ERROR_UNKNOWN_PLACE);
480 return generate_plan(goal.
x(),
483 : std::numeric_limits<float>::quiet_NaN(),
490 "Starting at (%f,%f), closest node is '%s'",
491 pose_.getOrigin().x(),
492 pose_.getOrigin().y(),
493 init.name().c_str());
499 "Failed to generate path from (%.2f,%.2f) to %s: %s",
504 if (cfg_enable_path_execution_) {
506 pp_nav_if_->
set_error_code(NavigatorInterface::ERROR_PATH_GEN_FAIL);
511 if (!path_.
empty()) {
512 constrained_plan_ =
true;
514 constrained_plan_ =
false;
519 if (cfg_enable_path_execution_) {
521 pp_nav_if_->
set_error_code(NavigatorInterface::ERROR_PATH_GEN_FAIL);
530 pp_nav_if_->
set_error_code(NavigatorInterface::ERROR_PATH_GEN_FAIL);
539 NavGraphThread::generate_plan(
const std::string &goal_name,
float ori)
541 if (generate_plan(goal_name)) {
542 if (!path_.
empty() && std::isfinite(ori)) {
543 path_.
nodes_mutable().back().set_property(
"orientation", ori);
549 if (cfg_enable_path_execution_) {
551 pp_nav_if_->
set_error_code(NavigatorInterface::ERROR_PATH_GEN_FAIL);
558 NavGraphThread::generate_plan(
float x,
float y,
float ori,
const std::string &target_name)
563 "Cannot find closest node to target (%.2f,%.2f,%.2f) alias %s",
567 target_name.c_str());
570 if (generate_plan(close_to_goal.
name())) {
572 if (std::isfinite(ori)) {
573 n.set_property(
"orientation", ori);
581 if (cfg_enable_path_execution_) {
583 pp_nav_if_->
set_error_code(NavigatorInterface::ERROR_PATH_GEN_FAIL);
597 if (goal.
name() ==
"free-target") {
599 act_goal = close_to_goal;
607 if (!new_path.
empty()) {
609 NavGraphNode pose(
"current-pose", pose_.getOrigin().x(), pose_.getOrigin().y());
611 float new_cost = new_path.
cost();
613 if (new_cost <= old_cost * cfg_replan_factor_) {
614 constrained_plan_ =
true;
616 if (goal.
name() ==
"free-target") {
622 "Executing after re-planning from '%s' to '%s', " 623 "old cost: %f new cost: %f (%f * %f)",
624 start.name().c_str(),
627 new_cost * cfg_replan_factor_,
633 "Re-planning from '%s' to '%s' resulted in " 634 "more expensive plan: %f > %f (%f * %f), keeping old",
635 start.name().c_str(),
638 old_cost * cfg_replan_factor_,
645 "Failed to re-plan from '%s' to '%s'",
646 start.name().c_str(),
647 goal.
name().c_str());
663 NavGraphThread::optimize_plan()
668 double sqr_dist_a = (pow(pose_.getOrigin().x() - path.
nodes()[0].x(), 2)
669 + pow(pose_.getOrigin().y() - path.
nodes()[0].y(), 2));
670 double sqr_dist_b = (pow(path.
nodes()[0].x() - path.
nodes()[1].x(), 2)
671 + pow(path.
nodes()[0].y() - path.
nodes()[1].y(), 2));
672 double sqr_dist_c = (pow(pose_.getOrigin().x() - path.
nodes()[1].x(), 2)
673 + pow(pose_.getOrigin().y() - path.
nodes()[1].y(), 2));
675 if (sqr_dist_a + sqr_dist_b >= sqr_dist_c) {
682 NavGraphThread::start_plan()
684 if (!cfg_enable_path_execution_)
687 path_planned_at_->
stamp();
689 target_reached_ =
false;
690 target_ori_reached_ =
false;
691 target_rotating_ =
false;
693 exec_active_ =
false;
695 pp_nav_if_->
set_error_code(NavigatorInterface::ERROR_UNKNOWN_PLACE);
698 #ifdef HAVE_VISUALIZATION 701 visualized_at_->stamp();
708 std::string m = path_.
nodes()[0].name();
709 for (
unsigned int i = 1; i < path_.
size(); ++i) {
710 m +=
" - " + path_.
nodes()[i].name();
713 #ifdef HAVE_VISUALIZATION 715 vt_->set_traversal(traversal_);
716 visualized_at_->stamp();
742 NavGraphThread::stop_motion()
744 if (!cfg_enable_path_execution_)
755 exec_active_ =
false;
756 target_ori_reached_ =
false;
757 target_rotating_ =
false;
762 #ifdef HAVE_VISUALIZATION 765 visualized_at_->stamp();
771 NavGraphThread::send_next_goal()
773 if (!cfg_enable_path_execution_)
776 bool stop_at_target =
false;
777 bool orient_at_target =
false;
780 throw Exception(
"Cannot send next goal if plan is empty");
786 if (traversal_.
last()) {
787 stop_at_target =
true;
790 orient_at_target =
true;
802 ori = atan2f(next_next_target.
y() - next_target.
y(), next_next_target.
x() - next_target.
x());
808 tf::Vector3(next_target.
x(), next_target.
y(), 0)),
818 if (target_reached_) {
820 tpose.setOrigin(tf::Vector3(0.f, 0.f, 0.f));
825 tpose.getOrigin().y(),
826 tf::get_yaw(tpose.getRotation()));
831 if (orient_at_target) {
833 fawkes::NavigatorInterface::OrientationMode::OrientAtTarget);
836 fawkes::NavigatorInterface::OrientationMode::OrientDuringTravel);
840 #ifdef HAVE_VISUALIZATION 842 vt_->set_current_edge(last_node_, next_target.
name());
846 throw Exception(
"No writer for navigator interface");
853 "Sending goto(x=%f,y=%f,ori=%f) for node '%s'",
854 tpose.getOrigin().x(),
855 tpose.getOrigin().y(),
856 tf::get_yaw(tpose.getRotation()),
857 next_target.
name().c_str());
861 cmd_msgid_ = gotomsg->
id();
863 cmd_sent_at_->
stamp();
869 if (cfg_abort_on_error_) {
871 "Failed to send cartesian goto for " 872 "next goal, exception follows");
874 exec_active_ =
false;
876 pp_nav_if_->
set_error_code(NavigatorInterface::ERROR_OBSTRUCTION);
878 #ifdef HAVE_VISUALIZATION 888 "Failed to send cartesian goto for " 889 "next goal, exception follows");
901 NavGraphThread::node_reached()
916 float dist = sqrt(pow(pose_.getOrigin().x() - cur_target.
x(), 2)
917 + pow(pose_.getOrigin().y() - cur_target.
y(), 2));
921 if (traversal_.
last()) {
927 if (tolerance == 0.) {
932 return (dist <= tolerance);
940 NavGraphThread::node_ori_reached()
953 return node_ori_reached(cur_target);
961 NavGraphThread::node_ori_reached(
const NavGraphNode &node)
969 return (ori_diff <= ori_tolerance);
977 NavGraphThread::shortcut_possible()
979 if (!traversal_ || traversal_.
remaining() < 1) {
988 sqrt(pow(pose_.getOrigin().x() - node.
x(), 2) + pow(pose_.getOrigin().y() - node.
y(), 2));
992 if (tolerance == 0.0)
994 if (dist <= tolerance)
1014 **graph_ = **new_graph;
1021 #ifdef HAVE_VISUALIZATION 1023 vt_->set_graph(graph_);
1024 visualized_at_->stamp();
1034 bool gen_ok =
false;
1035 if (goal.
name() ==
"free-target") {
1038 gen_ok = generate_plan(goal.
name());
1052 NavGraphThread::log_graph()
1054 const std::vector<NavGraphNode> & nodes = graph_->
nodes();
1055 std::vector<NavGraphNode>::const_iterator n;
1056 for (n = nodes.begin(); n != nodes.end(); ++n) {
1058 "Node %s @ (%f,%f)%s",
1062 n->unconnected() ?
" UNCONNECTED" :
"");
1064 const std::map<std::string, std::string> & props = n->properties();
1065 std::map<std::string, std::string>::const_iterator p;
1066 for (p = props.begin(); p != props.end(); ++p) {
1071 std::vector<NavGraphEdge> edges = graph_->
edges();
1072 std::vector<NavGraphEdge>::iterator e;
1073 for (e = edges.begin(); e != edges.end(); ++e) {
1075 "Edge %10s --%s %s",
1077 e->is_directed() ?
">" :
"-",
1080 const std::map<std::string, std::string> & props = e->properties();
1081 std::map<std::string, std::string>::const_iterator p;
1082 for (p = props.begin(); p != props.end(); ++p) {
1089 NavGraphThread::publish_path()
1091 if (!cfg_enable_path_execution_)
1094 std::vector<std::string> vpath(40,
"");
1099 for (; r < traversal_.
path().
size(); ++r) {
1100 vpath[ind++] = traversal_.
path().
nodes()[r].name();
void set_navgraph(LockPtr< NavGraph > &navgraph)
Set navgraph.
void set_path_node_37(const char *new_path_node_37)
Set path_node_37 value.
fawkes::LockPtr< NavGraphConstraintRepo > constraint_repo() const
Get locked pointer to constraint repository.
size_t size() const
Get size of path.
float default_property_as_float(const std::string &prop) const
Get property converted to float.
void set_path_node_29(const char *new_path_node_29)
Set path_node_29 value.
unsigned int id() const
Get message ID.
virtual ~NavGraphThread()
Destructor.
void set_path_node_26(const char *new_path_node_26)
Set path_node_26 value.
void set_path_node_14(const char *new_path_node_14)
Set path_node_14 value.
bool msgq_empty()
Check if queue is empty.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
void unref()
Decrement reference count and conditionally delete this instance.
float y() const
Get y value.
NavGraphThread()
Constructor.
char * place() const
Get place value.
bool compute()
Call compute method on all registered constraints.
void set_path_node_16(const char *new_path_node_16)
Set path_node_16 value.
void set_path_node_1(const char *new_path_node_1)
Set path_node_1 value.
void set_path_node_15(const char *new_path_node_15)
Set path_node_15 value.
std::vector< NavGraphNode > & nodes_mutable()
Get nodes along the path as mutable vector.
void set_path_node_27(const char *new_path_node_27)
Set path_node_27 value.
float normalize_rad(float angle_rad)
Normalize angle in radian between 0 (inclusive) and 2*PI (exclusive).
void set_path_node_19(const char *new_path_node_19)
Set path_node_19 value.
const NavGraphNode & current() const
Get current node in path.
Fawkes library namespace.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
void set_path_node_6(const char *new_path_node_6)
Set path_node_6 value.
void set_final(const bool new_final)
Set final value.
void set_path_node_18(const char *new_path_node_18)
Set path_node_18 value.
void set_path_node_30(const char *new_path_node_30)
Set path_node_30 value.
void unlock() const
Unlock object mutex.
void set_path_node_25(const char *new_path_node_25)
Set path_node_25 value.
A class for handling time.
bool has_property(const std::string &property) const
Check if node has specified property.
Class representing a path for a NavGraph.
Send Marker messages to rviz to show navgraph info.
fawkes::NavGraphPath search_path(const std::string &from, const std::string &to, bool use_constraints=true, bool compute_constraints=true)
Search for a path between two nodes with default distance costs.
virtual const char * what() const
Get primary string.
bool is_valid() const
Check if node is valid, i.e.
Thread class encapsulation of pthreads.
void set_current(size_t new_current)
Set the current node.
void set_path_node_36(const char *new_path_node_36)
Set path_node_36 value.
SetOrientationModeMessage Fawkes BlackBoard Interface Message.
void write()
Write from local copy into BlackBoard memory.
void set_path_node_11(const char *new_path_node_11)
Set path_node_11 value.
Logger * logger
This is the Logger member used to access the logger.
const std::vector< NavGraphNode > & nodes() const
Get nodes of the graph.
void add_change_listener(ChangeListener *listener)
Add a change listener.
float orientation() const
Get orientation value.
void set_path_node_24(const char *new_path_node_24)
Set path_node_24 value.
float cost() const
Get cost of path from start to to end.
void set_path_node_28(const char *new_path_node_28)
Set path_node_28 value.
virtual void init()
Initialize the thread.
LockPtr<> is a reference-counting shared lockable smartpointer.
bool last() const
Check if the current node is the last node in the path.
void set_path_node_22(const char *new_path_node_22)
Set path_node_22 value.
SetStopAtTargetMessage Fawkes BlackBoard Interface Message.
Clock * clock
By means of this member access to the clock is given.
Thread aspect to use blocked timing.
void set_path_node_35(const char *new_path_node_35)
Set path_node_35 value.
void msgq_pop()
Erase first message from queue.
void set_path_node_32(const char *new_path_node_32)
Set path_node_32 value.
PlaceGotoMessage Fawkes BlackBoard Interface Message.
void set_path_node_31(const char *new_path_node_31)
Set path_node_31 value.
void set_path_node_13(const char *new_path_node_13)
Set path_node_13 value.
bool is_final() const
Get final value.
void set_path_node_17(const char *new_path_node_17)
Set path_node_17 value.
static const unsigned int FAM_IGNORED
File was ignored.
Base class for exceptions in Fawkes.
Message * msgq_first()
Get the first message from the message queue.
void set_path_node_40(const char *new_path_node_40)
Set path_node_40 value.
const NavGraphNode & peek_next() const
Peek on the next node.
void read()
Read from BlackBoard into local copy.
CartesianGotoMessage Fawkes BlackBoard Interface Message.
void set_path_node_38(const char *new_path_node_38)
Set path_node_38 value.
NavGraph * load_yaml_navgraph(std::string filename, bool allow_multi_graph)
Load topological map graph stored in RCSoft format.
virtual void once()
Execute an action exactly once.
size_t remaining() const
Get the number of remaining nodes in path traversal.
char * place() const
Get place value.
void set_path_node_12(const char *new_path_node_12)
Set path_node_12 value.
void remove_change_listener(ChangeListener *listener)
Remove a change listener.
float orientation() const
Get orientation value.
void ref()
Increment reference count.
NavGraphNode node(const std::string &name) const
Get a specified node.
void set_path_node_2(const char *new_path_node_2)
Set path_node_2 value.
void set_path_length(const uint32_t new_path_length)
Set path_length value.
const std::string & name() const
Get name of node.
bool has_writer() const
Check if there is a writer for the interface.
const std::vector< NavGraphNode > & nodes() const
Get nodes along the path.
Traversal traversal() const
Get a new path traversal handle.
const char * name() const
Get name of thread.
void apply_default_properties(NavGraphNode &node)
Set default properties on node for which no local value exists.
Monitors files for changes.
const std::vector< NavGraphEdge > & edges() const
Get edges of the graph.
virtual const char * what_no_backtrace() const
Get primary string (does not implicitly print the back trace).
virtual void loop()
Code to execute in the thread.
bool msgq_first_is()
Check if first message has desired type.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
bool running() const
Check if traversal is currently runnung.
const NavGraphPath & path() const
Get parent path the traversal belongs to.
float y() const
Get Y coordinate in global frame.
void set_path_node_10(const char *new_path_node_10)
Set path_node_10 value.
bool empty() const
Check if path is empty.
void set_error_code(const uint32_t new_error_code)
Set error_code value.
size_t current_index() const
Get index of current node in path.
virtual void finalize()
Finalize the thread.
Thread aspect provide a new aspect.
void set_path_node_4(const char *new_path_node_4)
Set path_node_4 value.
NavGraphNode closest_node(float pos_x, float pos_y, const std::string &property="") const
Get node closest to a specified point with a certain property.
Wrapper class to add time stamp and frame ID to base types.
unsigned int msgq_enqueue(Message *message)
Enqueue message at end of queue.
void set_path_node_9(const char *new_path_node_9)
Set path_node_9 value.
void clear()
Set underlying instance to 0, decrementing reference count of existing instance appropriately.
bool unconnected() const
Check if this node shall be unconnected.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
float remaining_cost() const
Get the remaining cost to the goal.
void add_listener(FamListener *listener)
Add a listener.
void set_dest_x(const float new_dest_x)
Set dest_x value.
uint32_t msgid() const
Get msgid value.
void set_path_node_34(const char *new_path_node_34)
Set path_node_34 value.
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
void set_path_node_33(const char *new_path_node_33)
Set path_node_33 value.
void set_path_node_20(const char *new_path_node_20)
Set path_node_20 value.
bool has_default_property(const std::string &property) const
Check if graph has specified default property.
float x() const
Get X coordinate in global frame.
PlaceWithOriGotoMessage Fawkes BlackBoard Interface Message.
void invalidate()
Invalidate this traversal.
bool modified(bool reset_modified=false)
Check if the constraint repo has been modified.
void set_path_node_21(const char *new_path_node_21)
Set path_node_21 value.
virtual void fam_event(const char *filename, unsigned int mask)
Event has been raised.
void set_path_node_8(const char *new_path_node_8)
Set path_node_8 value.
void set_path_node_7(const char *new_path_node_7)
Set path_node_7 value.
float x() const
Get x value.
virtual bool exists(const char *path)=0
Check if a given value exists.
Time & stamp()
Set this time to the current time.
static const unsigned int FAM_MODIFY
File was modified.
float property_as_float(const std::string &prop) const
Get property converted to float.
void add_node(const NavGraphNode &node, float cost_from_end=0)
Add a node to the path.
void process_events(int timeout=0)
Process events.
void set_path_node_23(const char *new_path_node_23)
Set path_node_23 value.
bool next()
Move on traversal to next node.
void set_dest_y(const float new_dest_y)
Set dest_y value.
NavPathInterface Fawkes BlackBoard Interface.
void set_path_node_5(const char *new_path_node_5)
Set path_node_5 value.
void set_msgid(const uint32_t new_msgid)
Set msgid value.
uint32_t msgid() const
Get msgid value.
Configuration * config
This is the Configuration member used to access the configuration.
void set_path_node_39(const char *new_path_node_39)
Set path_node_39 value.
void set_path_node_3(const char *new_path_node_3)
Set path_node_3 value.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
void watch_file(const char *filepath)
Watch a file.
void lock() const
Lock access to the encapsulated object.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
float cost(const NavGraphNode &from, const NavGraphNode &to) const
Calculate cost between two adjacent nodes.
void start(bool wait=true)
Call this method to start the thread.
float angle_distance(float angle_rad1, float angle_rad2)
Determines the distance between two angle provided as radians.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
StopMessage Fawkes BlackBoard Interface Message.
NavigatorInterface Fawkes BlackBoard Interface.
const NavGraphNode & goal() const
Get goal of path.
virtual void close(Interface *interface)=0
Close interface.