various fixes

2026-04-15 15:32:28 +02:00
parent 4d4c598ce9
commit 980598718d
7 changed files with 107 additions and 52 deletions
--- a/toid_behaviors/src/rotate_acorns.cpp
+++ b/toid_behaviors/src/rotate_acorns.cpp
@@ -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) {