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>
41 Configuration *config)
42 : AbstractDriveMode(logger, config)
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) {