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 
29 namespace 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  */
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("EscapePotentialFieldOmniDriveModule",
110  "EscapePotentialFieldOmniDriveModule( update ): Calculating ESCAPING...");
111  }
112 
113  proposed_.x = proposed_.y = proposed_.rot = 0.f;
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
fawkes::NavigatorInterface::ESCAPE
@ ESCAPE
Escape constant.
Definition: NavigatorInterface.h:71
fawkes::AbstractDriveMode::max_rot_
float max_rot_
The maximum rotation speed.
Definition: abstract_drive_mode.h:112
fawkes::AbstractDriveMode::drive_mode_
NavigatorInterface::DriveMode drive_mode_
the drive mode name
Definition: abstract_drive_mode.h:106
fawkes::AbstractDriveMode::max_trans_
float max_trans_
The maximum translation speed.
Definition: abstract_drive_mode.h:111
fawkes::polar_coord_2d_t::r
float r
distance
Definition: types.h:97
fawkes::AbstractDriveMode::proposed_
colli_trans_rot_t proposed_
proposed translation and rotation for next timestep
Definition: abstract_drive_mode.h:104
fawkes::colli_trans_rot_t::rot
float rot
Rotation around z-axis.
Definition: types.h:70
fawkes::polar_coord_2d_t
Polar coordinates.
Definition: types.h:95
fawkes::Configuration::get_bool
virtual bool get_bool(const char *path)=0
fawkes::OccupancyGrid::get_width
int get_width()
Get the width of the grid.
Definition: occupancygrid.cpp:85
fawkes::colli_trans_rot_t::y
float y
Translation in y-direction.
Definition: types.h:69
fawkes::colli_cell_cost_t::occ
unsigned int occ
The cost for an occupied cell.
Definition: types.h:58
fawkes::colli_trans_rot_t::x
float x
Translation in x-direction.
Definition: types.h:68
fawkes::OccupancyGrid::get_prob
Probability get_prob(int x, int y)
Get the occupancy probability of a cell.
Definition: occupancygrid.cpp:170
fawkes::EscapePotentialFieldOmniDriveModule::EscapePotentialFieldOmniDriveModule
EscapePotentialFieldOmniDriveModule(Logger *logger, Configuration *config)
Constructor.
Definition: escape_potential_field_omni_drive_mode.cpp:45
fawkes
fawkes::AbstractDriveMode::config_
Configuration * config_
The fawkes configuration.
Definition: abstract_drive_mode.h:109
fawkes::EscapePotentialFieldOmniDriveModule::~EscapePotentialFieldOmniDriveModule
~EscapePotentialFieldOmniDriveModule()
Destruct your local values here.
Definition: escape_potential_field_omni_drive_mode.cpp:66
fawkes::AbstractDriveMode::logger_
Logger * logger_
The fawkes logger.
Definition: abstract_drive_mode.h:108
fawkes::EscapePotentialFieldOmniDriveModule::set_grid_information
void set_grid_information(LaserOccupancyGrid *occ_grid, int robo_x, int robo_y)
This function sets the Grid information for one escape step.
Definition: escape_potential_field_omni_drive_mode.cpp:79
fawkes::point_struct::y
int y
y coordinate
Definition: types.h:44
fawkes::normalize_mirror_rad
float normalize_mirror_rad(float angle_rad)
Normalize angle in radian between -PI (inclusive) and PI (exclusive).
Definition: angle.h:78
fawkes::LaserOccupancyGrid::get_cell_costs
colli_cell_cost_t get_cell_costs() const
Get cell costs.
Definition: og_laser.cpp:478
fawkes::Configuration::get_float
virtual float get_float(const char *path)=0
fawkes::EscapePotentialFieldOmniDriveModule::update
virtual void update()
Calculate here your desired settings.
Definition: escape_potential_field_omni_drive_mode.cpp:108
fawkes::point_struct::x
int x
x coordinate
Definition: types.h:43
fawkes::LaserOccupancyGrid
Definition: og_laser.h:53
fawkes::Logger::log_debug
virtual void log_debug(const char *component, const char *format,...)=0
fawkes::polar_coord_2d_t::phi
float phi
angle
Definition: types.h:98
fawkes::OccupancyGrid::get_cell_width
int get_cell_width()
Get the cell width (in cm)
Definition: occupancygrid.cpp:67
fawkes::OccupancyGrid::get_height
int get_height()
Get the height of the grid.
Definition: occupancygrid.cpp:94
fawkes::OccupancyGrid::get_cell_height
int get_cell_height()
Get the cell height (in cm)
Definition: occupancygrid.cpp:76