24 #include "escape_drive_mode.h"
26 #include "../utils/rob/roboshape_colli.h"
40 : AbstractDriveMode(logger, config)
47 robo_shape_.reset(
new RoboShapeColli(
"/plugins/colli/roboshape/", logger, config, 2));
78 logger_->
log_debug(
"EscapeDriveModule",
"EscapeDriveModule( update ): Calculating ESCAPING...");
82 fill_normalized_readings();
83 sort_normalized_readings();
85 bool danger_front = check_danger(readings_front_);
86 bool danger_back = check_danger(readings_back_);
88 bool can_turn_left = turn_left_allowed();
89 bool can_turn_right = turn_right_allowed();
97 if (check_danger(readings_left_front_))
100 if (check_danger(readings_left_back_))
103 if (check_danger(readings_right_front_))
106 if (check_danger(readings_right_back_))
115 if (danger_front && danger_back && can_turn_right) {
118 }
else if (danger_front && danger_back && can_turn_left) {
121 }
else if (!danger_front && danger_back) {
129 }
else if (danger_front && !danger_back) {
137 }
else if (!danger_front && !danger_back) {
158 laser_points_ = laser_points;
165 EscapeDriveModule::fill_normalized_readings()
167 readings_normalized_.clear();
169 for (
unsigned int i = 0; i < laser_points_.size(); i++) {
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);
178 EscapeDriveModule::sort_normalized_readings()
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();
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());
192 float ang_mr =
normalize_rad(robo_shape_->get_angle_right());
194 if (!((ang_fl < ang_ml) && (ang_ml < ang_bl) && (ang_bl < ang_br) && (ang_br < ang_mr)
195 && (ang_mr < ang_fr)))
201 while (i < laser_points_.size()) {
202 if (laser_points_.at(i).r > 0.01f) {
205 if (rad < ang_fl || rad >= ang_fr)
206 readings_front_.push_back(readings_normalized_[i]);
208 else if (rad < ang_ml)
209 readings_left_front_.push_back(readings_normalized_[i]);
211 else if (rad < ang_bl)
212 readings_left_back_.push_back(readings_normalized_[i]);
214 else if (rad < ang_br)
215 readings_back_.push_back(readings_normalized_[i]);
217 else if (rad < ang_mr)
218 readings_right_back_.push_back(readings_normalized_[i]);
220 else if (rad < ang_fr)
221 readings_right_front_.push_back(readings_normalized_[i]);
229 EscapeDriveModule::check_danger(std::vector<float> readings)
232 for (
unsigned int i = 0; i < readings.size(); i++)
233 if (readings[i] < 0.06f)
240 EscapeDriveModule::turn_left_allowed()
242 for (
unsigned int i = 0; i < readings_front_.size(); i++)
243 if (readings_front_[i] < 0.12f)
246 for (
unsigned int i = 0; i < readings_right_front_.size(); i++)
247 if (readings_right_front_[i] < 0.06f)
250 for (
unsigned int i = 0; i < readings_back_.size(); i++)
251 if (readings_back_[i] < 0.07f)
254 for (
unsigned int i = 0; i < readings_left_back_.size(); i++)
255 if (readings_left_back_[i] < 0.13f)
262 EscapeDriveModule::turn_right_allowed()
264 for (
unsigned int i = 0; i < readings_front_.size(); i++)
265 if (readings_front_[i] < 0.12f)
268 for (
unsigned int i = 0; i < readings_left_front_.size(); i++)
269 if (readings_left_front_[i] < 0.06f)
272 for (
unsigned int i = 0; i < readings_back_.size(); i++)
273 if (readings_back_[i] < 0.07f)
276 for (
unsigned int i = 0; i < readings_right_back_.size(); i++)
277 if (readings_right_back_[i] < 0.13f)