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 <interfaces/NavigatorInterface.h>
29 #include <logging/logger.h>
30 #include <config/config.h>
31 
32 #include <cmath>
33 
34 namespace fawkes
35 {
36 #if 0 /* just to make Emacs auto-indent happy */
37 }
38 #endif
39 
40 /** @class AbstractDriveMode <plugins/colli/drive_modes/abstract_drive_mode.h>
41  * This is the base class which calculates drive modes. Drive modes are the
42  * proposed settings for the drive-realization out of the found search things.
43  */
45 {
46  public:
47  AbstractDriveMode(Logger* logger, Configuration* config);
48  virtual ~AbstractDriveMode();
49 
50  ///\brief Sets the current target.
51  void set_current_target( float x, float y, float ori );
52 
53  ///\brief Sets the current robo position.
54  void set_current_robo_pos( float x, float y, float ori );
55 
56  ///\brief Sets the current robo speed.
57  void set_current_robo_speed( float x, float y, float rot );
58 
59  ///\brief Set the colli mode values for each drive mode.
61 
62  ///\brief Set the local targetpoint found by the search.
63  void set_local_target( float x, float y );
64 
65  ///\brief Set the local trajectory point found by the search.
66  void set_local_trajec( float x, float y );
67 
68  ///\brief Returns the drive modes name.
70 
71 
72  ///\brief Calculate the proposed settings which are asked for afterwards.
73  virtual void update() = 0;
74 
75  ///\brief Returns the proposed x translation
76  float get_proposed_trans_x();
77 
78  ///\brief Returns the proposed y translation
79  float get_proposed_trans_y();
80 
81  ///\brief Returns the proposed rotatio
82  float get_proposed_rot();
83 
84 
85  protected:
86 
87  ///\brief Perform linear interpolation
88  float lin_interpol( float x, float left, float right, float bot, float top );
89 
90  ///\brief Get velocity that guarantees a stop for a given distance
91  float guarantee_trans_stop( float distance, float current_trans, float desired_trans );
92 
93  field_pos_t target_; /**< current target */
94  field_pos_t robot_; /**< current robot pos */
95 
96  colli_trans_rot_t robot_vel_; /**< current robot velocity */
97  float robot_speed_; /**< current robo translation velocity */
98 
99  cart_coord_2d_t local_target_; /**< local target */
100  cart_coord_2d_t local_trajec_; /**< local trajectory */
101 
102  NavigatorInterface::OrientationMode orient_mode_; /**< orient mode of nav if */
103  bool stop_at_target_; /**< flag if stopping on or after target */
104 
105  colli_trans_rot_t proposed_; /**< proposed translation and rotation for next timestep */
106 
107  NavigatorInterface::DriveMode drive_mode_; /**< the drive mode name */
108 
109  Logger* logger_; /**< The fawkes logger */
110  Configuration* config_; /**< The fawkes configuration */
111 
112  float max_trans_; /**< The maximum translation speed */
113  float max_rot_; /**< The maximum rotation speed */
114 
115 private:
116 
117  float max_trans_acc_;
118  float max_trans_dec_;
119  float max_rot_acc_;
120  float max_rot_dec_;
121  int frequency_;
122 
123  float stopping_distance_;
124  float stopping_factor_;
125 };
126 
127 
128 
129 /* ************************************************************************** */
130 /* *********************** IMPLEMENTATION DETAILS ************************* */
131 /* ************************************************************************** */
132 
133 /** Constructor.
134  * @param logger The fawkes logger
135  * @param config The fawkes configuration
136  */
137 inline
139  : logger_( logger ),
140  config_( config )
141 {
142  logger_->log_debug("AbstractDriveMode", "(Constructor): Entering...");
143  proposed_.x = proposed_.y = proposed_.rot = 0.f;
145 
146  // read max_trans_dec_ and max_rot_dec_
147  max_trans_acc_ = /*0.75* */config_->get_float("/plugins/colli/motor_instruct/trans_acc");
148  max_trans_dec_ = /*0.75* */config_->get_float("/plugins/colli/motor_instruct/trans_dec");
149  max_rot_acc_ = /*0.75* */config_->get_float("/plugins/colli/motor_instruct/rot_acc");
150  max_rot_dec_ = /*0.75* */config_->get_float("/plugins/colli/motor_instruct/rot_dec");
151 
152  stopping_distance_ = config_->get_float("/plugins/colli/drive_mode/stopping_adjustment/distance_addition");
153  stopping_factor_ = config_->get_float("/plugins/colli/drive_mode/stopping_adjustment/deceleration_factor");
154  stopping_factor_ = std::min(1.f, std::max(0.f, stopping_factor_));
155 
156  frequency_ = config_->get_int("/plugins/colli/frequency");
157 
158  logger_->log_debug("AbstractDriveMode", "(Constructor): Exiting...");
159 }
160 
161 
162 /** Desctructor. */
163 inline
165 {
166  logger_->log_debug("AbstractDriveMode", "(Destructor): Entering...");
167  logger_->log_debug("AbstractDriveMode", "(Destructor): Exiting...");
168 }
169 
170 
171 /** Returns the proposed x translation which was calculated previously in
172  * 'update()' which has to be implemented!
173  * @return The proposed translation
174  */
175 inline float
177 {
178  return proposed_.x;
179 }
180 
181 /** Returns the proposed y translation which was calculated previously in
182  * 'update()' which has to be implemented!
183  * @return The proposed translation
184  */
185 inline float
187 {
188  return proposed_.y;
189 }
190 
191 /** Returns the proposed rotation which was calculated previously in
192  * 'update()' which has to be implemented!
193  * @return The proposed rotation
194  */
195 inline float
197 {
198  return proposed_.rot;
199 }
200 
201 
202 /** Sets the current target.
203  * Has to be set before update!
204  * @param x The target x position
205  * @param y The target y position
206  * @param ori The target orientation
207  */
208 inline void
209 AbstractDriveMode::set_current_target( float x, float y, float ori )
210 {
211  target_.x = x;
212  target_.y = y;
213  target_.ori = ori;
214 }
215 
216 /** Sets the current robo position.
217  * Has to be set before update!
218  * @param x The robot x position
219  * @param y The robot y position
220  * @param ori The robot orientation
221  */
222 inline void
223 AbstractDriveMode::set_current_robo_pos( float x, float y, float ori )
224 {
225  robot_.x = x;
226  robot_.y = y;
227  robot_.ori = ori;
228 }
229 
230 /** Sets the current robo speed.
231  * Has to be set before update!
232  * @param x The robot translation velocity in x-direction only
233  * @param y The robot translation velocity in y-direction only
234  * @param rot The robot rotation velocity
235  */
236 inline void
237 AbstractDriveMode::set_current_robo_speed( float x, float y, float rot )
238 {
239  robot_vel_.x = x;
240  robot_vel_.y = y;
241  robot_vel_.rot = rot;
242  robot_speed_ = sqrt(x*x + y*y);
243  if( x < 0 )
245 }
246 
247 /** Set the colli mode values for each drive mode.
248  * Has to be set before update!
249  * @param orient Orient at target after target position reached?
250  * @param stop Stop at target position?
251  */
252 inline void
254 {
255  orient_mode_ = orient;
256  stop_at_target_ = stop;
257 }
258 
259 
260 /** Set the local targetpoint found by the search.
261  * Has to be set before update!
262  * @param x The local target x position
263  * @param y The local target y position
264  */
265 inline void
267 {
268  local_target_.x = x;
269  local_target_.y = y;
270 }
271 
272 /** Set the local trajectory point found by the search.
273  * Has to be set before update!
274  * @param x The local target x trajectory
275  * @param y The local target y trajectory
276  */
277 inline void
279 {
280  local_trajec_.x = x;
281  local_trajec_.y = y;
282 }
283 
284 /** Get the drive mode.
285  * Has to be set in the constructor of your drive mode!
286  * @return The drive mode
287  */
290 {
291  return drive_mode_;
292 }
293 
294 /** Performs linear interpolation
295  * @param x x
296  * @param x1 x1
297  * @param x2 x2
298  * @param y1 y1
299  * @param y2 y2
300  * @return interpolated value
301  */
302 inline float
303 AbstractDriveMode::lin_interpol( float x, float x1, float x2,
304  float y1, float y2 )
305 {
306  return ( ((x-x1)*(y2-y1))/(x2-x1) + y1 );
307 }
308 
309 /** Get velocity that guarantees a stop for a given distance
310  * @param distance The distance to stop at
311  * @param current_trans Robot's current translation velocity
312  * @param desired_trans Robot's currently desired translation velocity
313  * @return Proposed translation velocity to stop at given distance
314  */
315 inline float
317  float current_trans,
318  float desired_trans )
319 {
320  distance = fabs( distance );
321  current_trans = fabs( current_trans );
322 
323  if ( distance < 0.05f )
324  return 0.f;
325 
326  if ( current_trans < 0.05f )
327  return desired_trans;
328 
329  // calculate riemann integral to get distance until robot is stoped
330  float trans_tmp = current_trans;
331  float distance_to_stop = stopping_distance_;
332  for (int loops_to_stop = 0; trans_tmp > 0; loops_to_stop++) {
333  distance_to_stop += trans_tmp / frequency_; //First calculate sum (Untersumme)
334  trans_tmp -= max_trans_dec_ * stopping_factor_; //Then decrease tmp speed
335  }
336 
337 // 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);
338  if (distance_to_stop >= distance) {
339  return 0.f;
340  } else {
341  return desired_trans;
342  }
343 //
344 // // dividing by 10 because we're called at 10Hz (TODO: use config value!!)
345 // int time_needed_to_distance = (int)( distance / (current_trans/10.0) );
346 //
347 // /* (changes made during AdoT)
348 // * 0.1 is an empirical value causing the expected deceleration while
349 // * calculating with a slower one. This is likely the difference between what
350 // * the colli wants and the motor actually does.
351 // * We also add 1, to start deceleration 1 step earlier than the calculation
352 // * suggests. Tests showed better results. We could probably skip this, if we
353 // * had a proper calculation of velocity adjustment...
354 // */
355 // int time_needed_to_stop = (int)( current_trans / (0.1*max_trans_dec_) ) +1;
356 //
357 // if( time_needed_to_stop >= time_needed_to_distance ) {
358 // float value = std::max( 0.f, current_trans - max_trans_dec_ );
359 // return value;
360 // } else {
361 // float value = std::min( current_trans + max_trans_acc_, desired_trans );
362 // // Use this if you are very cautions:
363 // //float value = std::min( current_trans + std::min(max_trans_dec_, max_trans_acc_), desired_trans );
364 // return value;
365 // }
366 }
367 
368 } // end namespace fawkes
369 
370 #endif
float x
Translation in x-direction.
Definition: types.h:61
virtual void update()=0
Calculate the proposed settings which are asked for afterwards.
float distance(float x1, float y1, float x2, float y2)
Get distance between two 2D cartesian coordinates.
Definition: angle.h:62
Cartesian coordinates (2D).
Definition: types.h:59
cart_coord_2d_t local_target_
local target
float get_proposed_trans_x()
Returns the proposed x translation.
Logger * logger_
The fawkes logger.
Fawkes library namespace.
NavigatorInterface::DriveMode get_drive_mode_name()
Returns the drive modes name.
colli_trans_rot_t proposed_
proposed translation and rotation for next timestep
NavigatorInterface::OrientationMode orient_mode_
orient mode of nav if
OrientationMode
Orientation mode enum.
void set_current_target(float x, float y, float ori)
Sets the current target.
bool stop_at_target_
flag if stopping on or after target
float max_trans_
The maximum translation speed.
virtual int get_int(const char *path)=0
Get value from configuration which is of type int.
Storing Translation and rotation.
Definition: types.h:60
float y
Translation in y-direction.
Definition: types.h:62
float rot
Rotation around z-axis.
Definition: types.h:63
float max_rot_
The maximum rotation speed.
void set_local_target(float x, float y)
Set the local targetpoint found by the search.
NavigatorInterface::DriveMode drive_mode_
the drive mode name
field_pos_t target_
current target
colli_trans_rot_t robot_vel_
current robot velocity
float guarantee_trans_stop(float distance, float current_trans, float desired_trans)
Get velocity that guarantees a stop for a given distance.
void set_local_trajec(float x, float y)
Set the local trajectory point found by the search.
void set_current_robo_pos(float x, float y, float ori)
Sets the current robo position.
float x
x coordinate in meters
Definition: types.h:111
void set_current_robo_speed(float x, float y, float rot)
Sets the current robo speed.
Position on the field.
Definition: types.h:110
field_pos_t robot_
current robot pos
float get_proposed_trans_y()
Returns the proposed y translation.
float lin_interpol(float x, float left, float right, float bot, float top)
Perform linear interpolation.
Configuration * config_
The fawkes configuration.
This is the base class which calculates drive modes.
float y
y coordinate
Definition: types.h:61
void set_current_colli_mode(NavigatorInterface::OrientationMode orient, bool stop)
Set the colli mode values for each drive mode.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
AbstractDriveMode(Logger *logger, Configuration *config)
Constructor.
float ori
orientation
Definition: types.h:113
float robot_speed_
current robo translation velocity
float get_proposed_rot()
Returns the proposed rotatio.
virtual ~AbstractDriveMode()
Desctructor.
float y
y coordinate in meters
Definition: types.h:112
Interface for configuration handling.
Definition: config.h:67
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
cart_coord_2d_t local_trajec_
local trajectory
Interface for logging.
Definition: logger.h:34
float x
x coordinate
Definition: types.h:60