22 #include "navgraph_thread.h" 24 #include <navgraph/yaml_navgraph.h> 25 #include <navgraph/constraints/constraint_repo.h> 26 #include <utils/math/angle.h> 28 #include <core/utils/lockptr.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;
93 cfg_enable_path_execution_ =
true;
95 cfg_enable_path_execution_ =
config->
get_bool(
"/navgraph/path_execution");
110 throw Exception(
"Navgraph tolerances may no longer be set in the config");
113 if (cfg_enable_path_execution_) {
120 if (! cfg_graph_file_.empty()) {
121 if (cfg_graph_file_[0] !=
'/') {
122 cfg_graph_file_ = std::string(CONFDIR) +
"/" + cfg_graph_file_;
124 graph_ = load_graph(cfg_graph_file_);
130 throw Exception(
"Graph must specify travel tolerance");
133 throw Exception(
"Graph must specify target tolerance");
136 throw Exception(
"Graph must specify orientation tolerance");
139 throw Exception(
"Graph must specify shortcut tolerance");
147 logger->
log_info(
name(),
"Using target orientation time %f from graph file", cfg_target_ori_time_);
151 if (cfg_log_graph_) {
155 if (cfg_monitor_file_) {
168 exec_active_ =
false;
169 target_reached_ =
false;
170 target_ori_reached_=
false;
171 target_rotating_ =
false;
174 constrained_plan_ =
false;
179 #ifdef HAVE_VISUALIZATION 193 delete path_planned_at_;
194 delete target_reached_at_;
196 #ifdef HAVE_VISUALIZATION 197 delete visualized_at_;
203 if (cfg_enable_path_execution_) {
213 #ifdef HAVE_VISUALIZATION 215 vt_->set_constraint_repo(constraint_repo_);
216 vt_->set_graph(graph_);
225 bool needs_write =
false;
226 while (cfg_enable_path_execution_ && ! pp_nav_if_->
msgq_empty()) {
234 exec_active_ =
false;
254 if (generate_plan(msg->
place())) {
277 if (cfg_monitor_file_) {
281 if (cfg_enable_path_execution_ && exec_active_) {
288 }
else if (target_reached_) {
294 exec_active_ =
false;
297 }
else if (target_ori_reached_) {
298 if ((now - target_reached_at_) >= target_time_) {
303 }
else if (!target_rotating_ && (now - target_reached_at_) >= target_time_) {
307 target_rotating_ =
true;
313 }
else if (target_rotating_ && node_ori_reached()) {
320 if (target_time_ == 0) target_time_ = cfg_target_ori_time_;
322 target_ori_reached_ =
true;
323 target_reached_at_->
stamp();
326 }
else if (node_reached()) {
330 if (traversal_.
last()) {
335 if (target_time_ == 0) target_time_ = cfg_target_time_;
337 target_reached_ =
true;
338 target_reached_at_->
stamp();
340 }
else if (traversal_.
next()) {
353 }
else if ((shortcut_to = shortcut_possible()) > 0) {
356 traversal_.
path().
nodes()[shortcut_to].name().c_str());
372 bool new_plan =
false;
374 if (traversal_.
remaining() > 2 && (now - path_planned_at_) > cfg_replan_interval_)
376 *path_planned_at_ = now;
377 constraint_repo_.
lock();
378 if (constraint_repo_->
compute() || constraint_repo_->
modified(
true)) {
387 constraint_repo_.
unlock();
390 if (! new_plan && (now - cmd_sent_at_) > cfg_resend_interval_) {
402 #ifdef HAVE_VISUALIZATION 405 if (now - visualized_at_ >= cfg_visual_interval_) {
406 *visualized_at_ = now;
407 constraint_repo_.
lock();
408 if (constraint_repo_->
compute() || constraint_repo_->
modified(
false)) {
411 constraint_repo_.
unlock();
416 if (cfg_enable_path_execution_ && needs_write) {
422 NavGraphThread::load_graph(std::string filename)
424 std::ifstream inf(filename);
425 std::string firstword;
429 if (firstword ==
"%YAML") {
438 NavGraphThread::generate_plan(std::string goal_name)
442 "Failed to compute pose, cannot generate plan");
443 if (cfg_enable_path_execution_) {
445 pp_nav_if_->
set_error_code(NavigatorInterface::ERROR_PATH_GEN_FAIL);
451 graph_->
closest_node(pose_.getOrigin().x(), pose_.getOrigin().y());
455 logger->
log_error(
name(),
"Failed to generate path from (%.2f,%.2f) to %s: goal is unknown",
456 init.
x(), init.
y(), goal_name.c_str());
457 if (cfg_enable_path_execution_) {
459 pp_nav_if_->
set_error_code(NavigatorInterface::ERROR_UNKNOWN_PLACE);
465 pose_.getOrigin().x(), pose_.getOrigin().y(), init.
name().c_str());
472 if (cfg_enable_path_execution_) {
474 pp_nav_if_->
set_error_code(NavigatorInterface::ERROR_PATH_GEN_FAIL);
479 if (! path_.
empty()) {
480 constrained_plan_ =
true;
482 constrained_plan_ =
false;
487 if (cfg_enable_path_execution_) {
489 pp_nav_if_->
set_error_code(NavigatorInterface::ERROR_PATH_GEN_FAIL);
505 NavGraphThread::generate_plan(std::string goal_name,
float ori)
507 if (generate_plan(goal_name)) {
509 if (! path_.
empty() && std::isfinite(ori)) {
510 path_.
nodes_mutable().back().set_property(
"orientation", ori);
516 if (cfg_enable_path_execution_) {
518 pp_nav_if_->
set_error_code(NavigatorInterface::ERROR_PATH_GEN_FAIL);
525 NavGraphThread::generate_plan(
float x,
float y,
float ori)
528 if (generate_plan(close_to_goal.
name())) {
531 if (std::isfinite(ori)) {
540 if (cfg_enable_path_execution_) {
542 pp_nav_if_->
set_error_code(NavigatorInterface::ERROR_PATH_GEN_FAIL);
557 if (goal.
name() ==
"free-target") {
559 act_goal = close_to_goal;
566 if (! new_path.
empty()) {
568 NavGraphNode pose(
"current-pose", pose_.getOrigin().x(), pose_.getOrigin().y());
570 float new_cost = new_path.
cost();
572 if (new_cost <= old_cost * cfg_replan_factor_) {
573 constrained_plan_ =
true;
575 if (goal.
name() ==
"free-target") {
581 "old cost: %f new cost: %f (%f * %f)",
582 start.
name().c_str(), goal.
name().c_str(),
583 old_cost, new_cost * cfg_replan_factor_, new_cost, cfg_replan_factor_);
587 "more expensive plan: %f > %f (%f * %f), keeping old",
588 start.
name().c_str(), goal.
name().c_str(),
589 new_cost, old_cost * cfg_replan_factor_, old_cost, cfg_replan_factor_);
594 start.
name().c_str(), goal.
name().c_str());
611 NavGraphThread::optimize_plan()
616 double sqr_dist_a = ( pow(pose_.getOrigin().x() - path.
nodes()[0].x(), 2) +
617 pow(pose_.getOrigin().y() - path.
nodes()[0].y(), 2) );
618 double sqr_dist_b = ( pow(path.
nodes()[0].x() - path.
nodes()[1].x(), 2) +
619 pow(path.
nodes()[0].y() - path.
nodes()[1].y(), 2) );
620 double sqr_dist_c = ( pow(pose_.getOrigin().x() - path.
nodes()[1].x(), 2) +
621 pow(pose_.getOrigin().y() - path.
nodes()[1].y(), 2) );
623 if (sqr_dist_a + sqr_dist_b >= sqr_dist_c){
631 NavGraphThread::start_plan()
633 if (! cfg_enable_path_execution_)
return;
635 path_planned_at_->
stamp();
637 target_reached_ =
false;
638 target_ori_reached_ =
false;
639 target_rotating_ =
false;
641 exec_active_ =
false;
643 pp_nav_if_->
set_error_code(NavigatorInterface::ERROR_UNKNOWN_PLACE);
646 #ifdef HAVE_VISUALIZATION 649 visualized_at_->stamp();
656 std::string m = path_.
nodes()[0].name();
657 for (
unsigned int i = 1; i < path_.
size(); ++i) {
658 m +=
" - " + path_.
nodes()[i].name();
661 #ifdef HAVE_VISUALIZATION 663 vt_->set_traversal(traversal_);
664 visualized_at_->stamp();
691 NavGraphThread::stop_motion()
693 if (! cfg_enable_path_execution_)
return;
703 exec_active_ =
false;
704 target_ori_reached_ =
false;
705 target_rotating_ =
false;
709 #ifdef HAVE_VISUALIZATION 712 visualized_at_->stamp();
720 NavGraphThread::send_next_goal()
722 if (! cfg_enable_path_execution_)
return;
724 bool stop_at_target =
false;
725 bool orient_at_target =
false;
728 throw Exception(
"Cannot send next goal if plan is empty");
734 if ( traversal_.
last() ) {
735 stop_at_target =
true;
738 orient_at_target =
true;
750 ori = atan2f( next_next_target.
y() - next_target.
y(),
751 next_next_target.
x() - next_target.
x());
758 tposeglob(tf::Transform(tf::create_quaternion_from_yaw(ori),
759 tf::Vector3(next_target.
x(), next_target.
y(), 0)),
760 Time(0,0), cfg_global_frame_);
765 "Failed to compute pose, cannot generate plan", e.
what());
769 if( target_reached_ ) {
771 tpose.setOrigin(tf::Vector3(0.f, 0.f, 0.f));
776 tpose.getOrigin().y(),
777 tf::get_yaw(tpose.getRotation()));
781 if ( orient_at_target ) {
783 fawkes::NavigatorInterface::OrientationMode::OrientAtTarget );
786 fawkes::NavigatorInterface::OrientationMode::OrientDuringTravel );
790 #ifdef HAVE_VISUALIZATION 791 if (vt_) vt_->set_current_edge(last_node_, next_target.
name());
795 throw Exception(
"No writer for navigator interface");
802 tpose.getOrigin().x(), tpose.getOrigin().y(),
803 tf::get_yaw(tpose.getRotation()), next_target.
name().c_str());
806 cmd_sent_at_->
stamp();
812 if (cfg_abort_on_error_) {
814 "next goal, exception follows");
816 exec_active_ =
false;
818 pp_nav_if_->
set_error_code(NavigatorInterface::ERROR_OBSTRUCTION);
820 #ifdef HAVE_VISUALIZATION 821 if (vt_) vt_->reset_plan();
829 "next goal, exception follows");
841 NavGraphThread::node_reached()
856 float dist = sqrt(pow(pose_.getOrigin().x() - cur_target.
x(), 2) +
857 pow( pose_.getOrigin().y() - cur_target.
y(), 2));
861 if (traversal_.
last()) {
867 if (tolerance == 0.) {
869 cur_target.
name().c_str());
873 return (dist <= tolerance);
882 NavGraphThread::node_ori_reached()
895 return node_ori_reached(cur_target);
904 NavGraphThread::node_ori_reached(
const NavGraphNode &node)
913 return (ori_diff <= ori_tolerance);
922 NavGraphThread::shortcut_possible()
924 if (!traversal_ || traversal_.
remaining() < 1) {
932 float dist = sqrt(pow(pose_.getOrigin().x() - node.
x(), 2) +
933 pow(pose_.getOrigin().y() - node.
y(), 2));
937 if (tolerance == 0.0)
return 0;
938 if (dist <= tolerance)
return i;
958 **graph_ = **new_graph;
965 #ifdef HAVE_VISUALIZATION 967 vt_->set_graph(graph_);
968 visualized_at_->stamp();
979 if (goal.
name() ==
"free-target") {
982 gen_ok = generate_plan(goal.
name());
997 NavGraphThread::log_graph()
999 const std::vector<NavGraphNode> & nodes = graph_->
nodes();
1000 std::vector<NavGraphNode>::const_iterator n;
1001 for (n = nodes.begin(); n != nodes.end(); ++n) {
1003 n->name().c_str(), n->x(), n->y(),
1004 n->unconnected() ?
" UNCONNECTED" :
"");
1006 const std::map<std::string, std::string> &props = n->properties();
1007 std::map<std::string, std::string>::const_iterator p;
1008 for (p = props.begin(); p != props.end(); ++p) {
1013 std::vector<NavGraphEdge> edges = graph_->
edges();
1014 std::vector<NavGraphEdge>::iterator e;
1015 for (e = edges.begin(); e != edges.end(); ++e) {
1017 e->from().c_str(), e->is_directed() ?
">" :
"-", e->to().c_str());
1019 const std::map<std::string, std::string> &props = e->properties();
1020 std::map<std::string, std::string>::const_iterator p;
1021 for (p = props.begin(); p != props.end(); ++p) {
1028 NavGraphThread::publish_path()
1030 if (! cfg_enable_path_execution_)
return;
1032 std::vector<std::string> vpath(40,
"");
1037 for (; r < traversal_.
path().
size(); ++r) {
1038 vpath[ind++] = traversal_.
path().
nodes()[r].name();
Thread(const char *name)
Constructor.
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.
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.
AspectProviderAspect(AspectIniFin *inifin)
Constructor.
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.
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 set_property(const std::string &property, const std::string &value)
Set property.
void remove_change_listener(ChangeListener *listener)
Remove a change listener.
float orientation() const
Get orientation value.
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.
NavGraph * load_yaml_navgraph(std::string filename)
Load topological map graph stored in RCSoft format.
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.
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.
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.
BlockedTimingAspect(WakeupHook wakeup_hook)
Constructor.
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.
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.