Fawkes API  Fawkes Development Version
escape_drive_mode.cpp
1 
2 /***************************************************************************
3  * escape_drive_mode.cpp - Implementation of drive-mode "escape"
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 "escape_drive_mode.h"
25 
26 #include "../utils/rob/roboshape_colli.h"
27 
28 namespace fawkes {
29 
30 /** @class EscapeDriveModule <plugins/colli/drive_modes/escape_drive_mode.h>
31  * Class Escape-Drive-Module. This module is called, if an escape is neccessary.
32  * It should try to maximize distance to the disturbing obstacle.
33  */
34 
35 /** Constructor.
36  * @param logger The fawkes logger
37  * @param config The fawkes configuration
38  */
40 : AbstractDriveMode(logger, config)
41 {
43 
44  max_trans_ = config_->get_float("/plugins/colli/drive_mode/escape/max_trans");
45  max_rot_ = config_->get_float("/plugins/colli/drive_mode/escape/max_rot");
46 
47  robo_shape_.reset(new RoboShapeColli("/plugins/colli/roboshape/", logger, config, 2));
48 }
49 
50 /** Destructor. Destruct your local values here. */
52 {
53 }
54 
55 /* ************************************************************************** */
56 /* *********************** U P D A T E ************************* */
57 /* ************************************************************************** */
58 
59 /** Calculate here your desired settings. What you desire is checked afterwards to the current
60  * settings of the physical boundaries, but take care also.
61  *
62  * How you do this is up to you, but be careful, our hardware is expensive!!!!
63  *
64  * All values of the other drive modes inherited by the abstract-drive-mode are
65  * non-valid, because search did not succeed or should not have been called!
66  * So do not use them. Instead here you use the m_pLaser!
67  *
68  * Afterwards filled should be:
69  *
70 * proposed_ --> Desired translation and rotation speed
71  *
72  * Those values are questioned after an update() was called.
73  */
74 void
76 {
77  // This is only called, if we recently stopped...
78  logger_->log_debug("EscapeDriveModule", "EscapeDriveModule( update ): Calculating ESCAPING...");
79 
80  proposed_.x = proposed_.y = proposed_.rot = 0.f;
81 
82  fill_normalized_readings();
83  sort_normalized_readings();
84 
85  bool danger_front = check_danger(readings_front_);
86  bool danger_back = check_danger(readings_back_);
87 
88  bool can_turn_left = turn_left_allowed();
89  bool can_turn_right = turn_right_allowed();
90 
91  if (danger_front)
92  logger_->log_debug("EscapeDriveModule", "DANGER IN FRONT");
93 
94  if (danger_back)
95  logger_->log_debug("EscapeDriveModule", "DANGER IN BACK");
96 
97  if (check_danger(readings_left_front_))
98  logger_->log_debug("EscapeDriveModule", "DANGER IN LEFT FRONT");
99 
100  if (check_danger(readings_left_back_))
101  logger_->log_debug("EscapeDriveModule", "DANGER IN LEFT BACK");
102 
103  if (check_danger(readings_right_front_))
104  logger_->log_debug("EscapeDriveModule", "DANGER IN RIGHT FRONT");
105 
106  if (check_danger(readings_right_back_))
107  logger_->log_debug("EscapeDriveModule", "DANGER IN RIGHT BACK");
108 
109  if (!can_turn_left)
110  logger_->log_debug("EscapeDriveModule", "DANGER IF TURNING LEFT!!!");
111 
112  if (!can_turn_right)
113  logger_->log_debug("EscapeDriveModule", "DANGER IF TURNING RIGHT!!!");
114 
115  if (danger_front && danger_back && can_turn_right) {
117 
118  } else if (danger_front && danger_back && can_turn_left) {
120 
121  } else if (!danger_front && danger_back) {
123 
124  if ((can_turn_right) && (local_target_.y <= robot_.y))
126  else if ((can_turn_left) && (local_target_.y >= robot_.y))
128 
129  } else if (danger_front && !danger_back) {
131 
132  if ((can_turn_right) && (local_target_.y <= robot_.y))
134  else if ((can_turn_left) && (local_target_.y >= robot_.y))
136 
137  } else if (!danger_front && !danger_back) {
138  // depending on target coordinates, decide which direction to escape to
139  if (target_.x > robot_.x)
141  else
143 
144  if ((can_turn_right) && (local_target_.y <= robot_.y))
146  else if ((can_turn_left) && (local_target_.y >= robot_.y))
148  }
149 }
150 
151 /**
152  * This function sets the laser points for one escape round
153  * @param laser_points vector of laser points
154  */
155 void
156 EscapeDriveModule::set_laser_data(std::vector<polar_coord_2d_t> &laser_points)
157 {
158  laser_points_ = laser_points;
159 }
160 
161 /* ************************************************************************** */
162 /* *********************** Private Methods ************************* */
163 /* ************************************************************************** */
164 void
165 EscapeDriveModule::fill_normalized_readings()
166 {
167  readings_normalized_.clear();
168 
169  for (unsigned int i = 0; i < laser_points_.size(); i++) {
170  float rad = normalize_rad(laser_points_.at(i).phi);
171  float sub = robo_shape_->get_robot_length_for_rad(rad);
172  float length = laser_points_.at(i).r;
173  readings_normalized_.push_back(length - sub);
174  }
175 }
176 
177 void
178 EscapeDriveModule::sort_normalized_readings()
179 {
180  readings_front_.clear();
181  readings_back_.clear();
182  readings_left_front_.clear();
183  readings_left_back_.clear();
184  readings_right_front_.clear();
185  readings_right_back_.clear();
186 
187  float ang_fl = normalize_rad(robo_shape_->get_angle_front_left());
188  float ang_fr = normalize_rad(robo_shape_->get_angle_front_right());
189  float ang_bl = normalize_rad(robo_shape_->get_angle_back_left());
190  float ang_br = normalize_rad(robo_shape_->get_angle_back_right());
191  float ang_ml = normalize_rad(robo_shape_->get_angle_left());
192  float ang_mr = normalize_rad(robo_shape_->get_angle_right());
193 
194  if (!((ang_fl < ang_ml) && (ang_ml < ang_bl) && (ang_bl < ang_br) && (ang_br < ang_mr)
195  && (ang_mr < ang_fr)))
196  logger_->log_error("RoboShape", "Angles are bad!!!");
197 
198  unsigned int i = 0;
199  float rad = 0.f;
200 
201  while (i < laser_points_.size()) {
202  if (laser_points_.at(i).r > 0.01f) {
203  rad = normalize_rad(laser_points_.at(i).phi);
204 
205  if (rad < ang_fl || rad >= ang_fr)
206  readings_front_.push_back(readings_normalized_[i]);
207 
208  else if (rad < ang_ml)
209  readings_left_front_.push_back(readings_normalized_[i]);
210 
211  else if (rad < ang_bl)
212  readings_left_back_.push_back(readings_normalized_[i]);
213 
214  else if (rad < ang_br)
215  readings_back_.push_back(readings_normalized_[i]);
216 
217  else if (rad < ang_mr)
218  readings_right_back_.push_back(readings_normalized_[i]);
219 
220  else if (rad < ang_fr)
221  readings_right_front_.push_back(readings_normalized_[i]);
222  }
223 
224  ++i;
225  }
226 }
227 
228 bool
229 EscapeDriveModule::check_danger(std::vector<float> readings)
230 {
231  // if something is smaller than 5 cm, you have to flee.......
232  for (unsigned int i = 0; i < readings.size(); i++)
233  if (readings[i] < 0.06f)
234  return true;
235 
236  return false;
237 }
238 
239 bool
240 EscapeDriveModule::turn_left_allowed()
241 {
242  for (unsigned int i = 0; i < readings_front_.size(); i++)
243  if (readings_front_[i] < 0.12f)
244  return false;
245 
246  for (unsigned int i = 0; i < readings_right_front_.size(); i++)
247  if (readings_right_front_[i] < 0.06f)
248  return false;
249 
250  for (unsigned int i = 0; i < readings_back_.size(); i++)
251  if (readings_back_[i] < 0.07f)
252  return false;
253 
254  for (unsigned int i = 0; i < readings_left_back_.size(); i++)
255  if (readings_left_back_[i] < 0.13f)
256  return false;
257 
258  return true;
259 }
260 
261 bool
262 EscapeDriveModule::turn_right_allowed()
263 {
264  for (unsigned int i = 0; i < readings_front_.size(); i++)
265  if (readings_front_[i] < 0.12f)
266  return false;
267 
268  for (unsigned int i = 0; i < readings_left_front_.size(); i++)
269  if (readings_left_front_[i] < 0.06f)
270  return false;
271 
272  for (unsigned int i = 0; i < readings_back_.size(); i++)
273  if (readings_back_[i] < 0.07f)
274  return false;
275 
276  for (unsigned int i = 0; i < readings_right_back_.size(); i++)
277  if (readings_right_back_[i] < 0.13f)
278  return false;
279 
280  return true;
281 }
282 
283 } // namespace fawkes
This is the base class which calculates drive modes.
float max_trans_
The maximum translation speed.
field_pos_t target_
current target
field_pos_t robot_
current robot pos
float max_rot_
The maximum rotation speed.
Configuration * config_
The fawkes configuration.
NavigatorInterface::DriveMode drive_mode_
the drive mode name
colli_trans_rot_t proposed_
proposed translation and rotation for next timestep
Logger * logger_
The fawkes logger.
cart_coord_2d_t local_target_
local target
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.
EscapeDriveModule(Logger *logger, Configuration *config)
Constructor.
virtual void update()
Calculate here your desired settings.
void set_laser_data(std::vector< polar_coord_2d_t > &laser_points)
This function sets the laser points for one escape round.
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.
This class is mainly the same as the basic class with the difference that all data is precalculated o...
Fawkes library namespace.
float normalize_rad(float angle_rad)
Normalize angle in radian between 0 (inclusive) and 2*PI (exclusive).
Definition: angle.h:90
float y
y coordinate
Definition: types.h:67
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
float y
y coordinate in meters
Definition: types.h:127
float x
x coordinate in meters
Definition: types.h:126