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