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 #include "colli_thread.h"
26 
27 #include <interfaces/NavigatorInterface.h>
28 
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 
93  logger->log_debug(name(), "Default drive_mode: %i (%s)", cfg_drive_mode_, if_navi_->tostring_DriveMode(cfg_drive_mode_));
94 
95  cfg_iface_navi_ = config->get_string((cfg_prefix + "interface/navigator").c_str());
96 
97  cfg_frame_odom_ = config->get_string((cfg_prefix + "frame/odometry").c_str());
98 
99  if_navi_ = blackboard->open_for_writing<NavigatorInterface>(cfg_iface_navi_.c_str());
100 
101  if_navi_->set_max_velocity(cfg_max_velocity_);
102  if_navi_->set_max_rotation(cfg_max_rotation_);
103  if_navi_->set_escaping_enabled(cfg_escaping_enabled_);
104  if_navi_->set_security_distance(cfg_security_distance_);
105  if_navi_->set_stop_at_target(cfg_stop_at_target_);
106  if_navi_->set_orientation_mode(cfg_orient_mode_);
107  if_navi_->set_drive_mode(cfg_drive_mode_);
108  if_navi_->set_final(true);
109  if_navi_->write();
110 }
111 
112 void
114 {
115  blackboard->close( if_navi_ );
116 }
117 
118 void
120 {
121  // update interfaces
122  if_navi_->set_final(colli_final());
123 
124  // process interface messages
125  Message* motion_msg = NULL;
126  while( !if_navi_->msgq_empty() ) {
128  if( motion_msg )
129  motion_msg->unref();
130  motion_msg = if_navi_->msgq_first<NavigatorInterface::StopMessage>();
131  motion_msg->ref();
132 
134  if( motion_msg )
135  motion_msg->unref();
136  motion_msg = if_navi_->msgq_first<NavigatorInterface::CartesianGotoMessage>();
137  motion_msg->ref();
138 
139  } else if ( if_navi_->msgq_first_is<NavigatorInterface::PolarGotoMessage>() ) {
140  if( motion_msg )
141  motion_msg->unref();
142  motion_msg = if_navi_->msgq_first<NavigatorInterface::PolarGotoMessage>();
143  motion_msg->ref();
144 
145  } else if ( if_navi_->msgq_first_is<NavigatorInterface::SetMaxVelocityMessage>() ) {
147 
148  logger->log_debug(name(), "setting max velocity to %f", msg->max_velocity());
149  if_navi_->set_max_velocity(msg->max_velocity());
150 
151  } else if ( if_navi_->msgq_first_is<NavigatorInterface::SetMaxRotationMessage>() ) {
153 
154  logger->log_debug(name(), "setting max rotation velocity to %f", msg->max_rotation());
155  if_navi_->set_max_rotation(msg->max_rotation());
156 
157  } else if ( if_navi_->msgq_first_is<NavigatorInterface::SetEscapingMessage>() ) {
159 
160  logger->log_debug(name(), "setting escaping allowed to %u", msg->is_escaping_enabled());
161  if_navi_->set_escaping_enabled(msg->is_escaping_enabled());
162 
165 
166  logger->log_debug(name(), "setting security distance to %f", msg->security_distance());
167  if_navi_->set_security_distance(msg->security_distance());
168 
169  } else if ( if_navi_->msgq_first_is<NavigatorInterface::SetStopAtTargetMessage>() ) {
171 
172  logger->log_debug(name(), "setting stop_at_target to %u", msg->is_stop_at_target());
173  if_navi_->set_stop_at_target(msg->is_stop_at_target());
174 
177 
178  logger->log_debug(name(), "setting orient_at_target to %s", if_navi_->tostring_OrientationMode( msg->orientation_mode() ) );
179  if_navi_->set_orientation_mode( msg->orientation_mode() );
180 
181  } else if ( if_navi_->msgq_first_is<NavigatorInterface::SetDriveModeMessage>() ) {
183 
184  logger->log_debug(name(), "setting drive_mode to %f", if_navi_->tostring_DriveMode(msg->drive_mode()));
185  if_navi_->set_drive_mode(msg->drive_mode());
186 
187  } else if ( if_navi_->msgq_first_is<NavigatorInterface::ResetParametersMessage>() ) {
188 
189  logger->log_debug(name(), "resetting colli parameters to default values (from config)");
190  if_navi_->set_max_velocity(cfg_max_velocity_);
191  if_navi_->set_max_rotation(cfg_max_rotation_);
192  if_navi_->set_escaping_enabled(cfg_escaping_enabled_);
193  if_navi_->set_security_distance(cfg_security_distance_);
194  if_navi_->set_stop_at_target(cfg_stop_at_target_);
195  if_navi_->set_orientation_mode(cfg_orient_mode_);
196  if_navi_->set_drive_mode(cfg_drive_mode_);
197 
198  } else {
199  logger->log_debug(name(), "Ignoring unhandled Navigator message");
200  }
201 
202  if_navi_->msgq_pop();
203  }
204 
205  // process last motion message
206  if( motion_msg ) {
207  if( motion_msg->is_of_type<NavigatorInterface::StopMessage>() ) {
208  logger->log_debug(name(), "StopMessage received");
209  thread_colli_->colli_stop();
210 
211  } else if( motion_msg->is_of_type<NavigatorInterface::CartesianGotoMessage>() ) {
213  logger->log_debug(name(), "CartesianGotoMessage received, x:%f y:%f ori:%f", msg->x(), msg->y(), msg->orientation());
214  // Converts from Fawkes Coord Sys -> RCSoftX Coord Sys, hence X_f = X_r, Y_f = -Y_r, Ori_f = -Ori_r
215  if_navi_->set_msgid(msg->id());
216  if_navi_->set_dest_x(msg->x());
217  if_navi_->set_dest_y(msg->y());
218  if_navi_->set_dest_ori(msg->orientation());
219  if_navi_->set_dest_dist(sqrt(msg->x()*msg->x() + msg->y()*msg->y()));
220  if_navi_->set_final(false);
221 
222  thread_colli_->colli_relgoto(msg->x(), msg->y(), msg->orientation(), if_navi_);
223 
224  } else if( motion_msg->is_of_type<NavigatorInterface::PolarGotoMessage>() ) {
226  logger->log_debug(name(), "PolarGotoMessage received, phi:%f dist:%f", msg->phi(), msg->dist());
227  // Converts from Fawkes Coord Sys -> RCSoftX Coord Sys, hence D_f = D_r, Phi_f = - Phi_r, Ori_f = Ori_r
228 
229  float cart_x = 0, cart_y = 0;
230  polar2cart2d(msg->phi(), msg->dist(), &cart_x, &cart_y);
231 
232  if_navi_->set_msgid(msg->id());
233  if_navi_->set_dest_x(cart_x);
234  if_navi_->set_dest_y(cart_y);
235  if_navi_->set_dest_ori(msg->orientation());
236  if_navi_->set_dest_dist(msg->dist());
237  if_navi_->set_final(false);
238 
239  thread_colli_->colli_relgoto(cart_x, cart_y, msg->orientation(), if_navi_);
240  }
241 
242  motion_msg->unref();
243  }
244 
245  if_navi_->write();
246 }
247 
248 bool
249 ColliActThread::colli_final()
250 {
251  return thread_colli_->is_final();
252  // RCSoftX had more. Full code(from libmonaco and navigator_server) :
253  //
254  // return (thread_colli_->is_final() || m_pNavServer->isColliFinal() );
255  // bbClients::Navigator_Server::isColliFinal()
256  // {
257  // bool alive = (bool)m_ColliFeedback.GetValue( 8 /* magic value for alive index */ );
258  // bool final = (bool)m_ColliFeedback.GetValue( 0 /* magic value for final index */ );
259  // Timestamp cf(m_itsColliFeedback_sec.GetValue(), m_itsColliFeedback_usec.GetValue());
260  // Timestamp tp(m_itsTargetPoint_sec.GetValue(), m_itsTargetPoint_usec.GetValue());
261 
262  // return alive && final && ( (cf - tp) == 0);
263  // }
264 }
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:70
virtual ~ColliActThread()
Desctructor.
Definition: act_thread.cpp:53
void set_stop_at_target(const bool new_stop_at_target)
Set stop_at_target value.
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Definition: message.h:44
unsigned int id() const
Get message ID.
Definition: message.cpp:197
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1048
void unref()
Decrement reference count and conditionally delete this instance.
Definition: refcount.cpp:99
void set_escaping_enabled(const bool new_escaping_enabled)
Set escaping_enabled value.
float orientation() const
Get orientation value.
Fawkes library namespace.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
void set_final(const bool new_final)
Set final value.
SetSecurityDistanceMessage Fawkes BlackBoard Interface Message.
bool is_escaping_enabled() const
Get escaping_enabled value.
STL namespace.
float max_rotation() const
Get max_rotation value.
void colli_stop()
Sends a stop-command.
PolarGotoMessage Fawkes BlackBoard Interface Message.
float max_velocity() const
Get max_velocity value.
Thread class encapsulation of pthreads.
Definition: thread.h:42
SetOrientationModeMessage Fawkes BlackBoard Interface Message.
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:500
bool is_stop_at_target() const
Get stop_at_target value.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:44
const char * tostring_DriveMode(DriveMode value) const
Convert DriveMode constant to string.
float orientation() const
Get orientation value.
void set_security_distance(const float new_security_distance)
Set security_distance value.
SetStopAtTargetMessage Fawkes BlackBoard Interface Message.
SetEscapingMessage Fawkes BlackBoard Interface Message.
Thread aspect to use blocked timing.
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1193
Thread that performs the navigation and collision avoidance algorithms.
Definition: colli_thread.h:58
SetDriveModeMessage Fawkes BlackBoard Interface Message.
Base class for exceptions in Fawkes.
Definition: exception.h:36
Message * msgq_first()
Get the first message from the message queue.
Definition: interface.cpp:1180
SetMaxRotationMessage Fawkes BlackBoard Interface Message.
void colli_relgoto(float x, float y, float ori, fawkes::NavigatorInterface *iface)
Sends a goto-command, using relative coordinates.
CartesianGotoMessage Fawkes BlackBoard Interface Message.
DriveMode drive_mode() const
Get drive_mode value.
void ref()
Increment reference count.
Definition: refcount.cpp:70
ColliActThread(ColliThread *colli_thread)
Constructor.
Definition: act_thread.cpp:45
bool is_final() const
Checks if the colli is final.
const char * name() const
Get name of thread.
Definition: thread.h:95
void set_drive_mode(const DriveMode new_drive_mode)
Set drive_mode value.
void set_max_velocity(const float new_max_velocity)
Set max_velocity value.
bool msgq_first_is()
Check if first message has desired type.
Definition: interface.h:314
float security_distance() const
Get security_distance value.
virtual void loop()
Code to execute in the thread.
Definition: act_thread.cpp:119
bool is_of_type()
Check if message has desired type.
Definition: message.h:138
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.
virtual void finalize()
Finalize the thread.
Definition: act_thread.cpp:113
virtual void init()
Initialize the thread.
Definition: act_thread.cpp:58
void set_max_rotation(const float new_max_rotation)
Set max_rotation value.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
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.
OrientationMode orientation_mode() const
Get orientation_mode value.
void set_orientation_mode(const OrientationMode new_orientation_mode)
Set orientation_mode value.
ResetParametersMessage Fawkes BlackBoard Interface Message.
void set_dest_y(const float new_dest_y)
Set dest_y value.
void set_msgid(const uint32_t new_msgid)
Set msgid value.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:44
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
SetMaxVelocityMessage Fawkes BlackBoard Interface Message.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
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.