22 #include "navgraph_breakout_thread.h" 24 #include <interfaces/NavigatorInterface.h> 35 :
Thread(
"RosNavgraphBreakoutThread",
Thread::OPMODE_WAITFORWAKEUP),
44 was_connected_ =
false;
49 e.
append(
"%s initialization failed, could not open navigator " 50 "interface for writing",
name());
55 cfg_action_topic_ =
config->
get_string(
"/ros/navgraph-breakout/action-topic");
57 ac_ =
new NavGraphGotoClient(cfg_action_topic_,
false);
76 if (! ac_->isServerConnected()) {
79 "not available, ignoring");
85 ac_ =
new NavGraphGotoClient(cfg_action_topic_,
false);
86 was_connected_ =
false;
90 was_connected_ =
true;
96 ac_->cancelAllGoals();
105 goal_.place = msg->place();
106 goal_.orientation = std::numeric_limits<float>::quiet_NaN();
108 ac_->cancelAllGoals();
110 ac_->sendGoal(goal_);
123 if (ac_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED){
124 fawkes_msgs::NavGraphGotoResult result = *( ac_->getResult() );
125 if (result.errcode != fawkes_msgs::NavGraphGotoResult::ERROR_NONE) {
126 if (result.errmsg.empty()) {
128 "Remote navgraph goto failed without error message (code %u)",
132 "Remote navgraph goto failed: %s", result.errmsg.c_str());
138 goal_active_ =
false;
139 }
else if (ac_->getState() == actionlib::SimpleClientGoalState::ABORTED ||
140 ac_->getState() == actionlib::SimpleClientGoalState::REJECTED)
143 pp_nav_if_->
set_error_code(NavigatorInterface::ERROR_OBSTRUCTION);
145 goal_active_ =
false;
RosNavgraphBreakoutThread()
Contructor.
bool msgq_empty()
Check if queue is empty.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
Fawkes library namespace.
void set_final(const bool new_final)
Set final value.
Thread class encapsulation of pthreads.
void write()
Write from local copy into BlackBoard memory.
Logger * logger
This is the Logger member used to access the logger.
virtual void loop()
Code to execute in the thread.
Thread aspect to use blocked timing.
void msgq_pop()
Erase first message from queue.
PlaceGotoMessage Fawkes BlackBoard Interface Message.
Base class for exceptions in Fawkes.
virtual void init()
Initialize the thread.
const char * name() const
Get name of thread.
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.
void set_error_code(const uint32_t new_error_code)
Set error_code value.
MessageType * msgq_first_safe(MessageType *&msg)
Get first message casted to the desired type without exceptions.
void msgq_flush()
Flush all messages.
void set_msgid(const uint32_t new_msgid)
Set msgid value.
Configuration * config
This is the Configuration member used to access the configuration.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
void append(const char *format,...)
Append messages to the message list.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
virtual void finalize()
Finalize the thread.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
StopMessage Fawkes BlackBoard Interface Message.
NavigatorInterface Fawkes BlackBoard Interface.
virtual void close(Interface *interface)=0
Close interface.