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

various fixes

Browse Source
This commit is contained in:
Pimpest
2026-04-15 15:32:28 +02:00
parent 4d4c598ce9
commit 980598718d
7 changed files with 107 additions and 52 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
toid_behaviors/src/approach_acorns.cpp
View File

@@ -68,7 +68,7 @@ void ApproachAcorns::activateCB()
auto node = node_.lock();
using namespace std::placeholders;
acorn_pose_sub_ = node->create_subscription<PoseStamped>(
"closest_acorn", rclcpp::QoS(2), std::bind(&ApproachAcorns::acorn_position_cb, this, _1));
"closest_acorn", rclcpp::QoS(2).keep_last(2), std::bind(&ApproachAcorns::acorn_position_cb, this, _1));
target_pose_pub_->on_activate();
distance_ = 1.0;
}
@@ -110,8 +110,8 @@ 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;
pose_global.pose.position.x += std::cos(yaw) * -0.35 - std::sin(yaw) * 0.010;
pose_global.pose.position.y += std::sin(yaw) * -0.35 + std::cos(yaw) * 0.010;
tf2::convert(tf2::Quaternion::createFromRPY(0, 0, yaw), pose_global.pose.orientation);
yaw = angles::normalize_angle(yaw);

3
toid_behaviors/src/rotate_acorns.cpp
View File

@@ -43,7 +43,7 @@ void RotateAcorns::activateCB()
auto node = node_.lock();
using namespace std::placeholders;
acorn_pose_sub_ = node->create_subscription<PoseStamped>(
"closest_acorn", rclcpp::QoS(2), std::bind(&RotateAcorns::acorn_position_cb, this, _1));
"closest_acorn", rclcpp::QoS(2).keep_last(2), std::bind(&RotateAcorns::acorn_position_cb, this, _1));
distance_ = 1.0;
}
@@ -101,6 +101,7 @@ ResultStatus RotateAcorns::onStart(
initial_direction_ = (command->min_angle >= 0.0) ? 1.0 : -1.0;
max_angular_speed_ = command->max_speed;
mode_ = command->mode;
distance_ = 1.0;
avg_.reset();
if (command->max_speed == 0) {