22 #include "escape_potential_field_drive_mode.h" 23 #include "../search/og_laser.h" 24 #include "../common/types.h" 26 #include <utils/math/angle.h> 56 cfg_write_spam_debug_ =
config_->
get_bool(
"/plugins/colli/write_spam_debug" );
80 robot_pos_.
x = robo_x;
81 robot_pos_.
y = robo_y;
110 if (cfg_write_spam_debug_) {
111 logger_->
log_debug(
"EscapeDriveModule",
"EscapeDriveModule( update ): Calculating ESCAPING...");
124 float target_x = 0.f;
125 float target_y = 0.f;
127 for (
int posX = 0; posX < width; ++posX) {
128 for (
int posY = 0; posY < height; ++posY) {
129 if (occ_grid_->
get_prob(posX,posY) >= cell_cost_occ) {
130 float dx = float(posX - robot_pos_.
x) * cell_height/100;
131 float dy = float(posY - robot_pos_.
y) * cell_width/100;
133 if (dx != 0.f && dy != 0.f) {
134 float factor = 1.f / ( (dx*dx + dy*dy) * (dx*dx + dy*dy) );
136 target_x -= factor * dx;
137 target_y -= factor * dy;
143 target.
r = sqrt( target_x*target_x + target_y*target_y );
144 target.
phi = atan2(target_y, target_x);
146 if (cfg_write_spam_debug_) {
147 logger_->
log_debug(
"EscapePotentialFieldDriveModule",
"Target vector: phi: %f\t%f", target.
phi, target.
r);
151 float angle_difference = 0.2f;
153 float angle_abs = fabs( angle );
156 float turn_direction = 0.f;
157 float drive_direction = 0.f;
159 if ( angle_abs > angle_difference ) {
165 turn_direction = -1.f;
167 turn_direction = 1.f;
181 drive_direction = 1.f;
191 if (cfg_write_spam_debug_) {
192 logger_->
log_debug(
"EscapePotentialFieldDriveModule",
"Turn %f", turn_direction);
196 if (cfg_write_spam_debug_) {
197 logger_->
log_debug(
"EscapePotentialFieldDriveModule",
"Drive %f", drive_direction);
float x
Translation in x-direction.
int get_width()
Get the width of the grid.
int get_height()
Get the height of the grid.
int get_cell_width()
Get the cell width (in cm)
Logger * logger_
The fawkes logger.
Fawkes library namespace.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
colli_trans_rot_t proposed_
proposed translation and rotation for next timestep
int get_cell_height()
Get the cell height (in cm)
Probability get_prob(int x, int y)
Get the occupancy probability of a cell.
~EscapePotentialFieldDriveModule()
Destruct your local values here.
float max_trans_
The maximum translation speed.
colli_cell_cost_t get_cell_costs() const
Get cell costs.
float y
Translation in y-direction.
float rot
Rotation around z-axis.
float max_rot_
The maximum rotation speed.
NavigatorInterface::DriveMode drive_mode_
the drive mode name
This OccGrid is derived by the Occupancy Grid originally from Andreas Strack, but modified for speed ...
float normalize_mirror_rad(float angle_rad)
Normalize angle in radian between -PI (inclusive) and PI (exclusive).
virtual void update()
Calculate here your desired settings.
unsigned int occ
The cost for an occupied cell.
Configuration * config_
The fawkes configuration.
This is the base class which calculates drive modes.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
EscapePotentialFieldDriveModule(Logger *logger, Configuration *config)
Constructor.
void set_grid_information(LaserOccupancyGrid *occ_grid, int robo_x, int robo_y)
This function sets the Grid information for one escape step.
Interface for configuration handling.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.