Fawkes API
Fawkes Development Version
|
22 #include "escape_potential_field_omni_drive_mode.h"
24 #include "../common/types.h"
25 #include "../search/og_laser.h"
27 #include <utils/math/angle.h>
44 logger_->
log_debug(
"EscapePotentialFieldOmniDriveModule",
"(Constructor): Entering...");
54 cfg_write_spam_debug_ =
config_->
get_bool(
"/plugins/colli/write_spam_debug");
56 logger_->
log_debug(
"EscapePotentialFieldOmniDriveModule",
"(Constructor): Exiting...");
63 logger_->
log_debug(
"EscapePotentialFieldOmniDriveModule",
"(Destructor): Entering...");
64 logger_->
log_debug(
"EscapePotentialFieldOmniDriveModule",
"(Destructor): Exiting...");
79 robot_pos_.
x = robo_x;
80 robot_pos_.
y = robo_y;
108 if (cfg_write_spam_debug_) {
110 "EscapePotentialFieldOmniDriveModule( update ): Calculating ESCAPING...");
123 float target_x = 0.f;
124 float target_y = 0.f;
126 for (
int posX = 0; posX < width; ++posX) {
127 for (
int posY = 0; posY < height; ++posY) {
128 if (occ_grid_->
get_prob(posX, posY) >= cell_cost_occ) {
129 float dx = float(posX - robot_pos_.
x) * cell_height / 100;
130 float dy = float(posY - robot_pos_.
y) * cell_width / 100;
132 if (dx != 0.f && dy != 0.f) {
133 float factor = 1.f / ((dx * dx + dy * dy) * (dx * dx + dy * dy));
135 target_x -= factor * dx;
136 target_y -= factor * dy;
142 target.
r = sqrt(target_x * target_x + target_y * target_y);
143 target.
phi = atan2(target_y, target_x);
145 if (cfg_write_spam_debug_) {
147 "Target vector: phi: %f\t%f",
153 float angle_difference = M_PI_2 - 0.2f;
155 float angle_abs = fabs(angle);
158 float turn_direction = 0.f;
159 float drive_part_x = 1.f;
160 float drive_part_y = 0.f;
162 if (angle_abs > angle_difference) {
167 turn_direction = -1.f;
169 turn_direction = 1.f;
174 drive_part_x = std::cos(target.
phi);
175 drive_part_y = std::sin(target.
phi);
179 if (cfg_write_spam_debug_) {
180 logger_->
log_debug(
"EscapePotentialFieldOmniDriveModule",
"Turn %f", turn_direction);
184 if (cfg_write_spam_debug_) {
192 if (fabs(turn_direction) > 0.2f) {
float max_rot_
The maximum rotation speed.
NavigatorInterface::DriveMode drive_mode_
the drive mode name
float max_trans_
The maximum translation speed.
colli_trans_rot_t proposed_
proposed translation and rotation for next timestep
float rot
Rotation around z-axis.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
int get_width()
Get the width of the grid.
float y
Translation in y-direction.
unsigned int occ
The cost for an occupied cell.
Interface for configuration handling.
This is the base class which calculates drive modes.
float x
Translation in x-direction.
Probability get_prob(int x, int y)
Get the occupancy probability of a cell.
EscapePotentialFieldOmniDriveModule(Logger *logger, Configuration *config)
Constructor.
Fawkes library namespace.
Configuration * config_
The fawkes configuration.
~EscapePotentialFieldOmniDriveModule()
Destruct your local values here.
Logger * logger_
The fawkes logger.
void set_grid_information(LaserOccupancyGrid *occ_grid, int robo_x, int robo_y)
This function sets the Grid information for one escape step.
float normalize_mirror_rad(float angle_rad)
Normalize angle in radian between -PI (inclusive) and PI (exclusive).
colli_cell_cost_t get_cell_costs() const
Get cell costs.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
virtual void update()
Calculate here your desired settings.
This OccGrid is derived by the Occupancy Grid originally from Andreas Strack, but modified for speed ...
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
int get_cell_width()
Get the cell width (in cm)
int get_height()
Get the height of the grid.
int get_cell_height()
Get the cell height (in cm)