22 #include "forward_omni_drive_mode.h"
24 #include <utils/math/angle.h>
25 #include <utils/math/common.h>
40 : AbstractDriveMode(logger, config)
60 ForwardOmniDriveModule::calculate_rotation(
float ori_alpha_target,
61 float ori_alpha_next_target,
63 float angle_allowed_to_next_target)
67 if (!std::isfinite(ori_alpha_next_target)) {
68 des_alpha = ori_alpha_target;
70 float angle_min = ori_alpha_target - angle_allowed_to_next_target;
71 float angle_max = ori_alpha_target + angle_allowed_to_next_target;
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_) {
87 ForwardOmniDriveModule::calculate_translation(
float dist_to_target,
88 float ori_alpha_target,
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) {