Fawkes API  Fawkes Development Version
forward_omni_drive_mode.cpp
1 
2 /***************************************************************************
3  * forward_omni_drive_mode.cpp - Implementation of drive-mode "forward"
4  *
5  * Created: Fri Oct 18 15:16:23 2013
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 "forward_omni_drive_mode.h"
23 #include <utils/math/common.h>
24 #include <utils/math/angle.h>
25 
26 #include <cmath>
27 
28 namespace fawkes
29 {
30 #if 0 /* just to make Emacs auto-indent happy */
31 }
32 #endif
33 
34 /** @class ForwardOmniDriveModule <plugins/colli/drive_modes/forward_drive_mode.h>
35  * This is the SlowForward drive-module, for slow forward only movements.
36  */
37 
38 /** Constructor.
39  * @param logger The fawkes logger
40  * @param config The fawkes configuration
41  */
43  : AbstractDriveMode(logger, config)
44 {
45  logger_->log_debug("ForwardOmniDriveModule", "(Constructor): Entering...");
47 
48  max_trans_ = config_->get_float( "/plugins/colli/drive_mode/normal/max_trans" );
49  max_rot_ = config_->get_float( "/plugins/colli/drive_mode/normal/max_rot" );
50 
51  logger_->log_debug("ForwardOmniDriveModule", "(Constructor): Exiting...");
52 }
53 
54 
55 /** Descturctor. Destruct your local values here. */
57 {
58  logger_->log_debug("ForwardOmniDriveModule", "(Destructor): Entering...");
60  logger_->log_debug("ForwardOmniDriveModule", "(Destructor): Exiting...");
61 }
62 
63 void
64 ForwardOmniDriveModule::calculate_rotation(float ori_alpha_target, float ori_alpha_next_target,
65  float dist_to_target, float angle_allowed_to_next_target)
66 {
67  // first calculate desired angle
68  float des_alpha;
69  if ( ! std::isfinite( ori_alpha_next_target ) ) {
70  des_alpha = ori_alpha_target;
71  } else {
72  float angle_min = ori_alpha_target - angle_allowed_to_next_target;
73  float angle_max = ori_alpha_target + angle_allowed_to_next_target;
74  des_alpha = normalize_mirror_rad( std::max( angle_min, std::min(ori_alpha_target, angle_max) ) );
75  }
76 
77  // then choose rotation speed, depending on desired angle
78  const float _TURN_MAX_SPEED_LIMIT_ = M_PI_4;
79  if ( des_alpha > _TURN_MAX_SPEED_LIMIT_ ) {
81  } else if ( des_alpha < -_TURN_MAX_SPEED_LIMIT_ ) {
83  } else {
84  proposed_.rot = des_alpha * ( max_rot_ / _TURN_MAX_SPEED_LIMIT_ );
85  }
86 }
87 
88 void
89 ForwardOmniDriveModule::calculate_translation(float dist_to_target, float ori_alpha_target, float dec_factor)
90 {
91  float part_x = 0;
92  float part_y = 0;
93  if ( ! (local_target_.x == 0 && local_target_.y == 0) ) {
94  part_x = local_target_.x / (fabs(local_target_.x) + fabs(local_target_.y));
95  part_y = local_target_.y / (fabs(local_target_.x) + fabs(local_target_.y));
96  }
97  proposed_.x = part_x * max_trans_ * dec_factor;
98  proposed_.y = part_y * max_trans_ * dec_factor;
99 
100  // Check translation limits
101  if ( proposed_.x < 0. || fabs(ori_alpha_target) >= M_PI_2 - 0.2 ) {
102  proposed_.x = 0.;
103  proposed_.y = 0.;
104  }
105 }
106 
107 /* ************************************************************************** */
108 /* *********************** U P D A T E ************************* */
109 /* ************************************************************************** */
110 
111 /** Calculate here your desired settings. What you desire is checked afterwards to the current
112  * settings of the physical boundaries, but take care also.
113  *
114  * How you do this is up to you, but be careful, our hardware is expensive!!!!
115  *
116  * Available are:
117  *
118  * target_ --> current target coordinates to drive to
119  * robot_ --> current robot coordinates
120  * robot_vel_ --> current Motor velocities
121  *
122  * local_target_ --> our local target found by the search component we want to reach
123  * local_trajec_ --> The point we would collide with, if we would drive WITHOUT Rotation
124  *
125  * orient_at_target_ --> Do we have to orient ourself at the target?
126  * stop_at_target_ --> Do we have to stop really ON the target?
127  *
128  * Afterwards filled should be:
129  *
130  * proposed_ --> Desired translation and rotation speed
131  *
132  * Those values are questioned after an update() was called.
133  */
134 void
136 {
137  proposed_.x = 0.f;
138  proposed_.rot = 0.f;
139 
140  float dist_to_target = sqrt( sqr(local_target_.x) + sqr(local_target_.y) );
141  float alpha_target = normalize_mirror_rad( atan2( local_target_.y, local_target_.x ) );
142  float alpha_next_target = angle_distance_signed(robot_.ori, target_.ori);
143 
144  // last time border check............. IMPORTANT!!!
145  // because the motorinstructor just tests robots physical borders.
146  if ( dist_to_target < 0.04 ) {
147  proposed_.x = proposed_.y = proposed_.rot = 0.f;
148 
149  } else {
150  float angle_tollerance = M_PI_4;
151  calculate_rotation(alpha_target, alpha_next_target, dist_to_target, angle_tollerance / 2.);
152 
153  float dec_factor = 1;
154  if ( fabs(alpha_target) >= angle_tollerance ) { // if we need to turn a lot => drive slower
155  dec_factor = 0.5;
156  }
157 
158  calculate_translation(dist_to_target, alpha_target, dec_factor);
159 
160  if ( stop_at_target_ ) {
161  float target_rel = std::sqrt( sqr(target_.x - robot_.x) + sqr(target_.y - robot_.y) );
162  float robo_trans = std::sqrt( sqr(robot_vel_.x) + sqr(robot_vel_.y) );
163  float proposed_trans = std::sqrt( sqr(proposed_.x) + sqr(proposed_.y) );
164  float target_trans = guarantee_trans_stop(target_rel, robo_trans, proposed_trans);
165 
166  float des = fabs(target_trans / proposed_trans);
167  if ( proposed_trans == 0 ) { des = 0; }
168 
169  proposed_.x *= des;
170  proposed_.y *= des;
171  }
172  }
173 }
174 
175 } // namespace fawkes
float x
Translation in x-direction.
Definition: types.h:61
cart_coord_2d_t local_target_
local target
Logger * logger_
The fawkes logger.
Fawkes library namespace.
colli_trans_rot_t proposed_
proposed translation and rotation for next timestep
bool stop_at_target_
flag if stopping on or after target
float max_trans_
The maximum translation speed.
float y
Translation in y-direction.
Definition: types.h:62
float angle_distance_signed(float angle_from, float angle_to)
Determines the signed distance between from "angle_from" to "angle_to" provided as radians...
Definition: angle.h:140
float rot
Rotation around z-axis.
Definition: types.h:63
double sqr(double x)
Fast square multiplication.
Definition: common.h:37
float max_rot_
The maximum rotation speed.
field_pos_t target_
current target
NavigatorInterface::DriveMode drive_mode_
the drive mode name
colli_trans_rot_t robot_vel_
current robot velocity
float normalize_mirror_rad(float angle_rad)
Normalize angle in radian between -PI (inclusive) and PI (exclusive).
Definition: angle.h:75
float guarantee_trans_stop(float distance, float current_trans, float desired_trans)
Get velocity that guarantees a stop for a given distance.
float x
x coordinate in meters
Definition: types.h:111
virtual void update()
Calculate here your desired settings.
field_pos_t robot_
current robot pos
Configuration * config_
The fawkes configuration.
This is the base class which calculates drive modes.
float y
y coordinate
Definition: types.h:61
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
ForwardOmniDriveModule(Logger *logger, Configuration *config)
Constructor.
float ori
orientation
Definition: types.h:113
float y
y coordinate in meters
Definition: types.h:112
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.
Interface for logging.
Definition: logger.h:34
float x
x coordinate
Definition: types.h:60