Fawkes API  Fawkes Development Version
navigator_thread.cpp
1 
2 /***************************************************************************
3  * navigator_thread.cpp - Robotino ROS Navigator Thread
4  *
5  * Created: Sat June 09 15:13:27 2012
6  * Copyright 2012 Sebastian Reuter
7  ****************************************************************************/
8 
9 /* This program is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2 of the License, or
12  * (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU Library General Public License for more details.
18  *
19  * Read the full text in the LICENSE.GPL file in the doc directory.
20  */
21 
22 #include "navigator_thread.h"
23 
24 using namespace fawkes;
25 
26 /** @class RosNavigatorThread "navigator_thread.h"
27  * Send Fawkes locomotion commands off to ROS.
28  * @author Sebastian Reuter
29  */
30 
31 /** Contructor. */
33  : Thread("RosNavigatorThread", Thread::OPMODE_WAITFORWAKEUP),
35 {
36 }
37 
38 void
40 {
41  // navigator interface to give feedback of navigation task (writer)
42  try {
43  nav_if_ = blackboard->open_for_writing<NavigatorInterface>("Navigator");
44  } catch (Exception& e) {
45  e.append("%s initialization failed, could not open navigator "
46  "interface for writing", name());
47  logger->log_error(name(), e);
48  throw;
49  }
50 
51  //tell the action client that we want to spin a thread by default
52  ac_ = new MoveBaseClient("move_base", false);
53 
54  logger->log_error(name(),"Change Interface (x,y) ");
55  cmd_sent_ = false;
56  connected_history_ = false;
57 }
58 
59 void
61 {
62  // close interfaces
63  try {
64  blackboard->close(nav_if_);
65  } catch (Exception& e) {
66  logger->log_error(name(), "Closing interface failed!");
67  logger->log_error(name(), e);
68  }
69  delete ac_;
70 }
71 
72 void
73 RosNavigatorThread::check_status()
74 {
75  if (cmd_sent_){
76  if (ac_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) {
77  nav_if_->set_final(true);
78  nav_if_->set_error_code(0);
79  }
80  else if (ac_->getState() == actionlib::SimpleClientGoalState::ABORTED ||
81  ac_->getState() == actionlib::SimpleClientGoalState::REJECTED)
82  {
83  nav_if_->set_final(true);
84  nav_if_->set_error_code(2);
85  }
86  else {
87  nav_if_->set_final(false);
88  nav_if_->set_error_code(0);
89  }
90  nav_if_->write();
91  }
92 }
93 
94 void
95 RosNavigatorThread::send_goal()
96 {
97  //get goal from Navigation interface
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();
107 
108  ac_->sendGoal(goal_);
109 
110  cmd_sent_ = true;
111 }
112 
113 void
114 RosNavigatorThread::stop_goals()
115 {
116  //cancel all existing goals and send Goal={0/0/0}
117  ac_->cancelAllGoals();
118 }
119 
120 void
122 {
123  if (! ac_->isServerConnected()) {
124  if (! nav_if_->msgq_empty()) {
125  logger->log_warn(name(), "Command received while ROS ActionClient "
126  "not reachable, ignoring");
127  nav_if_->msgq_flush();
128  }
129 
130  if (connected_history_){
131  delete ac_;
132  ac_ = new MoveBaseClient("move_base", false);
133  connected_history_ = false;
134  }
135 
136  } else {
137 
138  connected_history_ = true;
139  // process incoming messages from fawkes
140  while (! nav_if_->msgq_empty()) {
141 
142  // stop
143  if (NavigatorInterface::StopMessage *msg = nav_if_->msgq_first_safe(msg)) {
144  logger->log_info(name(), "Stop message received");
145  nav_if_->set_dest_x(0);
146  nav_if_->set_dest_y(0);
147  nav_if_->set_dest_ori(0);
148 
149  nav_if_->set_msgid(msg->id());
150  nav_if_->write();
151 
152  stop_goals();
153  }
154 
155  // cartesian goto
156  else if (NavigatorInterface::CartesianGotoMessage *msg = nav_if_->msgq_first_safe(msg)) {
157  logger->log_info(name(), "Cartesian goto message received (x,y) = (%f,%f)",
158  msg->x(), msg->y());
159  nav_if_->set_dest_x(msg->x());
160  nav_if_->set_dest_y(msg->y());
161  nav_if_->set_dest_ori(msg->orientation());
162 
163  nav_if_->set_msgid(msg->id());
164 
165  nav_if_->write();
166 
167  send_goal();
168  }
169 
170  // polar goto
171  else if (NavigatorInterface::PolarGotoMessage *msg = nav_if_->msgq_first_safe(msg)) {
172  logger->log_info(name(), "Polar goto message received (phi,dist) = (%f,%f)",
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()));
176  nav_if_->set_dest_ori(msg->phi());
177  nav_if_->set_msgid(msg->id());
178  nav_if_->write();
179 
180  send_goal();
181  }
182 
183  // max velocity
184  else if (NavigatorInterface::SetMaxVelocityMessage *msg = nav_if_->msgq_first_safe(msg)) {
185  logger->log_info(name(),"velocity message received %f",msg->max_velocity());
186  nav_if_->set_max_velocity(msg->max_velocity());
187  nav_if_->set_msgid(msg->id());
188  nav_if_->write();
189 
190  send_goal();
191  }
192 
194  nav_if_->msgq_first_safe(msg))
195  {
196  logger->log_info(name(),"velocity message received %f",msg->security_distance ());
197  nav_if_->set_security_distance (msg->security_distance ());
198  nav_if_->set_msgid(msg->id());
199  nav_if_->write();
200 
201  send_goal();
202  }
203 
204  nav_if_->msgq_pop();
205  } // while
206 
207  check_status();
208  }
209 }
virtual void finalize()
Finalize the thread.
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1048
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.
Definition: thread.h:42
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:500
float dest_ori() const
Get dest_ori value.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:44
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.
Definition: interface.cpp:1193
float dest_y() const
Get dest_y value.
virtual void init()
Initialize the thread.
Base class for exceptions in Fawkes.
Definition: exception.h:36
CartesianGotoMessage Fawkes BlackBoard Interface Message.
virtual void loop()
Code to execute in the thread.
const char * name() const
Get name of thread.
Definition: thread.h:95
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.
Definition: interface.h:302
float dest_x() const
Get dest_x value.
void msgq_flush()
Flush all messages.
Definition: interface.cpp:1064
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.
Definition: exception.cpp:341
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
StopMessage Fawkes BlackBoard Interface Message.
NavigatorInterface Fawkes BlackBoard Interface.
virtual void close(Interface *interface)=0
Close interface.