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 #include "../search/og_laser.h"
24 #include "../common/types.h"
25 
26 #include <utils/math/angle.h>
27 
28 namespace fawkes
29 {
30 #if 0 /* just to make Emacs auto-indent happy */
31 }
32 #endif
33 
34 /** @class EscapePotentialFieldOmniDriveModule <plugins/colli/drive_modes/escape_potential_field_omni_drive_mode.h>
35  * Class Escape-Drive-Module. This module is called, if an escape is neccessary.
36  * It should try to maximize distance to the disturbing obstacle.
37  */
38 
39 /** Constructor.
40  * @param logger The fawkes logger
41  * @param config The fawkes configuration
42  */
44  : AbstractDriveMode(logger, config)
45 {
46  logger_->log_debug("EscapePotentialFieldOmniDriveModule", "(Constructor): Entering...");
48  occ_grid_ = NULL;
49  robot_pos_.x = 0.f;
50  robot_pos_.y = 0.f;
51  turn_ = 0;
52 
53  max_trans_ = config_->get_float( "/plugins/colli/drive_mode/escape/max_trans" );
54  max_rot_ = config_->get_float( "/plugins/colli/drive_mode/escape/max_rot" );
55 
56  cfg_write_spam_debug_ = config_->get_bool( "/plugins/colli/write_spam_debug" );
57 
58  logger_->log_debug("EscapePotentialFieldOmniDriveModule", "(Constructor): Exiting...");
59 }
60 
61 
62 /** Destruct your local values here.
63  */
65 {
66  logger_->log_debug("EscapePotentialFieldOmniDriveModule", "(Destructor): Entering...");
67  logger_->log_debug("EscapePotentialFieldOmniDriveModule", "(Destructor): Exiting...");
68 }
69 
70 /**
71  * This function sets the Grid information for one escape step
72  * @param occ_grid pointer to the occ_grid
73  * @param robo_x robot position on the grid in x
74  * @param robo_y robot position on the grid in y
75  */
76 void
78 {
79  occ_grid_ = occ_grid;
80  robot_pos_.x = robo_x;
81  robot_pos_.y = robo_y;
82 }
83 
84 
85 /* ************************************************************************** */
86 /* *********************** U P D A T E ************************* */
87 /* ************************************************************************** */
88 
89 /** Calculate here your desired settings. What you desire is checked afterwards to the current
90  * settings of the physical boundaries, but take care also.
91  *
92  * How you do this is up to you, but be careful, our hardware is expensive!!!!
93  *
94  * All values of the other drive modes inherited by the abstract-drive-mode are
95  * non-valid, because search did not succeed or should not have been called!
96  * So do not use them. Instead here you use the m_pLaser!
97  *
98  * Afterwards filled should be:
99  *
100  * proposed_ --> Desired translation and rotation speed
101  *
102  * Those values are questioned after an update() was called.
103  */
104 void
106 {
107  static unsigned int cell_cost_occ = occ_grid_->get_cell_costs().occ;
108 
109  // This is only called, if we recently stopped...
110  if (cfg_write_spam_debug_) {
111  logger_->log_debug("EscapePotentialFieldOmniDriveModule", "EscapePotentialFieldOmniDriveModule( update ): Calculating ESCAPING...");
112  }
113 
114  proposed_.x = proposed_.y = proposed_.rot = 0.f;
115 
116  int cell_height = occ_grid_->get_cell_height();
117  int cell_width = occ_grid_->get_cell_width();
118  int width = occ_grid_->get_width();
119  int height = occ_grid_->get_height();
120 
121  polar_coord_2d_t target;
122  target.r = 0.1f;
123  target.phi = M_PI;
124  float target_x = 0.f;
125  float target_y = 0.f;
126 
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;
132 
133  if (dx != 0.f && dy != 0.f) {
134  float factor = 1.f / ( (dx*dx + dy*dy) * (dx*dx + dy*dy) );
135 
136  target_x -= factor * dx;
137  target_y -= factor * dy;
138  }
139  }
140  }
141  }
142 
143  target.r = sqrt( target_x*target_x + target_y*target_y );
144  target.phi = atan2(target_y, target_x);
145 
146  if (cfg_write_spam_debug_) {
147  logger_->log_debug("EscapePotentialFieldOmniDriveModule","Target vector: phi: %f\t%f", target.phi, target.r);
148  }
149 
150  // decide route
151  float angle_difference = M_PI_2 - 0.2f;
152  float angle = normalize_mirror_rad(target.phi);
153  float angle_abs = fabs( angle );
154 
155  bool turn = true;
156  float turn_direction = 0.f;
157  float drive_part_x = 1.f;
158  float drive_part_y = 0.f;
159 
160  if ( angle_abs > angle_difference ) { // just turn
161  turn = true;
162 
163  turn_ = 1;
164  if (angle < 0.f) {
165  turn_direction = -1.f;
166  } else {
167  turn_direction = 1.f;
168  }
169  } else { // drive
170  turn = false;
171 
172  drive_part_x = std::cos( target.phi );
173  drive_part_y = std::sin( target.phi );
174  }
175 
176  if ( turn ) {
177  if (cfg_write_spam_debug_) {
178  logger_->log_debug("EscapePotentialFieldOmniDriveModule","Turn %f", turn_direction);
179  }
180  proposed_.rot = turn_direction * max_rot_;
181  } else {
182  if (cfg_write_spam_debug_) {
183  logger_->log_debug("EscapePotentialFieldOmniDriveModule","Drive ( %f , %f )", drive_part_x, drive_part_y);
184  }
185  proposed_.x = drive_part_x * max_trans_;
186  proposed_.y = drive_part_y * max_trans_;
187  if ( fabs(turn_direction) > 0.2f ) {
188  proposed_.rot = turn_direction * max_rot_;
189  }
190  }
191 }
192 
193 
194 /* ************************************************************************** */
195 /* *********************** Private Methods ************************* */
196 /* ************************************************************************** */
197 
198 
199 } // 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.
float max_trans_
The maximum translation speed.
colli_cell_cost_t get_cell_costs() const
Get cell costs.
Definition: og_laser.cpp:442
EscapePotentialFieldOmniDriveModule(Logger *logger, Configuration *config)
Constructor.
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:85
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:49
float normalize_mirror_rad(float angle_rad)
Normalize angle in radian between -PI (inclusive) and PI (exclusive).
Definition: angle.h:75
virtual void update()
Calculate here your desired settings.
unsigned int occ
The cost for an occupied cell.
Definition: types.h:52
int y
y coordinate
Definition: types.h:42
Configuration * config_
The fawkes configuration.
This is the base class which calculates drive modes.
float r
distance
Definition: types.h:86
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
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:87
Interface for configuration handling.
Definition: config.h:67
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
int x
x coordinate
Definition: types.h:41
Interface for logging.
Definition: logger.h:34