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
28namespace 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 */
74void
76{
77 // This is only called, if we recently stopped...
78 logger_->log_debug("EscapeDriveModule", "EscapeDriveModule( update ): Calculating ESCAPING...");
79
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 */
155void
156EscapeDriveModule::set_laser_data(std::vector<polar_coord_2d_t> &laser_points)
157{
158 laser_points_ = laser_points;
159}
160
161/* ************************************************************************** */
162/* *********************** Private Methods ************************* */
163/* ************************************************************************** */
164void
165EscapeDriveModule::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
177void
178EscapeDriveModule::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
228bool
229EscapeDriveModule::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
239bool
240EscapeDriveModule::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
261bool
262EscapeDriveModule::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