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

Added start plug action server

Browse Source
This commit is contained in:
Pimpest
2026-04-13 13:24:52 +02:00
parent 5624c44574
commit 6633ef41fa
8 changed files with 134 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
toid_behaviors/src/approach_acorns.cpp
View File

@@ -89,9 +89,10 @@ void ApproachAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg)
RCLCPP_ERROR_ONCE(logger_, "Transform timeout [showing once]...");
return;
}
double x = pose_local.pose.position.x;
double y = pose_local.pose.position.y;
if (x * x + y * y > distance_ + 0.01) {
if (x * x + y * y > distance_ + 0.005) {
return;
}
@@ -104,10 +105,12 @@ void ApproachAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg)
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);
if(debug_marker_) {
visualization_msgs::msg::Marker marker;
marker.lifetime.sec = 1.0;
@@ -124,10 +127,15 @@ void ApproachAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg)
target_pose_pub_->publish(marker);
}
yaw = angles::normalize_angle(yaw);
std::lock_guard _lock(mutex_);
auto[a,b,c] = avg_.push(pose_global.pose);
distance_ = x * x + y * y;
new_target_pose_ = pose_global.pose;
new_target_angle_ = yaw;
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;
}
ResultStatus ApproachAcorns::onStart(
@@ -146,6 +154,8 @@ ResultStatus ApproachAcorns::onStart(
target_sign_ = backwards_ ? -1.0 : 1.0;
max_vel_speed_ = command->max_speed;
avg_.reset();
if (command->max_speed == 0) {
auto node = node_.lock();
node->get_parameter(behavior_name_ + ".max_vel_speed", max_vel_speed_);