22 #include "navigator_thread.h" 33 :
Thread(
"RosNavigatorThread",
Thread::OPMODE_WAITFORWAKEUP),
45 e.
append(
"%s initialization failed, could not open navigator " 46 "interface for writing",
name());
52 ac_ =
new MoveBaseClient(
"move_base",
false);
56 connected_history_ =
false;
73 RosNavigatorThread::check_status()
76 if (ac_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) {
80 else if (ac_->getState() == actionlib::SimpleClientGoalState::ABORTED ||
81 ac_->getState() == actionlib::SimpleClientGoalState::REJECTED)
95 RosNavigatorThread::send_goal()
98 goal_.target_pose.header.frame_id =
"base_link";
99 goal_.target_pose.header.stamp = ros::Time::now();
100 goal_.target_pose.pose.position.x = nav_if_->
dest_x();
101 goal_.target_pose.pose.position.y = nav_if_->
dest_y();
102 fawkes::tf::Quaternion q(nav_if_->
dest_ori(), 0, 0);
103 goal_.target_pose.pose.orientation.x = q.x();
104 goal_.target_pose.pose.orientation.y = q.y();
105 goal_.target_pose.pose.orientation.z = q.z();
106 goal_.target_pose.pose.orientation.w = q.w();
108 ac_->sendGoal(goal_);
114 RosNavigatorThread::stop_goals()
117 ac_->cancelAllGoals();
123 if (! ac_->isServerConnected()) {
126 "not reachable, ignoring");
130 if (connected_history_){
132 ac_ =
new MoveBaseClient(
"move_base",
false);
133 connected_history_ =
false;
138 connected_history_ =
true;
173 msg->phi(), msg->dist());
174 nav_if_->
set_dest_x(msg->dist() * cos(msg->phi()));
175 nav_if_->
set_dest_y(msg->dist() * cos(msg->phi()));
virtual void finalize()
Finalize the thread.
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.
RosNavigatorThread()
Contructor.
SetSecurityDistanceMessage Fawkes BlackBoard Interface Message.
PolarGotoMessage Fawkes BlackBoard Interface Message.
Thread class encapsulation of pthreads.
void write()
Write from local copy into BlackBoard memory.
float dest_ori() const
Get dest_ori value.
Logger * logger
This is the Logger member used to access the logger.
void set_security_distance(const float new_security_distance)
Set security_distance value.
Thread aspect to use blocked timing.
void msgq_pop()
Erase first message from queue.
float dest_y() const
Get dest_y value.
virtual void init()
Initialize the thread.
Base class for exceptions in Fawkes.
CartesianGotoMessage Fawkes BlackBoard Interface Message.
virtual void loop()
Code to execute in the thread.
const char * name() const
Get name of thread.
void set_max_velocity(const float new_max_velocity)
Set max_velocity value.
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.
void set_dest_ori(const float new_dest_ori)
Set dest_ori value.
void set_dest_x(const float new_dest_x)
Set dest_x value.
MessageType * msgq_first_safe(MessageType *&msg)
Get first message casted to the desired type without exceptions.
float dest_x() const
Get dest_x value.
void msgq_flush()
Flush all messages.
void set_dest_y(const float new_dest_y)
Set dest_y value.
void set_msgid(const uint32_t new_msgid)
Set msgid value.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
SetMaxVelocityMessage Fawkes BlackBoard Interface Message.
void append(const char *format,...)
Append messages to the message list.
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.