Fawkes API  Fawkes Development Version
act_thread.cpp
1 
2 /***************************************************************************
3  * act_thread.cpp - Colli Act Thread
4  *
5  * Created: Thu Oct 17 16:58:00 2013
6  * Copyright 2013-2014 Bahram Maleki-Fard
7  * 2014 Tobias Neumann
8  *
9  ****************************************************************************/
10 
11 /* This program is free software; you can redistribute it and/or modify
12  * it under the terms of the GNU General Public License as published by
13  * the Free Software Foundation; either version 2 of the License, or
14  * (at your option) any later version.
15  *
16  * This program is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19  * GNU Library General Public License for more details.
20  *
21  * Read the full text in the LICENSE.GPL file in the doc directory.
22  */
23 
24 #include "act_thread.h"
25 
26 #include "colli_thread.h"
27 
28 #include <interfaces/NavigatorInterface.h>
29 #include <utils/math/coord.h>
30 
31 #include <string>
32 
33 using namespace fawkes;
34 using namespace std;
35 
36 /** @class ColliActThread "act_thread.h"
37  * This thread hooks onto Fawkes main loop at the ACT hook. It is
38  * resoponsible for receiving the messages of the main NavigatorInterface
39  * and sending commands to the colli.
40  */
41 
42 /** Constructor.
43  * @param colli_thread The continuous colli thread that handles the colli behavior
44  */
46 : Thread("ColliActThread", Thread::OPMODE_WAITFORWAKEUP),
47  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_ACT),
48  thread_colli_(colli_thread)
49 {
50 }
51 
52 /** Desctructor. */
54 {
55 }
56 
57 void
59 {
60  std::string cfg_prefix = "/plugins/colli/";
61  cfg_security_distance_ = config->get_float((cfg_prefix + "security_distance").c_str());
62  cfg_max_velocity_ = config->get_float((cfg_prefix + "max_velocity").c_str());
63  cfg_max_rotation_ = config->get_float((cfg_prefix + "max_rotation").c_str());
64  cfg_escaping_enabled_ = config->get_bool((cfg_prefix + "escaping_enabled").c_str());
65  cfg_stop_at_target_ = config->get_bool((cfg_prefix + "stop_at_target").c_str());
66 
67  std::string cfg_orient_mode = config->get_string((cfg_prefix + "orient_mode/default").c_str());
68  if (cfg_orient_mode == "OrientAtTarget") {
69  cfg_orient_mode_ = fawkes::NavigatorInterface::OrientationMode::OrientAtTarget;
70  } else if (cfg_orient_mode == "OrientDuringTravel") {
71  cfg_orient_mode_ = fawkes::NavigatorInterface::OrientationMode::OrientDuringTravel;
72  } else {
73  cfg_orient_mode_ = fawkes::NavigatorInterface::OrientationMode::OrientAtTarget;
74  throw fawkes::Exception("Default orient_mode is unknown");
75  }
76 
77  std::string cfg_drive_mode = config->get_string((cfg_prefix + "drive_mode/default").c_str());
78  if (cfg_drive_mode == "MovingNotAllowed") {
79  cfg_drive_mode_ = NavigatorInterface::MovingNotAllowed;
80  } else if (cfg_drive_mode == "Forward") {
81  cfg_drive_mode_ = NavigatorInterface::Forward;
82  } else if (cfg_drive_mode == "AllowBackward") {
83  cfg_drive_mode_ = NavigatorInterface::AllowBackward;
84  } else if (cfg_drive_mode == "Backward") {
85  cfg_drive_mode_ = NavigatorInterface::Backward;
86  } else if (cfg_drive_mode == "ESCAPE") {
87  cfg_drive_mode_ = NavigatorInterface::ESCAPE;
88  } else {
89  cfg_drive_mode_ = NavigatorInterface::MovingNotAllowed;
90  throw fawkes::Exception("Default drive_mode is unknown");
91  }
92 
94  "Default drive_mode: %i (%s)",
95  cfg_drive_mode_,
96  if_navi_->tostring_DriveMode(cfg_drive_mode_));
97 
98  cfg_iface_navi_ = config->get_string((cfg_prefix + "interface/navigator").c_str());
99 
100  cfg_frame_odom_ = config->get_string((cfg_prefix + "frame/odometry").c_str());
101 
102  if_navi_ = blackboard->open_for_writing<NavigatorInterface>(cfg_iface_navi_.c_str());
103 
104  if_navi_->set_max_velocity(cfg_max_velocity_);
105  if_navi_->set_max_rotation(cfg_max_rotation_);
106  if_navi_->set_escaping_enabled(cfg_escaping_enabled_);
107  if_navi_->set_security_distance(cfg_security_distance_);
108  if_navi_->set_stop_at_target(cfg_stop_at_target_);
109  if_navi_->set_orientation_mode(cfg_orient_mode_);
110  if_navi_->set_drive_mode(cfg_drive_mode_);
111  if_navi_->set_final(true);
112  if_navi_->write();
113 }
114 
115 void
117 {
118  blackboard->close(if_navi_);
119 }
120 
121 void
123 {
124  // update interfaces
125  if_navi_->set_final(colli_final());
126 
127  // process interface messages
128  Message *motion_msg = NULL;
129  while (!if_navi_->msgq_empty()) {
131  if (motion_msg)
132  motion_msg->unref();
133  motion_msg = if_navi_->msgq_first<NavigatorInterface::StopMessage>();
134  motion_msg->ref();
135 
137  if (motion_msg)
138  motion_msg->unref();
139  motion_msg = if_navi_->msgq_first<NavigatorInterface::CartesianGotoMessage>();
140  motion_msg->ref();
141 
142  } else if (if_navi_->msgq_first_is<NavigatorInterface::PolarGotoMessage>()) {
143  if (motion_msg)
144  motion_msg->unref();
145  motion_msg = if_navi_->msgq_first<NavigatorInterface::PolarGotoMessage>();
146  motion_msg->ref();
147 
151 
152  logger->log_debug(name(), "setting max velocity to %f", msg->max_velocity());
153  if_navi_->set_max_velocity(msg->max_velocity());
154 
158 
159  logger->log_debug(name(), "setting max rotation velocity to %f", msg->max_rotation());
160  if_navi_->set_max_rotation(msg->max_rotation());
161 
162  } else if (if_navi_->msgq_first_is<NavigatorInterface::SetEscapingMessage>()) {
165 
166  logger->log_debug(name(), "setting escaping allowed to %u", msg->is_escaping_enabled());
167  if_navi_->set_escaping_enabled(msg->is_escaping_enabled());
168 
172 
173  logger->log_debug(name(), "setting security distance to %f", msg->security_distance());
174  if_navi_->set_security_distance(msg->security_distance());
175 
179 
180  logger->log_debug(name(), "setting stop_at_target to %u", msg->is_stop_at_target());
181  if_navi_->set_stop_at_target(msg->is_stop_at_target());
182 
186 
187  logger->log_debug(name(),
188  "setting orient_at_target to %s",
189  if_navi_->tostring_OrientationMode(msg->orientation_mode()));
190  if_navi_->set_orientation_mode(msg->orientation_mode());
191 
192  } else if (if_navi_->msgq_first_is<NavigatorInterface::SetDriveModeMessage>()) {
195 
196  logger->log_debug(name(),
197  "setting drive_mode to %s",
198  if_navi_->tostring_DriveMode(msg->drive_mode()));
199  if_navi_->set_drive_mode(msg->drive_mode());
200 
202  logger->log_debug(name(), "resetting colli parameters to default values (from config)");
203  if_navi_->set_max_velocity(cfg_max_velocity_);
204  if_navi_->set_max_rotation(cfg_max_rotation_);
205  if_navi_->set_escaping_enabled(cfg_escaping_enabled_);
206  if_navi_->set_security_distance(cfg_security_distance_);
207  if_navi_->set_stop_at_target(cfg_stop_at_target_);
208  if_navi_->set_orientation_mode(cfg_orient_mode_);
209  if_navi_->set_drive_mode(cfg_drive_mode_);
210 
211  } else {
212  logger->log_debug(name(), "Ignoring unhandled Navigator message");
213  }
214 
215  if_navi_->msgq_pop();
216  }
217 
218  // process last motion message
219  if (motion_msg) {
220  if (motion_msg->is_of_type<NavigatorInterface::StopMessage>()) {
221  logger->log_debug(name(), "StopMessage received");
222  thread_colli_->colli_stop();
223 
224  } else if (motion_msg->is_of_type<NavigatorInterface::CartesianGotoMessage>()) {
226  dynamic_cast<NavigatorInterface::CartesianGotoMessage *>(motion_msg);
227  logger->log_debug(name(),
228  "CartesianGotoMessage received, x:%f y:%f ori:%f",
229  msg->x(),
230  msg->y(),
231  msg->orientation());
232  // Converts from Fawkes Coord Sys -> RCSoftX Coord Sys, hence X_f = X_r, Y_f = -Y_r, Ori_f = -Ori_r
233  if_navi_->set_msgid(msg->id());
234  if_navi_->set_dest_x(msg->x());
235  if_navi_->set_dest_y(msg->y());
236  if_navi_->set_dest_ori(msg->orientation());
237  if_navi_->set_dest_dist(sqrt(msg->x() * msg->x() + msg->y() * msg->y()));
238  if_navi_->set_final(false);
239 
240  thread_colli_->colli_relgoto(msg->x(), msg->y(), msg->orientation(), if_navi_);
241 
242  } else if (motion_msg->is_of_type<NavigatorInterface::PolarGotoMessage>()) {
244  dynamic_cast<NavigatorInterface::PolarGotoMessage *>(motion_msg);
245  logger->log_debug(name(),
246  "PolarGotoMessage received, phi:%f dist:%f",
247  msg->phi(),
248  msg->dist());
249  // Converts from Fawkes Coord Sys -> RCSoftX Coord Sys, hence D_f = D_r, Phi_f = - Phi_r, Ori_f = Ori_r
250 
251  float cart_x = 0, cart_y = 0;
252  polar2cart2d(msg->phi(), msg->dist(), &cart_x, &cart_y);
253 
254  if_navi_->set_msgid(msg->id());
255  if_navi_->set_dest_x(cart_x);
256  if_navi_->set_dest_y(cart_y);
257  if_navi_->set_dest_ori(msg->orientation());
258  if_navi_->set_dest_dist(msg->dist());
259  if_navi_->set_final(false);
260 
261  thread_colli_->colli_relgoto(cart_x, cart_y, msg->orientation(), if_navi_);
262  }
263 
264  motion_msg->unref();
265  }
266 
267  if_navi_->write();
268 }
269 
270 bool
271 ColliActThread::colli_final()
272 {
273  return thread_colli_->is_final();
274  // RCSoftX had more. Full code(from libmonaco and navigator_server) :
275  //
276  // return (thread_colli_->is_final() || m_pNavServer->isColliFinal() );
277  // bbClients::Navigator_Server::isColliFinal()
278  // {
279  // bool alive = (bool)m_ColliFeedback.GetValue( 8 /* magic value for alive index */ );
280  // bool final = (bool)m_ColliFeedback.GetValue( 0 /* magic value for final index */ );
281  // Timestamp cf(m_itsColliFeedback_sec.GetValue(), m_itsColliFeedback_usec.GetValue());
282  // Timestamp tp(m_itsTargetPoint_sec.GetValue(), m_itsTargetPoint_usec.GetValue());
283 
284  // return alive && final && ( (cf - tp) == 0);
285  // }
286 }
virtual void finalize()
Finalize the thread.
Definition: act_thread.cpp:116
ColliActThread(ColliThread *colli_thread)
Constructor.
Definition: act_thread.cpp:45
virtual void init()
Initialize the thread.
Definition: act_thread.cpp:58
virtual ~ColliActThread()
Desctructor.
Definition: act_thread.cpp:53
virtual void loop()
Code to execute in the thread.
Definition: act_thread.cpp:122
Thread that performs the navigation and collision avoidance algorithms.
Definition: colli_thread.h:62
void colli_relgoto(float x, float y, float ori, fawkes::NavigatorInterface *iface)
Sends a goto-command, using relative coordinates.
void colli_stop()
Sends a stop-command.
bool is_final() const
Checks if the colli is final.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual void close(Interface *interface)=0
Close interface.
Thread aspect to use blocked timing.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
Base class for exceptions in Fawkes.
Definition: exception.h:36
bool msgq_first_is()
Check if first message has desired type.
Definition: interface.h:314
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1182
Message * msgq_first()
Get the first message from the message queue.
Definition: interface.cpp:1167
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:494
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1029
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Definition: message.h:45
bool is_of_type()
Check if message has desired type.
Definition: message.h:143
unsigned int id() const
Get message ID.
Definition: message.cpp:180
CartesianGotoMessage Fawkes BlackBoard Interface Message.
float orientation() const
Get orientation value.
PolarGotoMessage Fawkes BlackBoard Interface Message.
float orientation() const
Get orientation value.
ResetParametersMessage Fawkes BlackBoard Interface Message.
SetDriveModeMessage Fawkes BlackBoard Interface Message.
DriveMode drive_mode() const
Get drive_mode value.
SetEscapingMessage Fawkes BlackBoard Interface Message.
bool is_escaping_enabled() const
Get escaping_enabled value.
SetMaxRotationMessage Fawkes BlackBoard Interface Message.
float max_rotation() const
Get max_rotation value.
SetMaxVelocityMessage Fawkes BlackBoard Interface Message.
float max_velocity() const
Get max_velocity value.
SetOrientationModeMessage Fawkes BlackBoard Interface Message.
OrientationMode orientation_mode() const
Get orientation_mode value.
SetSecurityDistanceMessage Fawkes BlackBoard Interface Message.
float security_distance() const
Get security_distance value.
SetStopAtTargetMessage Fawkes BlackBoard Interface Message.
bool is_stop_at_target() const
Get stop_at_target value.
StopMessage Fawkes BlackBoard Interface Message.
NavigatorInterface Fawkes BlackBoard Interface.
void set_max_rotation(const float new_max_rotation)
Set max_rotation value.
void set_msgid(const uint32_t new_msgid)
Set msgid value.
void set_stop_at_target(const bool new_stop_at_target)
Set stop_at_target value.
void set_max_velocity(const float new_max_velocity)
Set max_velocity value.
void set_drive_mode(const DriveMode new_drive_mode)
Set drive_mode value.
void set_security_distance(const float new_security_distance)
Set security_distance value.
void set_dest_ori(const float new_dest_ori)
Set dest_ori value.
void set_dest_dist(const float new_dest_dist)
Set dest_dist value.
void set_dest_y(const float new_dest_y)
Set dest_y value.
const char * tostring_DriveMode(DriveMode value) const
Convert DriveMode constant to string.
void set_dest_x(const float new_dest_x)
Set dest_x value.
const char * tostring_OrientationMode(OrientationMode value) const
Convert OrientationMode constant to string.
void set_final(const bool new_final)
Set final value.
void set_orientation_mode(const OrientationMode new_orientation_mode)
Set orientation_mode value.
void set_escaping_enabled(const bool new_escaping_enabled)
Set escaping_enabled value.
void unref()
Decrement reference count and conditionally delete this instance.
Definition: refcount.cpp:95
void ref()
Increment reference count.
Definition: refcount.cpp:67
Thread class encapsulation of pthreads.
Definition: thread.h:46
const char * name() const
Get name of thread.
Definition: thread.h:100
Fawkes library namespace.
void polar2cart2d(float polar_phi, float polar_dist, float *cart_x, float *cart_y)
Convert a 2D polar coordinate to a 2D cartesian coordinate.
Definition: coord.h:72