Fawkes API Fawkes Development Version
escape_potential_field_omni_drive_mode.cpp
1
2/***************************************************************************
3 * escape_potential_field_omni_drive_mode.cpp - Implementation of drive-mode "escape"
4 *
5 * Created: Tue Mar 25 17:24:18 2014
6 * Copyright 2014 Tobias Neumann
7 ****************************************************************************/
8
9/* This program is free software; you can redistribute it and/or modify
10 * it under the terms of the GNU General Public License as published by
11 * the Free Software Foundation; either version 2 of the License, or
12 * (at your option) any later version.
13 *
14 * This program is distributed in the hope that it will be useful,
15 * but WITHOUT ANY WARRANTY; without even the implied warranty of
16 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17 * GNU Library General Public License for more details.
18 *
19 * Read the full text in the LICENSE.GPL file in the doc directory.
20 */
21
22#include "escape_potential_field_omni_drive_mode.h"
23
24#include "../common/types.h"
25#include "../search/og_laser.h"
26
27#include <utils/math/angle.h>
28
29namespace fawkes {
30
31/** @class EscapePotentialFieldOmniDriveModule <plugins/colli/drive_modes/escape_potential_field_omni_drive_mode.h>
32 * Class Escape-Drive-Module. This module is called, if an escape is neccessary.
33 * It should try to maximize distance to the disturbing obstacle.
34 */
35
36/** Constructor.
37 * @param logger The fawkes logger
38 * @param config The fawkes configuration
39 */
41 Configuration *config)
42: AbstractDriveMode(logger, config)
43{
44 logger_->log_debug("EscapePotentialFieldOmniDriveModule", "(Constructor): Entering...");
46 occ_grid_ = NULL;
47 robot_pos_.x = 0.f;
48 robot_pos_.y = 0.f;
49 turn_ = 0;
50
51 max_trans_ = config_->get_float("/plugins/colli/drive_mode/escape/max_trans");
52 max_rot_ = config_->get_float("/plugins/colli/drive_mode/escape/max_rot");
53
54 cfg_write_spam_debug_ = config_->get_bool("/plugins/colli/write_spam_debug");
55
56 logger_->log_debug("EscapePotentialFieldOmniDriveModule", "(Constructor): Exiting...");
57}
58
59/** Destruct your local values here.
60 */
62{
63 logger_->log_debug("EscapePotentialFieldOmniDriveModule", "(Destructor): Entering...");
64 logger_->log_debug("EscapePotentialFieldOmniDriveModule", "(Destructor): Exiting...");
65}
66
67/**
68 * This function sets the Grid information for one escape step
69 * @param occ_grid pointer to the occ_grid
70 * @param robo_x robot position on the grid in x
71 * @param robo_y robot position on the grid in y
72 */
73void
75 int robo_x,
76 int robo_y)
77{
78 occ_grid_ = occ_grid;
79 robot_pos_.x = robo_x;
80 robot_pos_.y = robo_y;
81}
82
83/* ************************************************************************** */
84/* *********************** U P D A T E ************************* */
85/* ************************************************************************** */
86
87/** Calculate here your desired settings. What you desire is checked afterwards to the current
88 * settings of the physical boundaries, but take care also.
89 *
90 * How you do this is up to you, but be careful, our hardware is expensive!!!!
91 *
92 * All values of the other drive modes inherited by the abstract-drive-mode are
93 * non-valid, because search did not succeed or should not have been called!
94 * So do not use them. Instead here you use the m_pLaser!
95 *
96 * Afterwards filled should be:
97 *
98 * proposed_ --> Desired translation and rotation speed
99 *
100 * Those values are questioned after an update() was called.
101 */
102void
104{
105 static unsigned int cell_cost_occ = occ_grid_->get_cell_costs().occ;
106
107 // This is only called, if we recently stopped...
108 if (cfg_write_spam_debug_) {
109 logger_->log_debug("EscapePotentialFieldOmniDriveModule",
110 "EscapePotentialFieldOmniDriveModule( update ): Calculating ESCAPING...");
111 }
112
114
115 int cell_height = occ_grid_->get_cell_height();
116 int cell_width = occ_grid_->get_cell_width();
117 int width = occ_grid_->get_width();
118 int height = occ_grid_->get_height();
119
120 polar_coord_2d_t target;
121 target.r = 0.1f;
122 target.phi = M_PI;
123 float target_x = 0.f;
124 float target_y = 0.f;
125
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;
131
132 if (dx != 0.f && dy != 0.f) {
133 float factor = 1.f / ((dx * dx + dy * dy) * (dx * dx + dy * dy));
134
135 target_x -= factor * dx;
136 target_y -= factor * dy;
137 }
138 }
139 }
140 }
141
142 target.r = sqrt(target_x * target_x + target_y * target_y);
143 target.phi = atan2(target_y, target_x);
144
145 if (cfg_write_spam_debug_) {
146 logger_->log_debug("EscapePotentialFieldOmniDriveModule",
147 "Target vector: phi: %f\t%f",
148 target.phi,
149 target.r);
150 }
151
152 // decide route
153 float angle_difference = M_PI_2 - 0.2f;
154 float angle = normalize_mirror_rad(target.phi);
155 float angle_abs = fabs(angle);
156
157 bool turn = true;
158 float turn_direction = 0.f;
159 float drive_part_x = 1.f;
160 float drive_part_y = 0.f;
161
162 if (angle_abs > angle_difference) { // just turn
163 turn = true;
164
165 turn_ = 1;
166 if (angle < 0.f) {
167 turn_direction = -1.f;
168 } else {
169 turn_direction = 1.f;
170 }
171 } else { // drive
172 turn = false;
173
174 drive_part_x = std::cos(target.phi);
175 drive_part_y = std::sin(target.phi);
176 }
177
178 if (turn) {
179 if (cfg_write_spam_debug_) {
180 logger_->log_debug("EscapePotentialFieldOmniDriveModule", "Turn %f", turn_direction);
181 }
182 proposed_.rot = turn_direction * max_rot_;
183 } else {
184 if (cfg_write_spam_debug_) {
185 logger_->log_debug("EscapePotentialFieldOmniDriveModule",
186 "Drive ( %f , %f )",
187 drive_part_x,
188 drive_part_y);
189 }
190 proposed_.x = drive_part_x * max_trans_;
191 proposed_.y = drive_part_y * max_trans_;
192 if (fabs(turn_direction) > 0.2f) {
193 proposed_.rot = turn_direction * max_rot_;
194 }
195 }
196}
197
198/* ************************************************************************** */
199/* *********************** Private Methods ************************* */
200/* ************************************************************************** */
201
202} // namespace fawkes
This is the base class which calculates drive modes.
float max_trans_
The maximum translation speed.
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.
Interface for configuration handling.
Definition: config.h:68
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
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.
void set_grid_information(LaserOccupancyGrid *occ_grid, int robo_x, int robo_y)
This function sets the Grid information for one escape step.
EscapePotentialFieldOmniDriveModule(Logger *logger, Configuration *config)
Constructor.
This OccGrid is derived by the Occupancy Grid originally from Andreas Strack, but modified for speed ...
Definition: og_laser.h:47
colli_cell_cost_t get_cell_costs() const
Get cell costs.
Definition: og_laser.cpp:471
Interface for logging.
Definition: logger.h:42
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
Probability get_prob(int x, int y)
Get the occupancy probability of a cell.
int get_cell_width()
Get the cell width (in cm)
int get_height()
Get the height of the grid.
int get_width()
Get the width of the grid.
int get_cell_height()
Get the cell height (in cm)
Fawkes library namespace.
float normalize_mirror_rad(float angle_rad)
Normalize angle in radian between -PI (inclusive) and PI (exclusive).
Definition: angle.h:72
unsigned int occ
The cost for an occupied cell.
Definition: types.h:51
float x
Translation in x-direction.
Definition: types.h:61
float y
Translation in y-direction.
Definition: types.h:62
float rot
Rotation around z-axis.
Definition: types.h:63
int x
x coordinate
Definition: types.h:43
int y
y coordinate
Definition: types.h:44
Polar coordinates.
Definition: types.h:96
float phi
angle
Definition: types.h:98
float r
distance
Definition: types.h:97