Pimpest/toid
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Revized simple rotate for easier redability

Browse Source
This commit is contained in:
Pimpest
2026-03-04 20:35:46 +01:00
parent cfba1f948d
commit f5e08389e2
1 changed files with 10 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files

25
toid_behaviors/src/simple_rotate.cpp
View File

@@ -53,30 +53,25 @@ ResultStatus SimpleRotateBehavior::updateVel(
geometry_msgs::msg::Twist & out_vel)
{
const double current_yaw = tf2::getYaw(pose.orientation);
const double angle_change = angles::normalize_angle(current_yaw - last_angle_);
const double angle_change = angles::shortest_angular_distance(last_angle_ , current_yaw);
last_angle_ = current_yaw;
double err = angles::shortest_angular_distance(current_yaw, target_angle_);
double sign = (err < 0)? -1.0 : 1.0;
err = std::abs(err);
if (min_turn_angle_ > 0.0) {
min_turn_angle_ = fmax(0.0, min_turn_angle_ - initial_direction_ * angle_change);
min_turn_angle_ = std::max(0.0, min_turn_angle_ - initial_direction_ * angle_change);
err = std::max( initial_direction_ * sign * err, 0.0);
err = std::max(min_turn_angle_, err);
sign = initial_direction_;
}
const double upper_vel_ = angular_speed_ + max_angular_accel_ * control_duration_;
const double lower_vel_ = angular_speed_ - max_angular_accel_ * control_duration_;
double anglular_distance_to_target =
angles::shortest_angular_distance(current_yaw, target_angle_);
double sign = (anglular_distance_to_target >= 0.0) ? 1.0 : -1.0;
if (min_turn_angle_ > 0.0 && initial_direction_ != sign) {
anglular_distance_to_target = angles::two_pi_complement(anglular_distance_to_target);
sign = initial_direction_;
}
const double angular_distance_to_heading =
sign * fmax(std::fabs(anglular_distance_to_target), min_turn_angle_);
const double speed_until_overshoot =
std::sqrt(2.0 * max_angular_accel_ * std::fabs(angular_distance_to_heading));
std::sqrt(2.0 * max_angular_accel_ * std::fabs(err));
const double requested_speed = sign * std::min(speed_until_overshoot, max_angular_speed_);
const double speed = std::clamp(requested_speed, lower_vel_, upper_vel_);