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

approach acorn modifications

Browse Source
This commit is contained in:
Pimpest
2026-04-13 18:33:16 +02:00
parent 6a8d7176f4
commit 773411951d
1 changed files with 4 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
toid_behaviors/src/approach_acorns.cpp
View File

@@ -96,7 +96,6 @@ void ApproachAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg)
return; return;
} }
double yaw = angles::normalize_angle(tf2::getYaw(pose_global.pose.orientation)); double yaw = angles::normalize_angle(tf2::getYaw(pose_global.pose.orientation));
if(tf2::getYaw(pose_local.pose.orientation) > 0) { if(tf2::getYaw(pose_local.pose.orientation) > 0) {
@@ -110,8 +109,6 @@ void ApproachAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg)
pose_global.pose.position.y += std::sin(yaw) * -0.35 + std::cos(yaw) * + 0.005; pose_global.pose.position.y += std::sin(yaw) * -0.35 + 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);
std::lock_guard _lock(mutex_); std::lock_guard _lock(mutex_);
@@ -247,15 +244,15 @@ ResultStatus ApproachAcorns::updateVel(
double dist_left = distanceToTarget(pose, target_pose_.position, target_angle_, backwards_); double dist_left = distanceToTarget(pose, target_pose_.position, target_angle_, backwards_);
if (dist_left <= 0.001) { const double current_yaw = tf2::getYaw(pose.orientation);
double angle_dist = angles::shortest_angular_distance(current_yaw, target_angle_);
if (dist_left <= 0.001 && std::fabs(dist_left) <= 0.005) {
out_vel.linear.x = 0; out_vel.linear.x = 0;
out_vel.angular.z = 0; out_vel.angular.z = 0;
return ResultStatus{Status::SUCCEEDED}; return ResultStatus{Status::SUCCEEDED};
} }
const double current_yaw = tf2::getYaw(pose.orientation);
double angle_dist = angles::shortest_angular_distance(current_yaw, target_angle_);
geometry_msgs::msg::Pose last_ok_pose; geometry_msgs::msg::Pose last_ok_pose;
if (collisionDetection(pose, last_ok_pose)) { if (collisionDetection(pose, last_ok_pose)) {
dist_left = distanceToTarget( dist_left = distanceToTarget(