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 }
fawkes::RefCount::unref
void unref()
Decrement reference count and conditionally delete this instance.
Definition: refcount.cpp:101
fawkes::NavigatorInterface::set_security_distance
void set_security_distance(const float new_security_distance)
Set security_distance value.
Definition: NavigatorInterface.cpp:584
ColliThread::colli_stop
void colli_stop()
Sends a stop-command.
Definition: colli_thread.cpp:270
fawkes::Interface::msgq_first_is
bool msgq_first_is()
Check if first message has desired type.
Definition: interface.h:314
fawkes::NavigatorInterface::SetOrientationModeMessage
Definition: NavigatorInterface.h:666
fawkes::Interface::msgq_pop
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1184
fawkes::Interface::msgq_empty
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1031
fawkes::NavigatorInterface::SetDriveModeMessage::drive_mode
DriveMode drive_mode() const
Get drive_mode value.
Definition: NavigatorInterface.cpp:3145
fawkes::NavigatorInterface::StopMessage
Definition: NavigatorInterface.h:122
fawkes::NavigatorInterface::set_dest_dist
void set_dest_dist(const float new_dest_dist)
Set dest_dist value.
Definition: NavigatorInterface.cpp:390
fawkes::NavigatorInterface::set_dest_ori
void set_dest_ori(const float new_dest_ori)
Set dest_ori value.
Definition: NavigatorInterface.cpp:359
fawkes::NavigatorInterface::CartesianGotoMessage::x
float x() const
Get x value.
Definition: NavigatorInterface.cpp:1196
fawkes::NavigatorInterface::SetOrientationModeMessage::orientation_mode
OrientationMode orientation_mode() const
Get orientation_mode value.
Definition: NavigatorInterface.cpp:3353
fawkes::Message
Definition: message.h:41
fawkes::NavigatorInterface::SetSecurityDistanceMessage::security_distance
float security_distance() const
Get security_distance value.
Definition: NavigatorInterface.cpp:3041
fawkes::NavigatorInterface::set_drive_mode
void set_drive_mode(const DriveMode new_drive_mode)
Set drive_mode value.
Definition: NavigatorInterface.cpp:648
fawkes::NavigatorInterface::set_dest_x
void set_dest_x(const float new_dest_x)
Set dest_x value.
Definition: NavigatorInterface.cpp:297
fawkes::NavigatorInterface::SetStopAtTargetMessage
Definition: NavigatorInterface.h:639
fawkes::NavigatorInterface::SetDriveModeMessage
Definition: NavigatorInterface.h:612
fawkes::NavigatorInterface::SetEscapingMessage::is_escaping_enabled
bool is_escaping_enabled() const
Get escaping_enabled value.
Definition: NavigatorInterface.cpp:2936
fawkes::Configuration::get_bool
virtual bool get_bool(const char *path)=0
fawkes::NavigatorInterface::ResetParametersMessage
Definition: NavigatorInterface.h:693
fawkes::NavigatorInterface::tostring_OrientationMode
const char * tostring_OrientationMode(OrientationMode value) const
Convert OrientationMode constant to string.
Definition: NavigatorInterface.cpp:168
fawkes::NavigatorInterface::PolarGotoMessage::orientation
float orientation() const
Get orientation value.
Definition: NavigatorInterface.cpp:2152
fawkes::NavigatorInterface::PolarGotoMessage::phi
float phi() const
Get phi value.
Definition: NavigatorInterface.cpp:2092
ColliThread::colli_relgoto
void colli_relgoto(float x, float y, float ori, fawkes::NavigatorInterface *iface)
Sends a goto-command, using relative coordinates.
Definition: colli_thread.cpp:249
fawkes::NavigatorInterface::set_max_rotation
void set_max_rotation(const float new_max_rotation)
Set max_rotation value.
Definition: NavigatorInterface.cpp:553
fawkes::NavigatorInterface::set_max_velocity
void set_max_velocity(const float new_max_velocity)
Set max_velocity value.
Definition: NavigatorInterface.cpp:522
fawkes::polar2cart2d
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:78
fawkes::BlockedTimingAspect
Definition: blocked_timing.h:56
ColliThread
Definition: colli_thread.h:56
fawkes::Thread::name
const char * name() const
Definition: thread.h:100
fawkes::NavigatorInterface::SetEscapingMessage
Definition: NavigatorInterface.h:557
fawkes::NavigatorInterface::SetMaxVelocityMessage
Definition: NavigatorInterface.h:503
fawkes::NavigatorInterface::PolarGotoMessage
Definition: NavigatorInterface.h:353
fawkes::NavigatorInterface::set_dest_y
void set_dest_y(const float new_dest_y)
Set dest_y value.
Definition: NavigatorInterface.cpp:328
fawkes::NavigatorInterface::SetMaxVelocityMessage::max_velocity
float max_velocity() const
Get max_velocity value.
Definition: NavigatorInterface.cpp:2727
fawkes::NavigatorInterface::set_escaping_enabled
void set_escaping_enabled(const bool new_escaping_enabled)
Set escaping_enabled value.
Definition: NavigatorInterface.cpp:617
fawkes::NavigatorInterface::set_msgid
void set_msgid(const uint32_t new_msgid)
Set msgid value.
Definition: NavigatorInterface.cpp:423
fawkes::NavigatorInterface::set_orientation_mode
void set_orientation_mode(const OrientationMode new_orientation_mode)
Set orientation_mode value.
Definition: NavigatorInterface.cpp:745
fawkes::NavigatorInterface::CartesianGotoMessage::y
float y() const
Get y value.
Definition: NavigatorInterface.cpp:1226
fawkes::LoggingAspect::logger
Logger * logger
Definition: logging.h:53
fawkes::RefCount::ref
void ref()
Increment reference count.
Definition: refcount.cpp:73
fawkes::BlackBoard::close
virtual void close(Interface *interface)=0
fawkes::NavigatorInterface::SetMaxRotationMessage::max_rotation
float max_rotation() const
Get max_rotation value.
Definition: NavigatorInterface.cpp:2831
fawkes::NavigatorInterface::set_stop_at_target
void set_stop_at_target(const bool new_stop_at_target)
Set stop_at_target value.
Definition: NavigatorInterface.cpp:714
fawkes::NavigatorInterface::set_final
void set_final(const bool new_final)
Set final value.
Definition: NavigatorInterface.cpp:456
fawkes
fawkes::NavigatorInterface::CartesianGotoMessage
Definition: NavigatorInterface.h:189
fawkes::NavigatorInterface::SetMaxRotationMessage
Definition: NavigatorInterface.h:530
fawkes::ConfigurableAspect::config
Configuration * config
Definition: configurable.h:53
fawkes::NavigatorInterface::SetSecurityDistanceMessage
Definition: NavigatorInterface.h:585
fawkes::NavigatorInterface::tostring_DriveMode
const char * tostring_DriveMode(DriveMode value) const
Convert DriveMode constant to string.
Definition: NavigatorInterface.cpp:152
ColliActThread::loop
virtual void loop()
Code to execute in the thread.
Definition: act_thread.cpp:122
fawkes::NavigatorInterface::CartesianGotoMessage::orientation
float orientation() const
Get orientation value.
Definition: NavigatorInterface.cpp:1256
ColliThread::is_final
bool is_final() const
Checks if the colli is final.
Definition: colli_thread.cpp:222
fawkes::NavigatorInterface::SetStopAtTargetMessage::is_stop_at_target
bool is_stop_at_target() const
Get stop_at_target value.
Definition: NavigatorInterface.cpp:3249
fawkes::Configuration::get_float
virtual float get_float(const char *path)=0
fawkes::Thread
Definition: thread.h:45
fawkes::BlackBoardAspect::blackboard
BlackBoard * blackboard
Definition: blackboard.h:49
fawkes::Configuration::get_string
virtual std::string get_string(const char *path)=0
fawkes::Message::id
unsigned int id() const
Get message ID.
Definition: message.cpp:186
fawkes::Message::is_of_type
bool is_of_type()
Check if message has desired type.
Definition: message.h:140
fawkes::Interface::msgq_first
Message * msgq_first()
Get the first message from the message queue.
Definition: interface.cpp:1169
ColliActThread::finalize
virtual void finalize()
Finalize the thread.
Definition: act_thread.cpp:116
fawkes::Logger::log_debug
virtual void log_debug(const char *component, const char *format,...)=0
fawkes::Interface::write
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:499
ColliActThread::~ColliActThread
virtual ~ColliActThread()
Desctructor.
Definition: act_thread.cpp:53
fawkes::NavigatorInterface
Definition: NavigatorInterface.h:39
fawkes::BlackBoard::open_for_writing
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
ColliActThread::ColliActThread
ColliActThread(ColliThread *colli_thread)
Constructor.
Definition: act_thread.cpp:45
ColliActThread::init
virtual void init()
Initialize the thread.
Definition: act_thread.cpp:58
fawkes::NavigatorInterface::PolarGotoMessage::dist
float dist() const
Get dist value.
Definition: NavigatorInterface.cpp:2122
fawkes::Exception
Definition: exception.h:41