22 #include "forward_omni_drive_mode.h" 23 #include <utils/math/common.h> 24 #include <utils/math/angle.h> 64 ForwardOmniDriveModule::calculate_rotation(
float ori_alpha_target,
float ori_alpha_next_target,
65 float dist_to_target,
float angle_allowed_to_next_target)
69 if ( ! std::isfinite( ori_alpha_next_target ) ) {
70 des_alpha = ori_alpha_target;
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) ) );
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_ ) {
89 ForwardOmniDriveModule::calculate_translation(
float dist_to_target,
float ori_alpha_target,
float dec_factor)
101 if (
proposed_.
x < 0. || fabs(ori_alpha_target) >= M_PI_2 - 0.2 ) {
146 if ( dist_to_target < 0.04 ) {
150 float angle_tollerance = M_PI_4;
151 calculate_rotation(alpha_target, alpha_next_target, dist_to_target, angle_tollerance / 2.);
153 float dec_factor = 1;
154 if ( fabs(alpha_target) >= angle_tollerance ) {
158 calculate_translation(dist_to_target, alpha_target, dec_factor);
166 float des = fabs(target_trans / proposed_trans);
167 if ( proposed_trans == 0 ) { des = 0; }
~ForwardOmniDriveModule()
Descturctor.
float x
Translation in x-direction.
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.
float angle_distance_signed(float angle_from, float angle_to)
Determines the signed distance between from "angle_from" to "angle_to" provided as radians...
float rot
Rotation around z-axis.
double sqr(double x)
Fast square multiplication.
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).
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
virtual void update()
Calculate here your desired settings.
field_pos_t robot_
current robot pos
Moving not allowed constant.
Configuration * config_
The fawkes configuration.
This is the base class which calculates drive modes.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
ForwardOmniDriveModule(Logger *logger, Configuration *config)
Constructor.
float y
y coordinate in meters
Interface for configuration handling.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.