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

wip-behaviors #3

Merged
Pimpest merged 86 commits from wip-behaviors into main 2026-05-28 06:21:26 +00:00
Conversation 0 Commits 86 Files Changed 129 +10 -15
1 changed files with 10 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit f5e08389e2 - Show all commits

25
toid_behaviors/src/simple_rotate.cpp
View File

@@ -53,30 +53,25 @@ ResultStatus SimpleRotateBehavior::updateVel(
geometry_msgs::msg::Twist & out_vel) geometry_msgs::msg::Twist & out_vel)
{ {
const double current_yaw = tf2::getYaw(pose.orientation); 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; 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) { 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 upper_vel_ = angular_speed_ + max_angular_accel_ * control_duration_;
const double lower_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 = 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 requested_speed = sign * std::min(speed_until_overshoot, max_angular_speed_);
const double speed = std::clamp(requested_speed, lower_vel_, upper_vel_); const double speed = std::clamp(requested_speed, lower_vel_, upper_vel_);