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

Added rotate acorn and misc changes to approach acorns

Browse Source
This commit is contained in:
Pimpest
2026-04-15 11:17:36 +02:00
parent 773411951d
commit 1558ca1217
6 changed files with 344 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files

45
toid_behaviors/src/approach_acorns.cpp
View File

@@ -73,9 +73,10 @@ void ApproachAcorns::activateCB()
distance_ = 1.0;
}
void ApproachAcorns::deactivateCB() {
acorn_pose_sub_.reset();
target_pose_pub_->on_deactivate();
void ApproachAcorns::deactivateCB()
{
acorn_pose_sub_.reset();
target_pose_pub_->on_deactivate();
}
void ApproachAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg)
@@ -96,30 +97,34 @@ void ApproachAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg)
return;
}
// Smooth out approach
if (x * x + y * y < 0.42) {
return;
}
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) {
yaw += M_PI;
}
yaw += M_PI/2;
yaw += M_PI / 2;
pose_global.pose.position.x += std::cos(yaw) * -0.35 - std::sin(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);
pose_global.pose.position.x += std::cos(yaw) * -0.35 - std::sin(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);
yaw = angles::normalize_angle(yaw);
std::lock_guard _lock(mutex_);
auto[a,b,c] = avg_.push(pose_global.pose);
auto [a, b, c] = avg_.push(pose_global.pose);
distance_ = x * x + y * y;
new_target_pose_.position.x = a;
new_target_pose_.position.y = b;
tf2::convert(tf2::Quaternion::createFromRPY(0,0, c), new_target_pose_.orientation);
new_target_pose_.position.x = a;
new_target_pose_.position.y = b;
tf2::convert(tf2::Quaternion::createFromRPY(0, 0, c), new_target_pose_.orientation);
new_target_angle_ = c;
if(debug_marker_) {
if (debug_marker_) {
visualization_msgs::msg::Marker marker;
marker.lifetime.sec = 1.0;
marker.header = pose_global.header;
@@ -134,14 +139,12 @@ void ApproachAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg)
marker.color.b = 0;
target_pose_pub_->publish(marker);
}
}
ResultStatus ApproachAcorns::onStart(
const std::shared_ptr<const MoveAction::Goal> command, const geometry_msgs::msg::Pose &,
const geometry_msgs::msg::Twist & vel)
{
std::lock_guard _lock(mutex_);
distance_ = 1.0;
new_target_pose_.position.x = command->x;
@@ -235,7 +238,6 @@ ResultStatus ApproachAcorns::updateVel(
const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist &,
geometry_msgs::msg::Twist & out_vel)
{
{
std::lock_guard _lock(mutex_);
target_pose_ = new_target_pose_;
@@ -247,7 +249,7 @@ ResultStatus ApproachAcorns::updateVel(
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) {
if (dist_left <= 0.001 && std::fabs(angle_dist) <= 0.030) {
out_vel.linear.x = 0;
out_vel.angular.z = 0;
return ResultStatus{Status::SUCCEEDED};
@@ -279,6 +281,13 @@ ResultStatus ApproachAcorns::updateVel(
last_speed_ = out_vel.linear.x;
return ResultStatus{Status::RUNNING};
}
if (dist_left <= 0.001) {
out_vel.linear.x = 0;
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};
}
scl_.v_linear_max = target_sign_ * velocityTarget(dist_left);
scl_.calculate_vel(target_pose_, pose, out_vel, backwards_);