Added support for setting max velocities on send_goal

toid_behaviors/src/move_coords.cpp

@@ -67,6 +67,12 @@ ResultStatus MoveCoords::onStart(
   target_pose_.orientation = tf2::toMsg(tf2::Quaternion::createFromRPY(0, 0, command->theta));
   target_angle_ = command->theta;
   target_sign_ = backwards_ ? -1.0 : 1.0;
+  max_vel_speed_ = command->max_speed;
+
+  if(command->max_speed == 0) {
+    auto node = node_.lock();
+    node->get_parameter(behavior_name_ + ".max_vel_speed", max_vel_speed_);
+  }
 
   scl.k_phi = k_phi_;
   scl.k_delta = k_delta_;

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};
 }
 

toid_behaviors/src/simple_translate_x.cpp

@@ -46,6 +46,12 @@ ResultStatus SimpleTranslateXBehavior::onStart(
   target_distance_ = command->distance;
   target_angle_ = tf2::getYaw(pose.orientation);
   target_sign_ = (target_distance_ < 0) ? -1.0 : 1.0;
+  max_vel_speed_ = command->max_speed;
+
+  if(command->max_speed == 0) {
+    auto node = node_.lock();
+    node->get_parameter(behavior_name_ + ".max_vel_speed", max_vel_speed_);
+  }
 
   target_distance_ *= target_sign_;
 

toid_msgs/action/SimpleMoveCoords.action

@@ -5,6 +5,8 @@ float64 y
 float64 theta
 uint8 backwards 0
 uint8 mode 0
+
+float64 max_speed
 ---
 uint16 NONE=0
 uint16 TF_ERROR=1

toid_msgs/action/SimpleRotate.action

@@ -3,6 +3,8 @@ uint8 IGNORE_OBSTACLES=1
 float64 angle
 float64 min_angle 0
 uint8 mode 0
+
+float64 max_speed 0
 ---
 uint16 NONE=0
 uint16 TF_ERROR=1

toid_msgs/action/SimpleTranslateX.action

@@ -2,6 +2,8 @@ uint8 IGNORE_OBSTACLES=1
 
 float64 distance
 uint8 mode 0
+
+float64 max_speed 0
 ---
 uint16 NONE=0
 uint16 TF_ERROR=1