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  nav_if_->set_final(true);
83  nav_if_->set_error_code(2);
84  }
85  else {
86  nav_if_->set_final(false);
87  nav_if_->set_error_code(0);
88  }
89  nav_if_->write();
90  }
91 }
92 
93 void
94 RosNavigatorThread::send_goal()
95 {
96  //get goal from Navigation interface
97  goal_.target_pose.header.frame_id = "base_link";
98  goal_.target_pose.header.stamp = ros::Time::now();
99  goal_.target_pose.pose.position.x = nav_if_->dest_x();
100  goal_.target_pose.pose.position.y = nav_if_->dest_y();
101  fawkes::tf::Quaternion q(nav_if_->dest_ori(), 0, 0);
102  goal_.target_pose.pose.orientation.x = q.x();
103  goal_.target_pose.pose.orientation.y = q.y();
104  goal_.target_pose.pose.orientation.z = q.z();
105  goal_.target_pose.pose.orientation.w = q.w();
106 
107  ac_->sendGoal(goal_);
108 
109  cmd_sent_ = true;
110 }
111 
112 void
113 RosNavigatorThread::stop_goals()
114 {
115  //cancel all existing goals and send Goal={0/0/0}
116  ac_->cancelAllGoals();
117 }
118 
119 void
121 {
122  if (! ac_->isServerConnected()) {
123  if (! nav_if_->msgq_empty()) {
124  logger->log_warn(name(), "Command received while ROS ActionClient "
125  "not reachable, ignoring");
126  nav_if_->msgq_flush();
127  }
128 
129  if (connected_history_){
130  delete ac_;
131  ac_ = new MoveBaseClient("move_base", false);
132  connected_history_ = false;
133  }
134 
135  } else {
136 
137  connected_history_ = true;
138  // process incoming messages from fawkes
139  while (! nav_if_->msgq_empty()) {
140 
141  // stop
142  if (NavigatorInterface::StopMessage *msg = nav_if_->msgq_first_safe(msg)) {
143  logger->log_info(name(), "Stop message received");
144  nav_if_->set_dest_x(0);
145  nav_if_->set_dest_y(0);
146  nav_if_->set_dest_ori(0);
147 
148  nav_if_->set_msgid(msg->id());
149  nav_if_->msgq_pop();
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_->msgq_pop();
166  nav_if_->write();
167 
168  send_goal();
169  }
170 
171  // polar goto
172  else if (NavigatorInterface::PolarGotoMessage *msg = nav_if_->msgq_first_safe(msg)) {
173  logger->log_info(name(), "Polar goto message received (phi,dist) = (%f,%f)",
174  msg->phi(), msg->dist());
175  nav_if_->set_dest_x(msg->dist() * cos(msg->phi()));
176  nav_if_->set_dest_y(msg->dist() * cos(msg->phi()));
177  nav_if_->set_dest_ori(msg->phi());
178 
179  nav_if_->set_msgid(msg->id());
180 
181  nav_if_->msgq_pop();
182  nav_if_->write();
183 
184  send_goal();
185  }
186 
187  // max velocity
188  else if (NavigatorInterface::SetMaxVelocityMessage *msg = nav_if_->msgq_first_safe(msg)) {
189  logger->log_info(name(),"velocity message received %f",msg->max_velocity());
190  nav_if_->set_max_velocity(msg->max_velocity());
191 
192  nav_if_->set_msgid(msg->id());
193 
194  nav_if_->msgq_pop();
195  nav_if_->write();
196 
197  send_goal();
198  }
199 
201  nav_if_->msgq_first_safe(msg))
202  {
203  logger->log_info(name(),"velocity message received %f",msg->security_distance ());
204  nav_if_->set_security_distance (msg->security_distance ());
205 
206  nav_if_->set_msgid(msg->id());
207 
208  nav_if_->msgq_pop();
209  nav_if_->write();
210 
211  send_goal();
212  }
213 
214  } // while
215 
216  check_status();
217 
218  }
219 }
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.