24 #include "escape_drive_mode.h" 26 #include "../utils/rob/roboshape_colli.h" 52 robo_shape_ =
new RoboShapeColli(
"/plugins/colli/roboshape/", logger, config, 2 );
89 logger_->
log_debug(
"EscapeDriveModule",
"EscapeDriveModule( update ): Calculating ESCAPING...");
93 fill_normalized_readings();
94 sort_normalized_readings();
96 bool danger_front = check_danger( readings_front_ );
97 bool danger_back = check_danger( readings_back_ );
99 bool can_turn_left = turn_left_allowed();
100 bool can_turn_right = turn_right_allowed();
108 if (check_danger(readings_left_front_))
111 if (check_danger(readings_left_back_))
114 if (check_danger(readings_right_front_))
117 if (check_danger(readings_right_back_))
127 if ( danger_front && danger_back && can_turn_right ) {
130 }
else if ( danger_front && danger_back && can_turn_left ) {
133 }
else if (!danger_front && danger_back) {
141 }
else if (danger_front && !danger_back) {
149 }
else if ( !danger_front && !danger_back ) {
171 laser_points_ = laser_points;
179 EscapeDriveModule::fill_normalized_readings()
181 readings_normalized_.clear();
183 for (
unsigned int i = 0; i < laser_points_.size(); i++ ) {
186 float length = laser_points_.at( i ).r;
187 readings_normalized_.push_back( length - sub );
193 EscapeDriveModule::sort_normalized_readings()
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();
209 if(!( (ang_fl < ang_ml) && (ang_ml < ang_bl) && (ang_bl < ang_br)
210 &&(ang_br < ang_mr) && (ang_mr < ang_fr) ))
216 while ( i < laser_points_.size() ) {
217 if( laser_points_.at(i).r > 0.01f ) {
220 if( rad < ang_fl || rad >= ang_fr )
221 readings_front_.push_back( readings_normalized_[i] );
223 else if( rad < ang_ml )
224 readings_left_front_.push_back( readings_normalized_[i] );
226 else if( rad < ang_bl )
227 readings_left_back_.push_back( readings_normalized_[i] );
229 else if( rad < ang_br )
230 readings_back_.push_back( readings_normalized_[i] );
232 else if( rad < ang_mr )
233 readings_right_back_.push_back( readings_normalized_[i] );
235 else if( rad < ang_fr )
236 readings_right_front_.push_back( readings_normalized_[i] );
245 EscapeDriveModule::check_danger( std::vector< float > readings )
248 for (
unsigned int i = 0; i < readings.size(); i++ )
249 if ( readings[i] < 0.06f )
257 EscapeDriveModule::turn_left_allowed()
259 for (
unsigned int i = 0; i < readings_front_.size(); i++ )
260 if ( readings_front_[i] < 0.12f )
263 for (
unsigned int i = 0; i < readings_right_front_.size(); i++ )
264 if ( readings_right_front_[i] < 0.06f )
267 for (
unsigned int i = 0; i < readings_back_.size(); i++ )
268 if ( readings_back_[i] < 0.07f )
271 for (
unsigned int i = 0; i < readings_left_back_.size(); i++ )
272 if ( readings_left_back_[i] < 0.13f )
281 EscapeDriveModule::turn_right_allowed()
283 for (
unsigned int i = 0; i < readings_front_.size(); i++ )
284 if ( readings_front_[i] < 0.12f )
287 for (
unsigned int i = 0; i < readings_left_front_.size(); i++ )
288 if ( readings_left_front_[i] < 0.06f )
291 for (
unsigned int i = 0; i < readings_back_.size(); i++ )
292 if ( readings_back_[i] < 0.07f )
295 for (
unsigned int i = 0; i < readings_right_back_.size(); i++ )
296 if ( readings_right_back_[i] < 0.13f )
float x
Translation in x-direction.
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).
Logger * logger_
The fawkes logger.
Fawkes library namespace.
float get_angle_right() const
Get angle to middle of the right side of the robot.
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.
virtual void update()
Calculate here your desired settings.
float rot
Rotation around z-axis.
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.
~EscapeDriveModule()
Destructor.
float x
x coordinate in meters
float get_angle_front_right() const
Get angle to the front right corner of the robot.
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 get_angle_front_left() const
Get angle to the front left corner of the robot.
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
Interface for configuration handling.
float get_angle_back_left() const
Get angle to of the rear left corner robot.
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.