Fawkes API  Fawkes Development Version
select_drive_mode.cpp
1 
2 /***************************************************************************
3  * select_drive_mode.cpp - Class that selects drive-mode from a collection
4  *
5  * Created: Fri Oct 18 15:16:23 2013
6  * Copyright 2002 Stefan Jacobs
7  * 2013-2014 Bahram Maleki-Fard
8  * 2014 Tobias Neumann
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 "../common/types.h"
25 #include "select_drive_mode.h"
26 
27 // INCLUDE HERE YOUR DRIVE MODES!!!
28 #include "stop_drive_mode.h"
29 #include "escape_drive_mode.h"
30 #include "escape_potential_field_drive_mode.h"
31 #include "forward_drive_mode.h"
32 #include "backward_drive_mode.h"
33 #include "biward_drive_mode.h"
34 
35 #include "forward_omni_drive_mode.h"
36 #include "escape_potential_field_omni_drive_mode.h"
37 // YOUR CHANGES SHOULD END HERE!!!
38 
39 #include "../search/og_laser.h"
40 
41 #include <interfaces/MotorInterface.h>
42 #include <interfaces/NavigatorInterface.h>
43 #include <logging/logger.h>
44 #include <config/config.h>
45 #include <utils/math/angle.h>
46 
47 namespace fawkes
48 {
49 #if 0 /* just to make Emacs auto-indent happy */
50 }
51 #endif
52 
53 /** @class SelectDriveMode <plugins/colli/drive_modes/select_drive_mode.h>
54  * This class selects the correct drive mode and calls the appopriate drive component
55  */
56 
57 /** Constructor.
58  * @param motor The motor interface
59  * @param target The "colli target" NavigatorInterface
60  * @param logger The fawkes logger
61  * @param config The fawkes configuration
62  * @param escape_mode The chosen escape mode
63  */
65  NavigatorInterface* target,
66  Logger* logger,
67  Configuration* config,
68  colli_escape_mode_t escape_mode )
69  : logger_( logger ),
70  config_( config ),
71  if_colli_target_( target ),
72  if_motor_( motor ),
73  cfg_escape_mode_( escape_mode ),
74  escape_flag_( 0 ) // no escaping at the beginning
75 {
76  logger_->log_debug("SelectDriveMode", "(Constructor): Entering");
77  drive_modes_.clear();
78 
79  std::string drive_restriction = config->get_string("/plugins/colli/drive_mode/restriction");
80 
81  if ( drive_restriction == "omnidirectional" ) {
82  drive_restriction_ = colli_drive_restriction_t::omnidirectional;
83  } else if ( drive_restriction == "differential" ) {
84  drive_restriction_ = colli_drive_restriction_t::differential;
85  } else {
86  throw fawkes::Exception("Drive restriction '%s' is unknown", drive_restriction.c_str());
87  }
88 
89  logger_->log_debug("SelectDriveMode", "(Constructor): Creating Drive Mode Objects");
90 
91  // Add generic drive modes
92  drive_modes_.push_back( (AbstractDriveMode *)new StopDriveModule(logger_, config_) );
93 
94  // Add specific drive modes
95  if ( drive_restriction_ == colli_drive_restriction_t::omnidirectional ) {
96  load_drive_modes_omnidirectional();
97  } else {
98  load_drive_modes_differential();
99  }
100 
101  logger_->log_debug("SelectDriveMode", "(Constructor): Exiting");
102 }
103 
104 /** Desctructor. */
106 {
107  logger_->log_debug("SelectDriveMode", "(Destructor): Entering");
108  for ( unsigned int i = 0; i < drive_modes_.size(); i++ )
109  delete drive_modes_[i];
110  logger_->log_debug("SelectDriveMode", "(Destructor): Exiting");
111 }
112 
113 
114 void
115 SelectDriveMode::load_drive_modes_differential()
116 {
117  // escape drive mode
118  if (cfg_escape_mode_ == colli_escape_mode_t::potential_field) {
119  drive_modes_.push_back( (AbstractDriveMode *)new EscapePotentialFieldDriveModule( logger_, config_) );
120  } else if (cfg_escape_mode_ == colli_escape_mode_t::basic) {
121  drive_modes_.push_back( (AbstractDriveMode *)new EscapeDriveModule( logger_, config_) );
122  } else {
123  logger_->log_error("SelectDriveMode", "Unknown escape drive mode. Using basic as default");
124  drive_modes_.push_back( (AbstractDriveMode *)new EscapeDriveModule( logger_, config_) );
125  }
126 
127  // forward drive mode (have to remember for biward driving!
128  ForwardDriveModule* forward = new ForwardDriveModule(logger_, config_);
129  drive_modes_.push_back( (AbstractDriveMode *)forward );
130 
131  // backward drive mode (have to remember for biward driving!
132  BackwardDriveModule* backward = new BackwardDriveModule(logger_, config_);
133  drive_modes_.push_back( (AbstractDriveMode *)backward );
134 
135  // biward drive mode (takes both forward and backward drive modes as argument!
136  drive_modes_.push_back( (AbstractDriveMode *)new BiwardDriveModule(forward, backward, logger_, config_) );
137 }
138 
139 
140 void
141 SelectDriveMode::load_drive_modes_omnidirectional()
142 {
143  // escape drive mode
144  if (cfg_escape_mode_ == colli_escape_mode_t::potential_field) {
145  drive_modes_.push_back( (AbstractDriveMode *)new EscapePotentialFieldOmniDriveModule( logger_, config_) );
146  } else if (cfg_escape_mode_ == colli_escape_mode_t::basic) {
147  // CAUTION: This is an differential drive mode!
148  drive_modes_.push_back( (AbstractDriveMode *)new EscapeDriveModule( logger_, config_) );
149  } else {
150  logger_->log_error("SelectDriveMode", "Unknown escape drive mode. Using potential field omni as default");
151  drive_modes_.push_back( (AbstractDriveMode *)new EscapePotentialFieldOmniDriveModule( logger_, config_) );
152  }
153 
154  ForwardOmniDriveModule* forward = new ForwardOmniDriveModule(logger_, config_);
155  drive_modes_.push_back( (AbstractDriveMode *)forward );
156 }
157 
158 
159 /** Set local target point before update!
160  * @param x x-coordinate
161  * @param y y-coordinate
162  */
163 void
165 {
166  local_target_.x = x;
167  local_target_.y = y;
168 }
169 
170 /** Set local target trajectory before update!
171  * @param x x-coordinate
172  * @param y y-coordinate
173  */
174 void
176 {
177  local_trajec_.x = x;
178  local_trajec_.y = y;
179 }
180 
181 /** Returns the proposed x translation which was previously calculated in update()
182  * @return The proposed translation
183  */
184 float
186 {
187  return proposed_.x;
188 }
189 
190 /** Returns the proposed y translation which was previously calculated in update()
191  * @return The proposed translation
192  */
193 float
195 {
196  return proposed_.y;
197 }
198 
199 /** Returns the proposed rotation which was previously calculated in update()
200  * @return The proposed rotation
201  */
202 float
204 {
205  return proposed_.rot;
206 }
207 
208 /**
209  * search for the escape drive mode and hands over the given information to the escape drive mode
210  * This should just be called if potential-field-escape mode is used!
211  * @param occ_grid pointer to the occ_grid
212  * @param robo_x robot position on the grid in x
213  * @param robo_y robot position on the grid in y
214  */
215 void
216 SelectDriveMode::set_grid_information( LaserOccupancyGrid* occ_grid, int robo_x, int robo_y )
217 {
218  for ( unsigned int i = 0; i < drive_modes_.size(); i++ ) {
219  // drive mode checking
220  if ( drive_modes_[i]->get_drive_mode_name() == NavigatorInterface::ESCAPE ) {
221  ((EscapePotentialFieldDriveModule*)drive_modes_[i])->set_grid_information( occ_grid, robo_x, robo_y );
222 
223  return;
224  }
225  }
226  logger_->log_error("SelectDriveMode", "Can't find escape drive mode to set grid information");
227 }
228 
229 /**
230  * search for the escape drive mode and hands over the given information to the escape drive mode
231  * This should just be called if basic-escape mode is used!
232  * @param laser_points vector of laser points
233  */
234 void
235 SelectDriveMode::set_laser_data( std::vector<polar_coord_2d_t>& laser_points )
236 {
237  for ( unsigned int i = 0; i < drive_modes_.size(); i++ ) {
238  // drive mode checking
239  if ( drive_modes_[i]->get_drive_mode_name() == NavigatorInterface::ESCAPE ) {
240  ((EscapeDriveModule*)drive_modes_[i])->set_laser_data( laser_points );
241 
242  return;
243  }
244  }
245  logger_->log_error("SelectDriveMode", "Can't find escape drive mode to set laser information");
246 }
247 
248 /* ****************************************************************************** */
249 /* ****************************************************************************** */
250 /* U P D A T E */
251 /* ****************************************************************************** */
252 /* ****************************************************************************** */
253 
254 /** Pick the drive-mode that should be used and calculate the proposed translation
255  * and rotation for the current target (which is set by set_local_target() and
256  * set_local_trajec(), so make sure to call them beforehand).
257  * update() has to be called before the proposed values are fetched.
258  * @param escape Set to true if we want to enter escape-mode
259  */
260 void
262 {
263  AbstractDriveMode * drive_mode = NULL;
264  proposed_.x = proposed_.y = proposed_.rot = 0.f;
265 
266  // choose the correct drive mode!
268  if ( escape == true ) {
269  if( escape_flag_ == 0
270  && if_motor_->des_vx() != 0.f
271  && if_motor_->des_vx() != 0.f
272  && if_motor_->des_omega() != 0.f ) {
274  // we have not yet stopped!
275 
276  } else {
277  // we have stopped recently, so do escape!
278  escape_flag_ = 1;
279  desired_mode = NavigatorInterface::ESCAPE;
280  }
281 
282  } else {
283  escape_flag_ = 0;
284  desired_mode = if_colli_target_->drive_mode();
285  }
286 
287  // now search this specific drive mode in the list
288  for ( unsigned int i = 0; i < drive_modes_.size(); i++ ) {
289  // error checking
290  if ( drive_modes_[i]->get_drive_mode_name() == desired_mode
291  && drive_mode != 0 ) {
292  logger_->log_error("SelectDriveMode", "Error while selecting drive mode. There is more than one mode with the same name!!! Stopping!");
293  drive_mode = 0;
294  break;
295  }
296 
297  // drive mode checking
298  if ( drive_modes_[i]->get_drive_mode_name() == desired_mode
299  && drive_mode == 0 ) {
300  drive_mode = drive_modes_[i];
301  }
302  }
303 
304 
305  if ( drive_mode == 0 ) {
306  // invalid pointer
307  logger_->log_error("SelectDriveMode", "INVALID DRIVE MODE POINTER, stopping!");
308  proposed_.x = proposed_.y = proposed_.rot = 0.f;
309 
310  } else {
311  // valid drive mode!
312  // set the values for the drive mode
313  drive_mode->set_current_robo_pos( if_motor_->odometry_position_x(),
314  if_motor_->odometry_position_y(),
316 
317  drive_mode->set_current_robo_speed( if_motor_->vx(),
318  if_motor_->vy(),
319  if_motor_->omega() );
320 
321  drive_mode->set_current_target( if_colli_target_->dest_x(),
322  if_colli_target_->dest_y(),
323  if_colli_target_->dest_ori() );
324 
325  drive_mode->set_local_target( local_target_.x, local_target_.y );
326  drive_mode->set_local_trajec( local_trajec_.x, local_trajec_.y );
327  drive_mode->set_current_colli_mode( if_colli_target_->orientation_mode(), if_colli_target_->is_stop_at_target() );
328 
329  // update the drive mode
330  drive_mode->update();
331 
332  // get the values from the drive mode
333  proposed_.x = drive_mode->get_proposed_trans_x();
334  proposed_.y = drive_mode->get_proposed_trans_y();
335  proposed_.rot = drive_mode->get_proposed_rot();
336 
337  // recheck with targetobj maximum settings
338  if( (if_colli_target_->max_velocity() != 0.0)
339  && (fabs( proposed_.x ) > fabs( if_colli_target_->max_velocity() )) ) {
340  if ( proposed_.x > 0.0 )
341  proposed_.x = if_colli_target_->max_velocity();
342  else
343  proposed_.x = -if_colli_target_->max_velocity();
344  }
345 
346  if( (if_colli_target_->max_velocity() != 0.0)
347  && (fabs( proposed_.y ) > fabs( if_colli_target_->max_velocity() )) ) {
348  if ( proposed_.y > 0.0 )
349  proposed_.y = if_colli_target_->max_velocity();
350  else
351  proposed_.y = -if_colli_target_->max_velocity();
352  }
353 
354  if( ( if_colli_target_->max_rotation() != 0.0 )
355  && (fabs( proposed_.rot ) > fabs( if_colli_target_->max_rotation() )) ) {
356  if ( proposed_.rot > 0.0 )
357  proposed_.rot = if_colli_target_->max_rotation();
358  else
359  proposed_.rot = -if_colli_target_->max_rotation();
360  }
361 
362  }
363 }
364 
365 } // namespace fawkes
SelectDriveMode(MotorInterface *motor, NavigatorInterface *colli_target, Logger *logger, Configuration *config, colli_escape_mode_t escape_mode=colli_escape_mode_t::basic)
Constructor.
float max_velocity() const
Get max_velocity value.
float x
Translation in x-direction.
Definition: types.h:61
float odometry_position_x() const
Get odometry_position_x value.
virtual void update()=0
Calculate the proposed settings which are asked for afterwards.
void set_laser_data(std::vector< fawkes::polar_coord_2d_t > &laser_points)
search for the escape drive mode and hands over the given information to the escape drive mode This s...
float get_proposed_rot()
Returns the proposed rotation. After an update.
float get_proposed_trans_x()
Returns the proposed x translation.
Fawkes library namespace.
bool is_stop_at_target() const
Get stop_at_target value.
void set_current_target(float x, float y, float ori)
Sets the current target.
void set_local_target(float x, float y)
Set local target point before update!
float dest_ori() const
Get dest_ori value.
float y
Translation in y-direction.
Definition: types.h:62
float vx() const
Get vx value.
float rot
Rotation around z-axis.
Definition: types.h:63
OrientationMode orientation_mode() const
Get orientation_mode value.
float odometry_orientation() const
Get odometry_orientation value.
float odometry_position_y() const
Get odometry_position_y value.
void set_local_target(float x, float y)
Set the local targetpoint found by the search.
float dest_y() const
Get dest_y value.
DriveMode drive_mode() const
Get drive_mode value.
This OccGrid is derived by the Occupancy Grid originally from Andreas Strack, but modified for speed ...
Definition: og_laser.h:49
float normalize_mirror_rad(float angle_rad)
Normalize angle in radian between -PI (inclusive) and PI (exclusive).
Definition: angle.h:75
float vy() const
Get vy value.
void set_local_trajec(float x, float y)
Set the local trajectory point found by the search.
This is the SlowForward drive-module, for slow forward only movements.
Base class for exceptions in Fawkes.
Definition: exception.h:36
void set_current_robo_pos(float x, float y, float ori)
Sets the current robo position.
Stop-Drive-Module.
float omega() const
Get omega value.
void set_current_robo_speed(float x, float y, float rot)
Sets the current robo speed.
void set_grid_information(LaserOccupancyGrid *occ_grid, int robo_x, int robo_y)
search for the escape drive mode and hands over the given information to the escape drive mode This s...
float get_proposed_trans_y()
Returns the proposed y translation.
void update(bool escape=false)
Has to be called before the proposed values are called.
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
This is the Forward drive-module, for forward only movements.
Class Escape-Drive-Module.
This is the base class which calculates drive modes.
float y
y coordinate
Definition: types.h:61
void set_local_trajec(float x, float y)
Set local trajectory point before update!
This is the SlowBackward drive-module, for slow backward only movements.
float des_vx() const
Get des_vx value.
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.
float dest_x() const
Get dest_x value.
colli_escape_mode_t
Colli Escape modes.
Definition: types.h:67
float des_omega() const
Get des_omega value.
MotorInterface Fawkes BlackBoard Interface.
float get_proposed_rot()
Returns the proposed rotatio.
Interface for configuration handling.
Definition: config.h:67
This is the SlowBiward drive-module.
float get_proposed_trans_x()
Returns the proposed translation. After an update.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
NavigatorInterface Fawkes BlackBoard Interface.
float max_rotation() const
Get max_rotation value.
float get_proposed_trans_y()
Returns the proposed translation. After an update.
Interface for logging.
Definition: logger.h:34
float x
x coordinate
Definition: types.h:60