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 +4 -4
1 changed files with 4 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 81ebd8b7a2 - Show all commits

8
toid_behaviors/src/rotate_acorns.cpp
View File

@@ -78,8 +78,8 @@ void RotateAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg)
yaw += M_PI/2; yaw += M_PI/2;
pose_global.pose.position.x += std::cos(yaw) * 0.0 - std::sin(yaw) * + 0.005; pose_global.pose.position.x += std::cos(yaw) * 0.0 - std::sin(yaw) * - 0.005;
pose_global.pose.position.y += std::sin(yaw) * 0.0 + std::cos(yaw) * + 0.005; pose_global.pose.position.y += std::sin(yaw) * 0.0 + std::cos(yaw) * - 0.005;
tf2::convert(tf2::Quaternion::createFromRPY(0,0,yaw ), pose_global.pose.orientation); tf2::convert(tf2::Quaternion::createFromRPY(0,0,yaw ), pose_global.pose.orientation);
yaw = angles::normalize_angle(yaw); yaw = angles::normalize_angle(yaw);
@@ -143,7 +143,7 @@ double RotateAcorns::calc_speed(
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_;
const double speed_until_overshoot = std::sqrt(2.0 * max_angular_accel_ * std::fabs(err)); const double speed_until_overshoot = std::sqrt(2.0 * max_angular_decel_ * 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_);
@@ -171,7 +171,7 @@ ResultStatus RotateAcorns::updateVel(
speed = calc_speed(err, sign, angular_speed); speed = calc_speed(err, sign, angular_speed);
} }
if (min_turn_angle_ == 0 && std::fabs(current_yaw - target_angle_) < 0.01) { if (min_turn_angle_ == 0 && std::fabs(current_yaw - target_angle_) < 0.004) {
return ResultStatus{Status::SUCCEEDED}; return ResultStatus{Status::SUCCEEDED};
} }