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

add backwards to behavior tree action

Browse Source
This commit is contained in:
Pimpest
2026-03-20 17:05:30 +01:00
parent 9010bac9a0
commit 8f4f7dba8b
4 changed files with 94 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
toid_behaviors/src/move_coords.cpp
View File

@@ -95,10 +95,6 @@ ResultStatus MoveCoords::updateVel(
const double current_yaw = tf2::getYaw(pose.orientation);
double angle_dist = angles::shortest_angular_distance(current_yaw, target_angle_);
if (backwards_) {
angle_dist = angles::two_pi_complement(angle_dist);
}
const double dx = target_pose_.position.x - pose.position.x;
const double dy = target_pose_.position.y - pose.position.y;
@@ -120,7 +116,7 @@ ResultStatus MoveCoords::updateVel(
if (dist_left <= 0.02) {
out_vel.linear.x = std::clamp(target_sign_ * vel, lower_bound, upper_bound);
out_vel.angular.z = std::clamp(target_sign_ * kp_ * angle_dist, -max_angular_speed_, max_angular_speed_);
out_vel.angular.z = std::clamp(kp_ * angle_dist, -max_angular_speed_, max_angular_speed_);
last_speed_ = out_vel.linear.x;
return ResultStatus{Status::RUNNING};
}