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

2
toid_behaviors/include/toid_behaviors/approach_acorns.hpp
View File

@@ -53,7 +53,7 @@ protected:
rclcpp::Subscription<PoseStamped>::SharedPtr acorn_pose_sub_;
rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::Marker>::SharedPtr target_pose_pub_;
std::shared_mutex mutex_;
RollingAverage avg_ = RollingAverage(5);
RollingAverage avg_ = RollingAverage(10);
//Goal
geometry_msgs::msg::Pose new_target_pose_;

4
toid_behaviors/include/toid_behaviors/rolling_average.hpp
View File

@@ -18,10 +18,10 @@ public:
Pose2D push(geometry_msgs::msg::Pose & pose)
{
if(size_ > 0) {
if(size_ == 0) {
return {};
}
if (size_ == data_count_) {
if (size_-1 == data_count_) {
accum_x_ -= poses_[front_idx_].position.x;
accum_y_ -= poses_[front_idx_].position.y;
accum_theta_ -= tf2::getYaw(poses_[front_idx_].orientation);