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