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

wip-behaviors #3

Merged
Pimpest merged 86 commits from wip-behaviors into main 2026-05-28 06:21:26 +00:00
Conversation 0 Commits 86 Files Changed 129 +20 -18
3 changed files with 20 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 6a8d7176f4 - Show all commits

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

@@ -53,7 +53,7 @@ protected:
rclcpp::Subscription<PoseStamped>::SharedPtr acorn_pose_sub_; rclcpp::Subscription<PoseStamped>::SharedPtr acorn_pose_sub_;
rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::Marker>::SharedPtr target_pose_pub_; rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::Marker>::SharedPtr target_pose_pub_;
std::shared_mutex mutex_; std::shared_mutex mutex_;
RollingAverage avg_ = RollingAverage(5); RollingAverage avg_ = RollingAverage(10);
//Goal //Goal
geometry_msgs::msg::Pose new_target_pose_; 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) Pose2D push(geometry_msgs::msg::Pose & pose)
{ {
if(size_ > 0) { if(size_ == 0) {
return {}; return {};
} }
if (size_ == data_count_) { if (size_-1 == data_count_) {
accum_x_ -= poses_[front_idx_].position.x; accum_x_ -= poses_[front_idx_].position.x;
accum_y_ -= poses_[front_idx_].position.y; accum_y_ -= poses_[front_idx_].position.y;
accum_theta_ -= tf2::getYaw(poses_[front_idx_].orientation); accum_theta_ -= tf2::getYaw(poses_[front_idx_].orientation);

22
toid_behaviors/src/approach_acorns.cpp
View File

@@ -111,11 +111,22 @@ void ApproachAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg)
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);
std::lock_guard _lock(mutex_);
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_angle_ = c;
if(debug_marker_) { if(debug_marker_) {
visualization_msgs::msg::Marker marker; visualization_msgs::msg::Marker marker;
marker.lifetime.sec = 1.0; marker.lifetime.sec = 1.0;
marker.header = pose_global.header; marker.header = pose_global.header;
marker.pose = pose_global.pose; marker.pose = new_target_pose_;
marker.type = visualization_msgs::msg::Marker::ARROW; marker.type = visualization_msgs::msg::Marker::ARROW;
marker.scale.x = 0.02; marker.scale.x = 0.02;
marker.scale.y = 0.02; marker.scale.y = 0.02;
@@ -127,15 +138,6 @@ void ApproachAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg)
target_pose_pub_->publish(marker); 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_.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( ResultStatus ApproachAcorns::onStart(