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 
24 #include <utils/math/angle.h>
25 #include <utils/math/common.h>
26 
27 #include <cmath>
28 
29 namespace fawkes {
30 
31 /** @class ForwardOmniDriveModule <plugins/colli/drive_modes/forward_drive_mode.h>
32  * This is the SlowForward drive-module, for slow forward only movements.
33  */
34 
35 /** Constructor.
36  * @param logger The fawkes logger
37  * @param config The fawkes configuration
38  */
40 : AbstractDriveMode(logger, config)
41 {
42  logger_->log_debug("ForwardOmniDriveModule", "(Constructor): Entering...");
44 
45  max_trans_ = config_->get_float("/plugins/colli/drive_mode/normal/max_trans");
46  max_rot_ = config_->get_float("/plugins/colli/drive_mode/normal/max_rot");
47 
48  logger_->log_debug("ForwardOmniDriveModule", "(Constructor): Exiting...");
49 }
50 
51 /** Descturctor. Destruct your local values here. */
53 {
54  logger_->log_debug("ForwardOmniDriveModule", "(Destructor): Entering...");
56  logger_->log_debug("ForwardOmniDriveModule", "(Destructor): Exiting...");
57 }
58 
59 void
60 ForwardOmniDriveModule::calculate_rotation(float ori_alpha_target,
61  float ori_alpha_next_target,
62  float dist_to_target,
63  float angle_allowed_to_next_target)
64 {
65  // first calculate desired angle
66  float des_alpha;
67  if (!std::isfinite(ori_alpha_next_target)) {
68  des_alpha = ori_alpha_target;
69  } else {
70  float angle_min = ori_alpha_target - angle_allowed_to_next_target;
71  float angle_max = ori_alpha_target + angle_allowed_to_next_target;
72  des_alpha = normalize_mirror_rad(std::max(angle_min, std::min(ori_alpha_target, angle_max)));
73  }
74 
75  // then choose rotation speed, depending on desired angle
76  const float _TURN_MAX_SPEED_LIMIT_ = M_PI_4;
77  if (des_alpha > _TURN_MAX_SPEED_LIMIT_) {
79  } else if (des_alpha < -_TURN_MAX_SPEED_LIMIT_) {
81  } else {
82  proposed_.rot = des_alpha * (max_rot_ / _TURN_MAX_SPEED_LIMIT_);
83  }
84 }
85 
86 void
87 ForwardOmniDriveModule::calculate_translation(float dist_to_target,
88  float ori_alpha_target,
89  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) {
168  des = 0;
169  }
170 
171  proposed_.x *= des;
172  proposed_.y *= des;
173  }
174  }
175 }
176 
177 } // 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:134
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:72
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:126
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:67
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:128
float y
y coordinate in meters
Definition: types.h:127
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.
Interface for logging.
Definition: logger.h:41
float x
x coordinate
Definition: types.h:66