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

Fixed Moving average for acorn approach

Browse Source
This commit is contained in:
Pimpest
2026-04-13 18:26:17 +02:00
parent 6d3794d154
commit 6a8d7176f4
3 changed files with 20 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files

32
toid_behaviors/src/approach_acorns.cpp
View File

@@ -111,21 +111,6 @@ void ApproachAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg)
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;
marker.header = pose_global.header;
marker.pose = pose_global.pose;
marker.type = visualization_msgs::msg::Marker::ARROW;
marker.scale.x = 0.02;
marker.scale.y = 0.02;
marker.scale.z = 0.02;
marker.color.a = 1.0;
marker.color.r = 1.0;
marker.color.g = 0;
marker.color.b = 0;
target_pose_pub_->publish(marker);
}
yaw = angles::normalize_angle(yaw);
@@ -136,6 +121,23 @@ void ApproachAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg)
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_) {
visualization_msgs::msg::Marker marker;
marker.lifetime.sec = 1.0;
marker.header = pose_global.header;
marker.pose = new_target_pose_;
marker.type = visualization_msgs::msg::Marker::ARROW;
marker.scale.x = 0.02;
marker.scale.y = 0.02;
marker.scale.z = 0.02;
marker.color.a = 1.0;
marker.color.r = 1.0;
marker.color.g = 0;
marker.color.b = 0;
target_pose_pub_->publish(marker);
}
}
ResultStatus ApproachAcorns::onStart(