Fawkes API Fawkes Development Version
abstract_drive_mode.h
1
2/***************************************************************************
3 * abstract_drive_mode.h - Abstract base class for a drive-mode
4 *
5 * Created: Fri Oct 18 15:16:23 2013
6 * Copyright 2002 Stefan Jacobs
7 * 2013 Bahram Maleki-Fard
8 ****************************************************************************/
9
10/* This program is free software; you can redistribute it and/or modify
11 * it under the terms of the GNU General Public License as published by
12 * the Free Software Foundation; either version 2 of the License, or
13 * (at your option) any later version.
14 *
15 * This program is distributed in the hope that it will be useful,
16 * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 * GNU Library General Public License for more details.
19 *
20 * Read the full text in the LICENSE.GPL file in the doc directory.
21 */
22
23#ifndef _PLUGINS_COLLI_ABSTRACT_DRIVE_MODE_H_
24#define _PLUGINS_COLLI_ABSTRACT_DRIVE_MODE_H_
25
26#include "../common/types.h"
27
28#include <config/config.h>
29#include <interfaces/NavigatorInterface.h>
30#include <logging/logger.h>
31
32#include <cmath>
33
34namespace fawkes {
35
36/** @class AbstractDriveMode <plugins/colli/drive_modes/abstract_drive_mode.h>
37 * This is the base class which calculates drive modes. Drive modes are the
38 * proposed settings for the drive-realization out of the found search things.
39 */
41{
42public:
43 AbstractDriveMode(Logger *logger, Configuration *config);
44 virtual ~AbstractDriveMode();
45
46 ///\brief Sets the current target.
47 void set_current_target(float x, float y, float ori);
48
49 ///\brief Sets the current robo position.
50 void set_current_robo_pos(float x, float y, float ori);
51
52 ///\brief Sets the current robo speed.
53 void set_current_robo_speed(float x, float y, float rot);
54
55 ///\brief Set the colli mode values for each drive mode.
57
58 ///\brief Set the local targetpoint found by the search.
59 void set_local_target(float x, float y);
60
61 ///\brief Set the local trajectory point found by the search.
62 void set_local_trajec(float x, float y);
63
64 ///\brief Returns the drive modes name.
66
67 ///\brief Calculate the proposed settings which are asked for afterwards.
68 virtual void update() = 0;
69
70 ///\brief Returns the proposed x translation
72
73 ///\brief Returns the proposed y translation
75
76 ///\brief Returns the proposed rotatio
77 float get_proposed_rot();
78
79protected:
80 ///\brief Perform linear interpolation
81 float lin_interpol(float x, float left, float right, float bot, float top);
82
83 ///\brief Get velocity that guarantees a stop for a given distance
84 float guarantee_trans_stop(float distance, float current_trans, float desired_trans);
85
86 field_pos_t target_; /**< current target */
87 field_pos_t robot_; /**< current robot pos */
88
89 colli_trans_rot_t robot_vel_; /**< current robot velocity */
90 float robot_speed_; /**< current robo translation velocity */
91
92 cart_coord_2d_t local_target_; /**< local target */
93 cart_coord_2d_t local_trajec_; /**< local trajectory */
94
96 bool stop_at_target_; /**< flag if stopping on or after target */
97
98 colli_trans_rot_t proposed_; /**< proposed translation and rotation for next timestep */
99
100 NavigatorInterface::DriveMode drive_mode_; /**< the drive mode name */
101
102 Logger * logger_; /**< The fawkes logger */
103 Configuration *config_; /**< The fawkes configuration */
104
105 float max_trans_; /**< The maximum translation speed */
106 float max_rot_; /**< The maximum rotation speed */
107
108private:
109 float max_trans_acc_;
110 float max_trans_dec_;
111 float max_rot_acc_;
112 float max_rot_dec_;
113 int frequency_;
114
115 float stopping_distance_;
116 float stopping_factor_;
117};
118
119/* ************************************************************************** */
120/* *********************** IMPLEMENTATION DETAILS ************************* */
121/* ************************************************************************** */
122
123/** Constructor.
124 * @param logger The fawkes logger
125 * @param config The fawkes configuration
126 */
128: logger_(logger), config_(config)
129{
130 logger_->log_debug("AbstractDriveMode", "(Constructor): Entering...");
133
134 // read max_trans_dec_ and max_rot_dec_
135 max_trans_acc_ = /*0.75* */ config_->get_float("/plugins/colli/motor_instruct/trans_acc");
136 max_trans_dec_ = /*0.75* */ config_->get_float("/plugins/colli/motor_instruct/trans_dec");
137 max_rot_acc_ = /*0.75* */ config_->get_float("/plugins/colli/motor_instruct/rot_acc");
138 max_rot_dec_ = /*0.75* */ config_->get_float("/plugins/colli/motor_instruct/rot_dec");
139
140 stopping_distance_ =
141 config_->get_float("/plugins/colli/drive_mode/stopping_adjustment/distance_addition");
142 stopping_factor_ =
143 config_->get_float("/plugins/colli/drive_mode/stopping_adjustment/deceleration_factor");
144 stopping_factor_ = std::min(1.f, std::max(0.f, stopping_factor_));
145
146 frequency_ = config_->get_int("/plugins/colli/frequency");
147
148 logger_->log_debug("AbstractDriveMode", "(Constructor): Exiting...");
149}
150
151/** Desctructor. */
153{
154 logger_->log_debug("AbstractDriveMode", "(Destructor): Entering...");
155 logger_->log_debug("AbstractDriveMode", "(Destructor): Exiting...");
156}
157
158/** Returns the proposed x translation which was calculated previously in
159 * 'update()' which has to be implemented!
160 * @return The proposed translation
161 */
162inline float
164{
165 return proposed_.x;
166}
167
168/** Returns the proposed y translation which was calculated previously in
169 * 'update()' which has to be implemented!
170 * @return The proposed translation
171 */
172inline float
174{
175 return proposed_.y;
176}
177
178/** Returns the proposed rotation which was calculated previously in
179 * 'update()' which has to be implemented!
180 * @return The proposed rotation
181 */
182inline float
184{
185 return proposed_.rot;
186}
187
188/** Sets the current target.
189 * Has to be set before update!
190 * @param x The target x position
191 * @param y The target y position
192 * @param ori The target orientation
193 */
194inline void
195AbstractDriveMode::set_current_target(float x, float y, float ori)
196{
197 target_.x = x;
198 target_.y = y;
199 target_.ori = ori;
200}
201
202/** Sets the current robo position.
203 * Has to be set before update!
204 * @param x The robot x position
205 * @param y The robot y position
206 * @param ori The robot orientation
207 */
208inline void
209AbstractDriveMode::set_current_robo_pos(float x, float y, float ori)
210{
211 robot_.x = x;
212 robot_.y = y;
213 robot_.ori = ori;
214}
215
216/** Sets the current robo speed.
217 * Has to be set before update!
218 * @param x The robot translation velocity in x-direction only
219 * @param y The robot translation velocity in y-direction only
220 * @param rot The robot rotation velocity
221 */
222inline void
224{
225 robot_vel_.x = x;
226 robot_vel_.y = y;
227 robot_vel_.rot = rot;
228 robot_speed_ = sqrt(x * x + y * y);
229 if (x < 0)
231}
232
233/** Set the colli mode values for each drive mode.
234 * Has to be set before update!
235 * @param orient Orient at target after target position reached?
236 * @param stop Stop at target position?
237 */
238inline void
240{
241 orient_mode_ = orient;
242 stop_at_target_ = stop;
243}
244
245/** Set the local targetpoint found by the search.
246 * Has to be set before update!
247 * @param x The local target x position
248 * @param y The local target y position
249 */
250inline void
252{
253 local_target_.x = x;
254 local_target_.y = y;
255}
256
257/** Set the local trajectory point found by the search.
258 * Has to be set before update!
259 * @param x The local target x trajectory
260 * @param y The local target y trajectory
261 */
262inline void
264{
265 local_trajec_.x = x;
266 local_trajec_.y = y;
267}
268
269/** Get the drive mode.
270 * Has to be set in the constructor of your drive mode!
271 * @return The drive mode
272 */
275{
276 return drive_mode_;
277}
278
279/** Performs linear interpolation
280 * @param x x
281 * @param x1 x1
282 * @param x2 x2
283 * @param y1 y1
284 * @param y2 y2
285 * @return interpolated value
286 */
287inline float
288AbstractDriveMode::lin_interpol(float x, float x1, float x2, float y1, float y2)
289{
290 return (((x - x1) * (y2 - y1)) / (x2 - x1) + y1);
291}
292
293/** Get velocity that guarantees a stop for a given distance
294 * @param distance The distance to stop at
295 * @param current_trans Robot's current translation velocity
296 * @param desired_trans Robot's currently desired translation velocity
297 * @return Proposed translation velocity to stop at given distance
298 */
299inline float
300AbstractDriveMode::guarantee_trans_stop(float distance, float current_trans, float desired_trans)
301{
302 distance = fabs(distance);
303 current_trans = fabs(current_trans);
304
305 if (distance < 0.05f)
306 return 0.f;
307
308 if (current_trans < 0.05f)
309 return desired_trans;
310
311 // calculate riemann integral to get distance until robot is stoped
312 float trans_tmp = current_trans;
313 float distance_to_stop = stopping_distance_;
314 for (int loops_to_stop = 0; trans_tmp > 0; loops_to_stop++) {
315 distance_to_stop += trans_tmp / frequency_; //First calculate sum (Untersumme)
316 trans_tmp -= max_trans_dec_ * stopping_factor_; //Then decrease tmp speed
317 }
318
319 // logger_->log_debug("AbstractDriveMode","guarantee_trans_stop: distance needed to stop - distance to goal: %f - %f = %f", distance_to_stop, distance, distance_to_stop - distance);
320 if (distance_to_stop >= distance) {
321 return 0.f;
322 } else {
323 return desired_trans;
324 }
325 //
326 // // dividing by 10 because we're called at 10Hz (TODO: use config value!!)
327 // int time_needed_to_distance = (int)( distance / (current_trans/10.0) );
328 //
329 // /* (changes made during AdoT)
330 // * 0.1 is an empirical value causing the expected deceleration while
331 // * calculating with a slower one. This is likely the difference between what
332 // * the colli wants and the motor actually does.
333 // * We also add 1, to start deceleration 1 step earlier than the calculation
334 // * suggests. Tests showed better results. We could probably skip this, if we
335 // * had a proper calculation of velocity adjustment...
336 // */
337 // int time_needed_to_stop = (int)( current_trans / (0.1*max_trans_dec_) ) +1;
338 //
339 // if( time_needed_to_stop >= time_needed_to_distance ) {
340 // float value = std::max( 0.f, current_trans - max_trans_dec_ );
341 // return value;
342 // } else {
343 // float value = std::min( current_trans + max_trans_acc_, desired_trans );
344 // // Use this if you are very cautions:
345 // //float value = std::min( current_trans + std::min(max_trans_dec_, max_trans_acc_), desired_trans );
346 // return value;
347 // }
348}
349
350} // end namespace fawkes
351
352#endif
This is the base class which calculates drive modes.
bool stop_at_target_
flag if stopping on or after target
virtual ~AbstractDriveMode()
Desctructor.
void set_current_target(float x, float y, float ori)
Sets the current target.
void set_current_colli_mode(NavigatorInterface::OrientationMode orient, bool stop)
Set the colli mode values for each drive mode.
float max_trans_
The maximum translation speed.
float get_proposed_trans_x()
Returns the proposed x translation.
float get_proposed_rot()
Returns the proposed rotatio.
NavigatorInterface::OrientationMode orient_mode_
orient mode of nav if
cart_coord_2d_t local_trajec_
local trajectory
float robot_speed_
current robo translation velocity
field_pos_t target_
current target
field_pos_t robot_
current robot pos
colli_trans_rot_t robot_vel_
current robot velocity
AbstractDriveMode(Logger *logger, Configuration *config)
Constructor.
NavigatorInterface::DriveMode get_drive_mode_name()
Returns the drive modes name.
float max_rot_
The maximum rotation speed.
Configuration * config_
The fawkes configuration.
void set_local_target(float x, float y)
Set the local targetpoint found by the search.
float get_proposed_trans_y()
Returns the proposed y translation.
NavigatorInterface::DriveMode drive_mode_
the drive mode name
void set_current_robo_speed(float x, float y, float rot)
Sets the current robo speed.
void set_local_trajec(float x, float y)
Set the local trajectory point found by the search.
colli_trans_rot_t proposed_
proposed translation and rotation for next timestep
float lin_interpol(float x, float left, float right, float bot, float top)
Perform linear interpolation.
float guarantee_trans_stop(float distance, float current_trans, float desired_trans)
Get velocity that guarantees a stop for a given distance.
Logger * logger_
The fawkes logger.
void set_current_robo_pos(float x, float y, float ori)
Sets the current robo position.
cart_coord_2d_t local_target_
local target
virtual void update()=0
Calculate the proposed settings which are asked for afterwards.
Interface for configuration handling.
Definition: config.h:68
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
virtual int get_int(const char *path)=0
Get value from configuration which is of type int.
Interface for logging.
Definition: logger.h:42
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
@ MovingNotAllowed
Moving not allowed constant.
OrientationMode
Orientation mode enum.
Fawkes library namespace.
float distance(float x1, float y1, float x2, float y2)
Get distance between two 2D cartesian coordinates.
Definition: angle.h:59
Cartesian coordinates (2D).
Definition: types.h:65
float y
y coordinate
Definition: types.h:67
float x
x coordinate
Definition: types.h:66
Storing Translation and rotation.
Definition: types.h:60
float x
Translation in x-direction.
Definition: types.h:61
float y
Translation in y-direction.
Definition: types.h:62
float rot
Rotation around z-axis.
Definition: types.h:63
Position on the field.
Definition: types.h:125
float y
y coordinate in meters
Definition: types.h:127
float ori
orientation
Definition: types.h:128
float x
x coordinate in meters
Definition: types.h:126