Added support for setting max velocities on send_goal

2026-03-07 12:32:34 +01:00
parent 5777a0e804
commit 77cb2c5573
6 changed files with 24 additions and 1 deletions
--- a/toid_behaviors/src/simple_rotate.cpp
+++ b/toid_behaviors/src/simple_rotate.cpp
@@ -39,12 +39,17 @@ ResultStatus SimpleRotateBehavior::onStart(
  target_angle_ = angles::normalize_angle(command->angle);
  min_turn_angle_ = abs(command->min_angle);
  initial_direction_ = (command->min_angle >= 0.0) ? 1.0 : -1.0;
+  max_angular_speed_ = command->max_speed;
+
+  if(command->max_speed == 0) {
+    auto node = node_.lock();
+    node->get_parameter(behavior_name_ + ".max_angular_vel", max_angular_speed_);
+  }

  last_angle_ = tf2::getYaw(pose.orientation);

  angular_speed_ = vel.angular.z;
  last_time_ = clock_->now().seconds();
-
  return ResultStatus{Status::SUCCEEDED};
 }