Fawkes API  Fawkes Development Version
escape_potential_field_drive_mode.cpp
1 
2 /***************************************************************************
3  * escape_potential_field_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_drive_mode.h"
23 
24 #include "../common/types.h"
25 #include "../search/og_laser.h"
26 
27 #include <utils/math/angle.h>
28 
29 namespace fawkes {
30 
31 /** @class EscapePotentialFieldDriveModule <plugins/colli/drive_modes/escape_potential_field_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("EscapeDriveModule", "(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("EscapeDriveModule", "(Constructor): Exiting...");
57 }
58 
59 /** Destruct your local values here.
60  */
62 {
63  logger_->log_debug("EscapeDriveModule", "(Destructor): Entering...");
64  logger_->log_debug("EscapeDriveModule", "(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  */
73 void
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  */
102 void
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("EscapeDriveModule", "EscapeDriveModule( update ): Calculating ESCAPING...");
110  }
111 
112  proposed_.x = proposed_.y = proposed_.rot = 0.f;
113 
114  int cell_height = occ_grid_->get_cell_height();
115  int cell_width = occ_grid_->get_cell_width();
116  int width = occ_grid_->get_width();
117  int height = occ_grid_->get_height();
118 
119  polar_coord_2d_t target;
120  target.r = 0.1f;
121  target.phi = M_PI;
122  float target_x = 0.f;
123  float target_y = 0.f;
124 
125  for (int posX = 0; posX < width; ++posX) {
126  for (int posY = 0; posY < height; ++posY) {
127  if (occ_grid_->get_prob(posX, posY) >= cell_cost_occ) {
128  float dx = float(posX - robot_pos_.x) * cell_height / 100;
129  float dy = float(posY - robot_pos_.y) * cell_width / 100;
130 
131  if (dx != 0.f && dy != 0.f) {
132  float factor = 1.f / ((dx * dx + dy * dy) * (dx * dx + dy * dy));
133 
134  target_x -= factor * dx;
135  target_y -= factor * dy;
136  }
137  }
138  }
139  }
140 
141  target.r = sqrt(target_x * target_x + target_y * target_y);
142  target.phi = atan2(target_y, target_x);
143 
144  if (cfg_write_spam_debug_) {
145  logger_->log_debug("EscapePotentialFieldDriveModule",
146  "Target vector: phi: %f\t%f",
147  target.phi,
148  target.r);
149  }
150 
151  // decide route
152  float angle_difference = 0.2f;
153  float angle = normalize_mirror_rad(target.phi);
154  float angle_abs = fabs(angle);
155 
156  bool turn = true;
157  float turn_direction = 0.f;
158  float drive_direction = 0.f;
159 
160  if (angle_abs > angle_difference /* && angle_abs < M_PI - angle_difference*/) { //just turn
161  turn = true;
162 
163  // if (angle_abs <= (M_PI_2 + 0.2 * turn_)) { //turn to 0
164  turn_ = 1;
165  if (angle < 0.f) {
166  turn_direction = -1.f;
167  } else {
168  turn_direction = 1.f;
169  }
170  /* } else { //turn to PI
171  turn_ = -1.0;
172  if (angle < 0) {
173  turn_direction = 1.f;
174  } else {
175  turn_direction = -1.f;
176  }
177  }*/
178  } else { //drive
179  turn = false;
180 
181  // if (angle_abs <= angle_difference) { //forward
182  drive_direction = 1.f;
183  /* } else if (angle_abs >= M_PI - angle_difference){ //backward
184  drive_direction = -1.f;
185  } else {
186  drive_direction = 0.f;
187  logger_->log_error("EscapePotentialFieldDriveModule","Should drive, but don't know the direction");
188  }*/
189  }
190 
191  if (turn) {
192  if (cfg_write_spam_debug_) {
193  logger_->log_debug("EscapePotentialFieldDriveModule", "Turn %f", turn_direction);
194  }
195  proposed_.rot = turn_direction * max_rot_;
196  } else {
197  if (cfg_write_spam_debug_) {
198  logger_->log_debug("EscapePotentialFieldDriveModule", "Drive %f", drive_direction);
199  }
200  proposed_.x = drive_direction * max_trans_;
201  }
202 
203  // if ( angle_abs > angle_difference && angle_abs < M_PI - angle_difference ) { //just turn
204  // if (angle_abs <= M_PI_2) { //turn to 0
205  // logger_->log_debug("EscapePotentialFieldDriveModule","Turn to 0");
206  // if (angle < 0) {
207  // logger_->log_debug("EscapePotentialFieldDriveModule","negative");
208  // proposed_.rot = -max_rot_;
209  // } else {
210  // logger_->log_debug("EscapePotentialFieldDriveModule","positive");
211  // proposed_.rot = max_rot_;
212  // }
213  // } else { //turn to PI
214  // logger_->log_debug("EscapePotentialFieldDriveModule","Turn to PI");
215  // if (angle < 0) {
216  // logger_->log_debug("EscapePotentialFieldDriveModule","positive");
217  // proposed_.rot = max_rot_;
218  // } else {
219  // logger_->log_debug("EscapePotentialFieldDriveModule","negative");
220  // proposed_.rot = -max_rot_;
221  // }
222  // }
223  // } else { //drive and turn
224  // if (angle_abs <= angle_difference) { //forward
225  // logger_->log_debug("EscapePotentialFieldDriveModule","Drive forward");
226  // m_ProposedTranslation = max_trans_;
227  // } else if (angle_abs >= M_PI - angle_difference){ //backward
228  // logger_->log_debug("EscapePotentialFieldDriveModule","Drive backward");
229  // m_ProposedTranslation = -max_trans_;
230  // } else {
231  // logger_->log_debug("EscapePotentialFieldDriveModule","Should drive, but don't know the direction");
232  // }
233  // }
234 }
235 
236 /* ************************************************************************** */
237 /* *********************** Private Methods ************************* */
238 /* ************************************************************************** */
239 
240 } // namespace fawkes
float x
Translation in x-direction.
Definition: types.h:61
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.
Definition: og_laser.cpp:471
float y
Translation in y-direction.
Definition: types.h:62
float rot
Rotation around z-axis.
Definition: types.h:63
Polar coordinates.
Definition: types.h:95
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 ...
Definition: og_laser.h:46
float normalize_mirror_rad(float angle_rad)
Normalize angle in radian between -PI (inclusive) and PI (exclusive).
Definition: angle.h:72
virtual void update()
Calculate here your desired settings.
unsigned int occ
The cost for an occupied cell.
Definition: types.h:51
int y
y coordinate
Definition: types.h:44
Configuration * config_
The fawkes configuration.
This is the base class which calculates drive modes.
float r
distance
Definition: types.h:97
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.
float phi
angle
Definition: types.h:98
Interface for configuration handling.
Definition: config.h:64
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
int x
x coordinate
Definition: types.h:43
Interface for logging.
Definition: logger.h:41