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;
105 throw Exception(
"Navgraph tolerances may no longer be set in the config");
113 if (! cfg_graph_file_.empty()) {
114 if (cfg_graph_file_[0] !=
'/') {
115 cfg_graph_file_ = std::string(CONFDIR) +
"/" + cfg_graph_file_;
117 graph_ = load_graph(cfg_graph_file_);
123 throw Exception(
"Graph must specify travel tolerance");
126 throw Exception(
"Graph must specify target tolerance");
129 throw Exception(
"Graph must specify orientation tolerance");
132 throw Exception(
"Graph must specify shortcut tolerance");
140 logger->
log_info(
name(),
"Using target orientation time %f from graph file", cfg_target_ori_time_);
144 if (cfg_log_graph_) {
148 if (cfg_monitor_file_) {
161 exec_active_ =
false;
162 target_reached_ =
false;
163 target_ori_reached_=
false;
164 target_rotating_ =
false;
167 constrained_plan_ =
false;
172 #ifdef HAVE_VISUALIZATION 186 delete path_planned_at_;
187 delete target_reached_at_;
189 #ifdef HAVE_VISUALIZATION 190 delete visualized_at_;
204 #ifdef HAVE_VISUALIZATION 206 vt_->set_constraint_repo(constraint_repo_);
207 vt_->set_graph(graph_);
216 bool needs_write =
false;
225 exec_active_ =
false;
245 if (generate_plan(msg->
place())) {
268 if (cfg_monitor_file_) {
279 }
else if (target_reached_) {
285 exec_active_ =
false;
288 }
else if (target_ori_reached_) {
289 if ((now - target_reached_at_) >= target_time_) {
294 }
else if (!target_rotating_ && (now - target_reached_at_) >= target_time_) {
298 target_rotating_ =
true;
304 }
else if (target_rotating_ && node_ori_reached()) {
311 if (target_time_ == 0) target_time_ = cfg_target_ori_time_;
313 target_ori_reached_ =
true;
314 target_reached_at_->
stamp();
317 }
else if (node_reached()) {
321 if (traversal_.
last()) {
326 if (target_time_ == 0) target_time_ = cfg_target_time_;
328 target_reached_ =
true;
329 target_reached_at_->
stamp();
331 }
else if (traversal_.
next()) {
344 }
else if ((shortcut_to = shortcut_possible()) > 0) {
347 traversal_.
path().
nodes()[shortcut_to].name().c_str());
363 bool new_plan =
false;
365 if (traversal_.
remaining() > 2 && (now - path_planned_at_) > cfg_replan_interval_)
367 *path_planned_at_ = now;
368 constraint_repo_.
lock();
369 if (constraint_repo_->
compute() || constraint_repo_->
modified(
true)) {
378 constraint_repo_.
unlock();
381 if (! new_plan && (now - cmd_sent_at_) > cfg_resend_interval_) {
393 #ifdef HAVE_VISUALIZATION 396 if (now - visualized_at_ >= cfg_visual_interval_) {
397 *visualized_at_ = now;
398 constraint_repo_.
lock();
399 if (constraint_repo_->
compute() || constraint_repo_->
modified(
false)) {
402 constraint_repo_.
unlock();
413 NavGraphThread::load_graph(std::string filename)
415 std::ifstream inf(filename);
416 std::string firstword;
420 if (firstword ==
"%YAML") {
429 NavGraphThread::generate_plan(std::string goal_name)
433 "Failed to compute pose, cannot generate plan");
435 pp_nav_if_->
set_error_code(NavigatorInterface::ERROR_PATH_GEN_FAIL);
440 graph_->
closest_node(pose_.getOrigin().x(), pose_.getOrigin().y());
444 logger->
log_error(
name(),
"Failed to generate path from (%.2f,%.2f) to %s: goal is unknown",
445 init.
x(), init.
y(), goal_name.c_str());
447 pp_nav_if_->
set_error_code(NavigatorInterface::ERROR_UNKNOWN_PLACE);
452 pose_.getOrigin().x(), pose_.getOrigin().y(), init.
name().c_str());
460 pp_nav_if_->
set_error_code(NavigatorInterface::ERROR_PATH_GEN_FAIL);
464 if (! path_.
empty()) {
465 constrained_plan_ =
true;
467 constrained_plan_ =
false;
473 pp_nav_if_->
set_error_code(NavigatorInterface::ERROR_PATH_GEN_FAIL);
488 NavGraphThread::generate_plan(std::string goal_name,
float ori)
490 if (generate_plan(goal_name)) {
492 if (! path_.
empty() && std::isfinite(ori)) {
493 path_.
nodes_mutable().back().set_property(
"orientation", ori);
500 pp_nav_if_->
set_error_code(NavigatorInterface::ERROR_PATH_GEN_FAIL);
506 NavGraphThread::generate_plan(
float x,
float y,
float ori)
509 if (generate_plan(close_to_goal.
name())) {
512 if (std::isfinite(ori)) {
522 pp_nav_if_->
set_error_code(NavigatorInterface::ERROR_PATH_GEN_FAIL);
536 if (goal.
name() ==
"free-target") {
538 act_goal = close_to_goal;
545 if (! new_path.
empty()) {
547 NavGraphNode pose(
"current-pose", pose_.getOrigin().x(), pose_.getOrigin().y());
549 float new_cost = new_path.
cost();
551 if (new_cost <= old_cost * cfg_replan_factor_) {
552 constrained_plan_ =
true;
554 if (goal.
name() ==
"free-target") {
560 "old cost: %f new cost: %f (%f * %f)",
561 start.
name().c_str(), goal.
name().c_str(),
562 old_cost, new_cost * cfg_replan_factor_, new_cost, cfg_replan_factor_);
566 "more expensive plan: %f > %f (%f * %f), keeping old",
567 start.
name().c_str(), goal.
name().c_str(),
568 new_cost, old_cost * cfg_replan_factor_, old_cost, cfg_replan_factor_);
573 start.
name().c_str(), goal.
name().c_str());
590 NavGraphThread::optimize_plan()
595 double sqr_dist_a = ( pow(pose_.getOrigin().x() - path.
nodes()[0].x(), 2) +
596 pow(pose_.getOrigin().y() - path.
nodes()[0].y(), 2) );
597 double sqr_dist_b = ( pow(path.
nodes()[0].x() - path.
nodes()[1].x(), 2) +
598 pow(path.
nodes()[0].y() - path.
nodes()[1].y(), 2) );
599 double sqr_dist_c = ( pow(pose_.getOrigin().x() - path.
nodes()[1].x(), 2) +
600 pow(pose_.getOrigin().y() - path.
nodes()[1].y(), 2) );
602 if (sqr_dist_a + sqr_dist_b >= sqr_dist_c){
610 NavGraphThread::start_plan()
612 path_planned_at_->
stamp();
614 target_reached_ =
false;
615 target_ori_reached_ =
false;
616 target_rotating_ =
false;
618 exec_active_ =
false;
620 pp_nav_if_->
set_error_code(NavigatorInterface::ERROR_UNKNOWN_PLACE);
623 #ifdef HAVE_VISUALIZATION 626 visualized_at_->stamp();
633 std::string m = path_.
nodes()[0].name();
634 for (
unsigned int i = 1; i < path_.
size(); ++i) {
635 m +=
" - " + path_.
nodes()[i].name();
638 #ifdef HAVE_VISUALIZATION 640 vt_->set_traversal(traversal_);
641 visualized_at_->stamp();
668 NavGraphThread::stop_motion()
678 exec_active_ =
false;
679 target_ori_reached_ =
false;
680 target_rotating_ =
false;
684 #ifdef HAVE_VISUALIZATION 687 visualized_at_->stamp();
695 NavGraphThread::send_next_goal()
697 bool stop_at_target =
false;
698 bool orient_at_target =
false;
701 throw Exception(
"Cannot send next goal if plan is empty");
707 if ( traversal_.
last() ) {
708 stop_at_target =
true;
711 orient_at_target =
true;
723 ori = atan2f( next_next_target.
y() - next_target.
y(),
724 next_next_target.
x() - next_target.
x());
731 tposeglob(tf::Transform(tf::create_quaternion_from_yaw(ori),
732 tf::Vector3(next_target.
x(), next_target.
y(), 0)),
733 Time(0,0), cfg_global_frame_);
738 "Failed to compute pose, cannot generate plan", e.
what());
742 if( target_reached_ ) {
744 tpose.setOrigin(tf::Vector3(0.f, 0.f, 0.f));
749 tpose.getOrigin().y(),
750 tf::get_yaw(tpose.getRotation()));
754 if ( orient_at_target ) {
756 fawkes::NavigatorInterface::OrientationMode::OrientAtTarget );
759 fawkes::NavigatorInterface::OrientationMode::OrientDuringTravel );
763 #ifdef HAVE_VISUALIZATION 764 if (vt_) vt_->set_current_edge(last_node_, next_target.
name());
768 throw Exception(
"No writer for navigator interface");
775 tpose.getOrigin().x(), tpose.getOrigin().y(),
776 tf::get_yaw(tpose.getRotation()), next_target.
name().c_str());
779 cmd_sent_at_->
stamp();
785 if (cfg_abort_on_error_) {
787 "next goal, exception follows");
789 exec_active_ =
false;
791 pp_nav_if_->
set_error_code(NavigatorInterface::ERROR_OBSTRUCTION);
793 #ifdef HAVE_VISUALIZATION 794 if (vt_) vt_->reset_plan();
802 "next goal, exception follows");
814 NavGraphThread::node_reached()
829 float dist = sqrt(pow(pose_.getOrigin().x() - cur_target.
x(), 2) +
830 pow( pose_.getOrigin().y() - cur_target.
y(), 2));
834 if (traversal_.
last()) {
840 if (tolerance == 0.) {
842 cur_target.
name().c_str());
846 return (dist <= tolerance);
855 NavGraphThread::node_ori_reached()
868 return node_ori_reached(cur_target);
877 NavGraphThread::node_ori_reached(
const NavGraphNode &node)
886 return (ori_diff <= ori_tolerance);
895 NavGraphThread::shortcut_possible()
897 if (!traversal_ || traversal_.
remaining() < 1) {
905 float dist = sqrt(pow(pose_.getOrigin().x() - node.
x(), 2) +
906 pow(pose_.getOrigin().y() - node.
y(), 2));
910 if (tolerance == 0.0)
return 0;
911 if (dist <= tolerance)
return i;
931 **graph_ = **new_graph;
938 #ifdef HAVE_VISUALIZATION 940 vt_->set_graph(graph_);
941 visualized_at_->stamp();
952 if (goal.
name() ==
"free-target") {
955 gen_ok = generate_plan(goal.
name());
970 NavGraphThread::log_graph()
972 const std::vector<NavGraphNode> & nodes = graph_->
nodes();
973 std::vector<NavGraphNode>::const_iterator n;
974 for (n = nodes.begin(); n != nodes.end(); ++n) {
976 n->name().c_str(), n->x(), n->y(),
977 n->unconnected() ?
" UNCONNECTED" :
"");
979 const std::map<std::string, std::string> &props = n->properties();
980 std::map<std::string, std::string>::const_iterator p;
981 for (p = props.begin(); p != props.end(); ++p) {
986 std::vector<NavGraphEdge> edges = graph_->
edges();
987 std::vector<NavGraphEdge>::iterator e;
988 for (e = edges.begin(); e != edges.end(); ++e) {
990 e->from().c_str(), e->is_directed() ?
">" :
"-", e->to().c_str());
992 const std::map<std::string, std::string> &props = e->properties();
993 std::map<std::string, std::string>::const_iterator p;
994 for (p = props.begin(); p != props.end(); ++p) {
1001 NavGraphThread::publish_path()
1003 std::vector<std::string> vpath(40,
"");
1008 for (; r < traversal_.
path().
size(); ++r) {
1009 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.