diff --git a/toid_behaviors/src/move_coords.cpp b/toid_behaviors/src/move_coords.cpp index 7a6a259..2cd985c 100644 --- a/toid_behaviors/src/move_coords.cpp +++ b/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_; diff --git a/toid_behaviors/src/simple_rotate.cpp b/toid_behaviors/src/simple_rotate.cpp index f31e36d..4c75f60 100644 --- 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}; } diff --git a/toid_behaviors/src/simple_translate_x.cpp b/toid_behaviors/src/simple_translate_x.cpp index 1dcd53f..e38a431 100644 --- a/toid_behaviors/src/simple_translate_x.cpp +++ b/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_; diff --git a/toid_msgs/action/SimpleMoveCoords.action b/toid_msgs/action/SimpleMoveCoords.action index 65df8ac..f3b3913 100644 --- a/toid_msgs/action/SimpleMoveCoords.action +++ b/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 diff --git a/toid_msgs/action/SimpleRotate.action b/toid_msgs/action/SimpleRotate.action index 0d6a1db..e403b9c 100644 --- a/toid_msgs/action/SimpleRotate.action +++ b/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 diff --git a/toid_msgs/action/SimpleTranslateX.action b/toid_msgs/action/SimpleTranslateX.action index 59545bb..5469ae8 100644 --- a/toid_msgs/action/SimpleTranslateX.action +++ b/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