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
33using namespace fawkes;
34using 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),
48 thread_colli_(colli_thread)
49{
50}
51
52/** Desctructor. */
54{
55}
56
57void
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
115void
117{
118 blackboard->close(if_navi_);
119}
120
121void
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();
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
165
166 logger->log_debug(name(), "setting escaping allowed to %u", msg->is_escaping_enabled());
168
172
173 logger->log_debug(name(), "setting security distance to %f", 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
188 "setting orient_at_target to %s",
190 if_navi_->set_orientation_mode(msg->orientation_mode());
191
195
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);
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);
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
270bool
271ColliActThread::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:351
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1215
Message * msgq_first()
Get the first message from the message queue.
Definition: interface.cpp:1200
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:501
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1062
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:44
bool is_of_type()
Check if message has desired type.
Definition: message.h:161
unsigned int id() const
Get message ID.
Definition: message.cpp:181
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