Fawkes API Fawkes Development Version
select_drive_mode.cpp
1
2/***************************************************************************
3 * select_drive_mode.cpp - Class that selects drive-mode from a collection
4 *
5 * Created: Fri Oct 18 15:16:23 2013
6 * Copyright 2002 Stefan Jacobs
7 * 2013-2014 Bahram Maleki-Fard
8 * 2014 Tobias Neumann
9 ****************************************************************************/
10
11/* This program is free software; you can redistribute it and/or modify
12 * it under the terms of the GNU General Public License as published by
13 * the Free Software Foundation; either version 2 of the License, or
14 * (at your option) any later version.
15 *
16 * This program is distributed in the hope that it will be useful,
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19 * GNU Library General Public License for more details.
20 *
21 * Read the full text in the LICENSE.GPL file in the doc directory.
22 */
23
24#include "select_drive_mode.h"
25
26// INCLUDE HERE YOUR DRIVE MODES!!!
27#include "backward_drive_mode.h"
28#include "biward_drive_mode.h"
29#include "escape_drive_mode.h"
30#include "escape_potential_field_drive_mode.h"
31#include "escape_potential_field_omni_drive_mode.h"
32#include "forward_drive_mode.h"
33#include "forward_omni_drive_mode.h"
34#include "stop_drive_mode.h"
35// YOUR CHANGES SHOULD END HERE!!!
36
37#include "../search/og_laser.h"
38
39#include <config/config.h>
40#include <interfaces/MotorInterface.h>
41#include <interfaces/NavigatorInterface.h>
42#include <logging/logger.h>
43#include <utils/math/angle.h>
44
45namespace fawkes {
46
47/** @class SelectDriveMode <plugins/colli/drive_modes/select_drive_mode.h>
48 * This class selects the correct drive mode and calls the appopriate drive component
49 */
50
51/** Constructor.
52 * @param motor The motor interface
53 * @param target The "colli target" NavigatorInterface
54 * @param logger The fawkes logger
55 * @param config The fawkes configuration
56 * @param escape_mode The chosen escape mode
57 */
59 NavigatorInterface *target,
60 Logger * logger,
61 Configuration * config,
62 colli_escape_mode_t escape_mode)
63: logger_(logger),
64 config_(config),
65 if_colli_target_(target),
66 if_motor_(motor),
67 cfg_escape_mode_(escape_mode),
68 escape_flag_(0) // no escaping at the beginning
69{
70 logger_->log_debug("SelectDriveMode", "(Constructor): Entering");
71
72 drive_modes_.clear();
73 proposed_.x = proposed_.y = proposed_.rot = 0.f;
74
75 std::string drive_restriction = config->get_string("/plugins/colli/drive_mode/restriction");
76
77 if (drive_restriction == "omnidirectional") {
78 drive_restriction_ = colli_drive_restriction_t::omnidirectional;
79 } else if (drive_restriction == "differential") {
80 drive_restriction_ = colli_drive_restriction_t::differential;
81 } else {
82 throw fawkes::Exception("Drive restriction '%s' is unknown", drive_restriction.c_str());
83 }
84
85 logger_->log_debug("SelectDriveMode", "(Constructor): Creating Drive Mode Objects");
86
87 // Add generic drive modes
88 drive_modes_.push_back(new StopDriveModule(logger_, config_));
89
90 // Add specific drive modes
91 if (drive_restriction_ == colli_drive_restriction_t::omnidirectional) {
92 load_drive_modes_omnidirectional();
93 } else {
94 load_drive_modes_differential();
95 }
96
97 logger_->log_debug("SelectDriveMode", "(Constructor): Exiting");
98}
99
100/** Desctructor. */
102{
103 logger_->log_debug("SelectDriveMode", "(Destructor): Entering");
104 for (unsigned int i = 0; i < drive_modes_.size(); i++)
105 delete drive_modes_[i];
106 logger_->log_debug("SelectDriveMode", "(Destructor): Exiting");
107}
108
109void
110SelectDriveMode::load_drive_modes_differential()
111{
112 // escape drive mode
113 if (cfg_escape_mode_ == colli_escape_mode_t::potential_field) {
114 drive_modes_.push_back(new EscapePotentialFieldDriveModule(logger_, config_));
115 } else if (cfg_escape_mode_ == colli_escape_mode_t::basic) {
116 drive_modes_.push_back(new EscapeDriveModule(logger_, config_));
117 } else {
118 logger_->log_error("SelectDriveMode", "Unknown escape drive mode. Using basic as default");
119 drive_modes_.push_back(new EscapeDriveModule(logger_, config_));
120 }
121
122 // forward drive mode (have to remember for biward driving!
123 ForwardDriveModule *forward = new ForwardDriveModule(logger_, config_);
124 drive_modes_.push_back(forward);
125
126 // backward drive mode (have to remember for biward driving!
127 BackwardDriveModule *backward = new BackwardDriveModule(logger_, config_);
128 drive_modes_.push_back(backward);
129
130 // biward drive mode (takes both forward and backward drive modes as argument!
131 drive_modes_.push_back(new BiwardDriveModule(forward, backward, logger_, config_));
132}
133
134void
135SelectDriveMode::load_drive_modes_omnidirectional()
136{
137 // escape drive mode
138 if (cfg_escape_mode_ == colli_escape_mode_t::potential_field) {
139 drive_modes_.push_back(new EscapePotentialFieldOmniDriveModule(logger_, config_));
140 } else if (cfg_escape_mode_ == colli_escape_mode_t::basic) {
141 // CAUTION: This is an differential drive mode!
142 drive_modes_.push_back(new EscapeDriveModule(logger_, config_));
143 } else {
144 logger_->log_error("SelectDriveMode",
145 "Unknown escape drive mode. "
146 "Using potential field omni as default");
147 drive_modes_.push_back(new EscapePotentialFieldOmniDriveModule(logger_, config_));
148 }
149
150 ForwardOmniDriveModule *forward = new ForwardOmniDriveModule(logger_, config_);
151 drive_modes_.push_back(forward);
152}
153
154/** Set local target point before update!
155 * @param x x-coordinate
156 * @param y y-coordinate
157 */
158void
160{
161 local_target_.x = x;
162 local_target_.y = y;
163}
164
165/** Set local target trajectory before update!
166 * @param x x-coordinate
167 * @param y y-coordinate
168 */
169void
171{
172 local_trajec_.x = x;
173 local_trajec_.y = y;
174}
175
176/** Returns the proposed x translation which was previously calculated in update()
177 * @return The proposed translation
178 */
179float
181{
182 return proposed_.x;
183}
184
185/** Returns the proposed y translation which was previously calculated in update()
186 * @return The proposed translation
187 */
188float
190{
191 return proposed_.y;
192}
193
194/** Returns the proposed rotation which was previously calculated in update()
195 * @return The proposed rotation
196 */
197float
199{
200 return proposed_.rot;
201}
202
203/**
204 * search for the escape drive mode and hands over the given information to the escape drive mode
205 * This should just be called if potential-field-escape mode is used!
206 * @param occ_grid pointer to the occ_grid
207 * @param robo_x robot position on the grid in x
208 * @param robo_y robot position on the grid in y
209 */
210void
212{
213 for (unsigned int i = 0; i < drive_modes_.size(); i++) {
214 // drive mode checking
215 if (drive_modes_[i]->get_drive_mode_name() == NavigatorInterface::ESCAPE) {
217 static_cast<EscapePotentialFieldDriveModule *>(drive_modes_[i]);
218 dm->set_grid_information(occ_grid, robo_x, robo_y);
219 return;
220 }
221 }
222 logger_->log_error("SelectDriveMode", "Can't find escape drive mode to set grid information");
223}
224
225/**
226 * search for the escape drive mode and hands over the given information to the escape drive mode
227 * This should just be called if basic-escape mode is used!
228 * @param laser_points vector of laser points
229 */
230void
231SelectDriveMode::set_laser_data(std::vector<polar_coord_2d_t> &laser_points)
232{
233 for (unsigned int i = 0; i < drive_modes_.size(); i++) {
234 // drive mode checking
235 if (drive_modes_[i]->get_drive_mode_name() == NavigatorInterface::ESCAPE) {
236 EscapeDriveModule *dm = static_cast<EscapeDriveModule *>(drive_modes_[i]);
237 dm->set_laser_data(laser_points);
238 return;
239 }
240 }
241 logger_->log_error("SelectDriveMode", "Can't find escape drive mode to set laser information");
242}
243
244/* ****************************************************************************** */
245/* ****************************************************************************** */
246/* U P D A T E */
247/* ****************************************************************************** */
248/* ****************************************************************************** */
249
250/** Pick the drive-mode that should be used and calculate the proposed translation
251 * and rotation for the current target (which is set by set_local_target() and
252 * set_local_trajec(), so make sure to call them beforehand).
253 * update() has to be called before the proposed values are fetched.
254 * @param escape Set to true if we want to enter escape-mode
255 */
256void
258{
259 AbstractDriveMode *drive_mode = NULL;
260 proposed_.x = proposed_.y = proposed_.rot = 0.f;
261
262 // choose the correct drive mode!
264 if (escape == true) {
265 if (escape_flag_ == 0 && if_motor_->des_vx() != 0.f && if_motor_->des_vy() != 0.f
266 && if_motor_->des_omega() != 0.f) {
268 // we have not yet stopped!
269
270 } else {
271 // we have stopped recently, so do escape!
272 escape_flag_ = 1;
273 desired_mode = NavigatorInterface::ESCAPE;
274 }
275
276 } else {
277 escape_flag_ = 0;
278 desired_mode = if_colli_target_->drive_mode();
279 }
280
281 // now search this specific drive mode in the list
282 for (unsigned int i = 0; i < drive_modes_.size(); i++) {
283 // error checking
284 if (drive_modes_[i]->get_drive_mode_name() == desired_mode && drive_mode != 0) {
285 logger_->log_error("SelectDriveMode",
286 "Error while selecting drive mode. There is more than one mode with the "
287 "same name!!! Stopping!");
288 drive_mode = 0;
289 break;
290 }
291
292 // drive mode checking
293 if (drive_modes_[i]->get_drive_mode_name() == desired_mode && drive_mode == 0) {
294 drive_mode = drive_modes_[i];
295 }
296 }
297
298 if (drive_mode == 0) {
299 // invalid pointer
300 logger_->log_error("SelectDriveMode", "INVALID DRIVE MODE POINTER, stopping!");
301 proposed_.x = proposed_.y = proposed_.rot = 0.f;
302
303 } else {
304 // valid drive mode!
305 // set the values for the drive mode
306 drive_mode->set_current_robo_pos(if_motor_->odometry_position_x(),
307 if_motor_->odometry_position_y(),
309
310 drive_mode->set_current_robo_speed(if_motor_->vx(), if_motor_->vy(), if_motor_->omega());
311
312 drive_mode->set_current_target(if_colli_target_->dest_x(),
313 if_colli_target_->dest_y(),
314 if_colli_target_->dest_ori());
315
316 drive_mode->set_local_target(local_target_.x, local_target_.y);
317 drive_mode->set_local_trajec(local_trajec_.x, local_trajec_.y);
318 drive_mode->set_current_colli_mode(if_colli_target_->orientation_mode(),
319 if_colli_target_->is_stop_at_target());
320
321 // update the drive mode
322 drive_mode->update();
323
324 // get the values from the drive mode
325 proposed_.x = drive_mode->get_proposed_trans_x();
326 proposed_.y = drive_mode->get_proposed_trans_y();
327 proposed_.rot = drive_mode->get_proposed_rot();
328
329 // recheck with targetobj maximum settings
330 if ((if_colli_target_->max_velocity() != 0.0)
331 && (fabs(proposed_.x) > fabs(if_colli_target_->max_velocity()))) {
332 if (proposed_.x > 0.0)
333 proposed_.x = if_colli_target_->max_velocity();
334 else
335 proposed_.x = -if_colli_target_->max_velocity();
336 }
337
338 if ((if_colli_target_->max_velocity() != 0.0)
339 && (fabs(proposed_.y) > fabs(if_colli_target_->max_velocity()))) {
340 if (proposed_.y > 0.0)
341 proposed_.y = if_colli_target_->max_velocity();
342 else
343 proposed_.y = -if_colli_target_->max_velocity();
344 }
345
346 if ((if_colli_target_->max_rotation() != 0.0)
347 && (fabs(proposed_.rot) > fabs(if_colli_target_->max_rotation()))) {
348 if (proposed_.rot > 0.0)
349 proposed_.rot = if_colli_target_->max_rotation();
350 else
351 proposed_.rot = -if_colli_target_->max_rotation();
352 }
353 }
354}
355
356} // namespace fawkes
This is the base class which calculates drive modes.
void set_current_target(float x, float y, float ori)
Sets the current target.
void set_current_colli_mode(NavigatorInterface::OrientationMode orient, bool stop)
Set the colli mode values for each drive mode.
float get_proposed_trans_x()
Returns the proposed x translation.
float get_proposed_rot()
Returns the proposed rotatio.
void set_local_target(float x, float y)
Set the local targetpoint found by the search.
float get_proposed_trans_y()
Returns the proposed y translation.
void set_current_robo_speed(float x, float y, float rot)
Sets the current robo speed.
void set_local_trajec(float x, float y)
Set the local trajectory point found by the search.
void set_current_robo_pos(float x, float y, float ori)
Sets the current robo position.
virtual void update()=0
Calculate the proposed settings which are asked for afterwards.
Interface for configuration handling.
Definition: config.h:68
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
Class Escape-Drive-Module.
void set_laser_data(std::vector< polar_coord_2d_t > &laser_points)
This function sets the laser points for one escape round.
void set_grid_information(LaserOccupancyGrid *occ_grid, int robo_x, int robo_y)
This function sets the Grid information for one escape step.
Base class for exceptions in Fawkes.
Definition: exception.h:36
This OccGrid is derived by the Occupancy Grid originally from Andreas Strack, but modified for speed ...
Definition: og_laser.h:47
Interface for logging.
Definition: logger.h:42
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
MotorInterface Fawkes BlackBoard Interface.
float omega() const
Get omega value.
float odometry_position_y() const
Get odometry_position_y value.
float odometry_orientation() const
Get odometry_orientation value.
float odometry_position_x() const
Get odometry_position_x value.
float des_vy() const
Get des_vy value.
float vx() const
Get vx value.
float vy() const
Get vy value.
float des_omega() const
Get des_omega value.
float des_vx() const
Get des_vx value.
NavigatorInterface Fawkes BlackBoard Interface.
float dest_x() const
Get dest_x value.
float dest_y() const
Get dest_y value.
float max_velocity() const
Get max_velocity value.
bool is_stop_at_target() const
Get stop_at_target value.
float max_rotation() const
Get max_rotation value.
@ MovingNotAllowed
Moving not allowed constant.
DriveMode drive_mode() const
Get drive_mode value.
float dest_ori() const
Get dest_ori value.
OrientationMode orientation_mode() const
Get orientation_mode value.
void set_grid_information(LaserOccupancyGrid *occ_grid, int robo_x, int robo_y)
search for the escape drive mode and hands over the given information to the escape drive mode This s...
SelectDriveMode(MotorInterface *motor, NavigatorInterface *colli_target, Logger *logger, Configuration *config, colli_escape_mode_t escape_mode=colli_escape_mode_t::basic)
Constructor.
float get_proposed_trans_y()
Returns the proposed translation. After an update.
void set_laser_data(std::vector< fawkes::polar_coord_2d_t > &laser_points)
search for the escape drive mode and hands over the given information to the escape drive mode This s...
float get_proposed_rot()
Returns the proposed rotation. After an update.
void set_local_target(float x, float y)
Set local target point before update!
float get_proposed_trans_x()
Returns the proposed translation. After an update.
void set_local_trajec(float x, float y)
Set local trajectory point before update!
void update(bool escape=false)
Has to be called before the proposed values are called.
Stop-Drive-Module.
Fawkes library namespace.
colli_escape_mode_t
Colli Escape modes.
Definition: types.h:67
float normalize_mirror_rad(float angle_rad)
Normalize angle in radian between -PI (inclusive) and PI (exclusive).
Definition: angle.h:72
float y
y coordinate
Definition: types.h:67
float x
x coordinate
Definition: types.h:66
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