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 
34 namespace 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  */
40 class AbstractDriveMode
41 {
42 public:
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
71  float get_proposed_trans_x();
72 
73  ///\brief Returns the proposed y translation
74  float get_proposed_trans_y();
75 
76  ///\brief Returns the proposed rotatio
77  float get_proposed_rot();
78 
79 protected:
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 
95  NavigatorInterface::OrientationMode orient_mode_; /**< orient mode of nav if */
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 
108 private:
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  */
162 inline 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  */
172 inline 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  */
182 inline 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  */
194 inline void
195 AbstractDriveMode::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  */
208 inline void
209 AbstractDriveMode::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  */
222 inline void
223 AbstractDriveMode::set_current_robo_speed(float x, float y, float rot)
224 {
225  robot_vel_.x = x;
226  robot_vel_.y = y;
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  */
238 inline 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  */
250 inline void
251 AbstractDriveMode::set_local_target(float x, float y)
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  */
262 inline void
263 AbstractDriveMode::set_local_trajec(float x, float y)
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  */
287 inline float
288 AbstractDriveMode::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  */
299 inline float
300 AbstractDriveMode::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
fawkes::AbstractDriveMode::max_rot_
float max_rot_
The maximum rotation speed.
Definition: abstract_drive_mode.h:110
fawkes::AbstractDriveMode::set_current_robo_speed
void set_current_robo_speed(float x, float y, float rot)
Sets the current robo speed.
Definition: abstract_drive_mode.h:227
fawkes::AbstractDriveMode::set_local_trajec
void set_local_trajec(float x, float y)
Set the local trajectory point found by the search.
Definition: abstract_drive_mode.h:267
fawkes::AbstractDriveMode::drive_mode_
NavigatorInterface::DriveMode drive_mode_
the drive mode name
Definition: abstract_drive_mode.h:104
fawkes::AbstractDriveMode::~AbstractDriveMode
virtual ~AbstractDriveMode()
Desctructor.
Definition: abstract_drive_mode.h:156
fawkes::AbstractDriveMode::max_trans_
float max_trans_
The maximum translation speed.
Definition: abstract_drive_mode.h:109
fawkes::AbstractDriveMode::robot_
field_pos_t robot_
current robot pos
Definition: abstract_drive_mode.h:91
fawkes::AbstractDriveMode::proposed_
colli_trans_rot_t proposed_
proposed translation and rotation for next timestep
Definition: abstract_drive_mode.h:102
fawkes::colli_trans_rot_t::rot
float rot
Rotation around z-axis.
Definition: types.h:68
fawkes::AbstractDriveMode::stop_at_target_
bool stop_at_target_
flag if stopping on or after target
Definition: abstract_drive_mode.h:100
fawkes::field_pos_t
Position on the field.
Definition: types.h:123
fawkes::AbstractDriveMode::get_proposed_rot
float get_proposed_rot()
Returns the proposed rotatio.
Definition: abstract_drive_mode.h:187
fawkes::AbstractDriveMode::get_drive_mode_name
NavigatorInterface::DriveMode get_drive_mode_name()
Returns the drive modes name.
Definition: abstract_drive_mode.h:278
fawkes::colli_trans_rot_t::y
float y
Translation in y-direction.
Definition: types.h:67
fawkes::cart_coord_2d_struct::y
float y
y coordinate
Definition: types.h:66
fawkes::AbstractDriveMode::robot_speed_
float robot_speed_
current robo translation velocity
Definition: abstract_drive_mode.h:94
fawkes::Configuration::get_int
virtual int get_int(const char *path)=0
fawkes::NavigatorInterface::MovingNotAllowed
Moving not allowed constant.
Definition: NavigatorInterface.h:64
fawkes::Configuration
Definition: config.h:68
fawkes::AbstractDriveMode::set_local_target
void set_local_target(float x, float y)
Set the local targetpoint found by the search.
Definition: abstract_drive_mode.h:255
fawkes::AbstractDriveMode::set_current_colli_mode
void set_current_colli_mode(NavigatorInterface::OrientationMode orient, bool stop)
Set the colli mode values for each drive mode.
Definition: abstract_drive_mode.h:243
fawkes::colli_trans_rot_t::x
float x
Translation in x-direction.
Definition: types.h:66
fawkes::colli_trans_rot_t
Storing Translation and rotation.
Definition: types.h:64
fawkes::AbstractDriveMode::local_target_
cart_coord_2d_t local_target_
local target
Definition: abstract_drive_mode.h:96
fawkes::field_pos_t::y
float y
y coordinate in meters
Definition: types.h:126
fawkes::AbstractDriveMode::orient_mode_
NavigatorInterface::OrientationMode orient_mode_
orient mode of nav if
Definition: abstract_drive_mode.h:99
fawkes::Logger
Definition: logger.h:40
fawkes::NavigatorInterface::DriveMode
DriveMode
Drive modes enum.
Definition: NavigatorInterface.h:63
fawkes
fawkes::distance
float distance(float x1, float y1, float x2, float y2)
Get distance between two 2D cartesian coordinates.
Definition: angle.h:63
fawkes::AbstractDriveMode::config_
Configuration * config_
The fawkes configuration.
Definition: abstract_drive_mode.h:107
fawkes::field_pos_t::x
float x
x coordinate in meters
Definition: types.h:125
fawkes::AbstractDriveMode::set_current_robo_pos
void set_current_robo_pos(float x, float y, float ori)
Sets the current robo position.
Definition: abstract_drive_mode.h:213
fawkes::AbstractDriveMode::guarantee_trans_stop
float guarantee_trans_stop(float distance, float current_trans, float desired_trans)
Get velocity that guarantees a stop for a given distance.
Definition: abstract_drive_mode.h:304
fawkes::AbstractDriveMode::get_proposed_trans_y
float get_proposed_trans_y()
Returns the proposed y translation.
Definition: abstract_drive_mode.h:177
fawkes::AbstractDriveMode::logger_
Logger * logger_
The fawkes logger.
Definition: abstract_drive_mode.h:106
fawkes::cart_coord_2d_struct
Cartesian coordinates (2D).
Definition: types.h:63
fawkes::cart_coord_2d_struct::x
float x
x coordinate
Definition: types.h:65
fawkes::AbstractDriveMode::get_proposed_trans_x
float get_proposed_trans_x()
Returns the proposed x translation.
Definition: abstract_drive_mode.h:167
fawkes::AbstractDriveMode::local_trajec_
cart_coord_2d_t local_trajec_
local trajectory
Definition: abstract_drive_mode.h:97
fawkes::NavigatorInterface::OrientationMode
OrientationMode
Orientation mode enum.
Definition: NavigatorInterface.h:73
fawkes::Configuration::get_float
virtual float get_float(const char *path)=0
fawkes::AbstractDriveMode::lin_interpol
float lin_interpol(float x, float left, float right, float bot, float top)
Perform linear interpolation.
Definition: abstract_drive_mode.h:292
fawkes::AbstractDriveMode::set_current_target
void set_current_target(float x, float y, float ori)
Sets the current target.
Definition: abstract_drive_mode.h:199
fawkes::AbstractDriveMode::robot_vel_
colli_trans_rot_t robot_vel_
current robot velocity
Definition: abstract_drive_mode.h:93
fawkes::AbstractDriveMode::target_
field_pos_t target_
current target
Definition: abstract_drive_mode.h:90
fawkes::field_pos_t::ori
float ori
orientation
Definition: types.h:127
fawkes::AbstractDriveMode::AbstractDriveMode
AbstractDriveMode(Logger *logger, Configuration *config)
Constructor.
Definition: abstract_drive_mode.h:131
fawkes::Logger::log_debug
virtual void log_debug(const char *component, const char *format,...)=0
fawkes::AbstractDriveMode::update
virtual void update()=0
Calculate the proposed settings which are asked for afterwards.