24#include "escape_drive_mode.h"
26#include "../utils/rob/roboshape_colli.h"
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;
165EscapeDriveModule::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);
178EscapeDriveModule::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]);
229EscapeDriveModule::check_danger(std::vector<float> readings)
232 for (
unsigned int i = 0; i < readings.size(); i++)
233 if (readings[i] < 0.06f)
240EscapeDriveModule::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)
262EscapeDriveModule::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)
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.
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.
~EscapeDriveModule()
Destructor.
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_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).
float x
Translation in x-direction.
float y
Translation in y-direction.
float rot
Rotation around z-axis.
float y
y coordinate in meters
float x
x coordinate in meters