From 54e01f6f671b5bf6b398d2d00c6484cf02721ad7 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 4 Apr 2024 22:24:56 +0000 Subject: [PATCH 1/3] Fix diff_drive docs --- diff_drive_controller/doc/userdoc.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/diff_drive_controller/doc/userdoc.rst b/diff_drive_controller/doc/userdoc.rst index 70d0d7ca5c..897a7b3d9c 100644 --- a/diff_drive_controller/doc/userdoc.rst +++ b/diff_drive_controller/doc/userdoc.rst @@ -46,7 +46,7 @@ Subscribers ,,,,,,,,,,,, ~/cmd_vel [geometry_msgs/msg/TwistStamped] - Velocity command for the controller, if ``use_stamped_vel=true``. The controller extracts the x component of the linear velocity and the z component of the angular velocity. Velocities on other components are ignored. + Velocity command for the controller. The controller extracts the x component of the linear velocity and the z component of the angular velocity. Velocities on other components are ignored. Publishers From cb2471e4cf5fc39871b3eba3f8b5b32324f4d5af Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 4 Apr 2024 22:25:23 +0000 Subject: [PATCH 2/3] Deprecate non-stamped twist --- tricycle_controller/doc/userdoc.rst | 18 ++++++++++-------- .../src/tricycle_controller.cpp | 3 +++ .../src/tricycle_controller_parameter.yaml | 2 +- .../test/config/test_tricycle_controller.yaml | 1 - 4 files changed, 14 insertions(+), 10 deletions(-) diff --git a/tricycle_controller/doc/userdoc.rst b/tricycle_controller/doc/userdoc.rst index 991627eca2..30973762fd 100644 --- a/tricycle_controller/doc/userdoc.rst +++ b/tricycle_controller/doc/userdoc.rst @@ -10,14 +10,6 @@ Controller for mobile robots with a single double-actuated wheel, including trac Input for control are robot base_link twist commands which are translated to traction and steering commands for the tricycle drive base. Odometry is computed from hardware feedback and published. -Velocity commands ------------------ - -The controller works with a velocity twist from which it extracts -the x component of the linear velocity and the z component of the angular velocity. -Velocities on other components are ignored. - - Other features -------------- @@ -26,6 +18,16 @@ Other features Velocity, acceleration and jerk limits Automatic stop after command timeout +ROS 2 Interfaces +------------------------ + +Subscribers +,,,,,,,,,,,, + +~/cmd_vel [geometry_msgs/msg/TwistStamped] + Velocity command for the controller. The controller extracts the x component of the linear velocity and the z component of the angular velocity. Velocities on other components are ignored. + + Parameters -------------- diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index 07be8c2aae..5586b87efc 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -316,6 +316,9 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & } else { + RCLCPP_WARN( + get_node()->get_logger(), + "[Deprecated] Using geometry_msgs::msg::Twist instead of TwistStamped is deprecated."); velocity_command_unstamped_subscriber_ = get_node()->create_subscription( DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), [this](const std::shared_ptr msg) -> void diff --git a/tricycle_controller/src/tricycle_controller_parameter.yaml b/tricycle_controller/src/tricycle_controller_parameter.yaml index 68fc2202c2..951c159510 100644 --- a/tricycle_controller/src/tricycle_controller_parameter.yaml +++ b/tricycle_controller/src/tricycle_controller_parameter.yaml @@ -81,7 +81,7 @@ tricycle_controller: use_stamped_vel: { type: bool, default_value: true, - description: "Use stamp from input velocity message to calculate how old the command actually is.", + description: "(deprecated) Use stamp from input velocity message to calculate how old the command actually is.", } traction: # "The positive limit will be applied to both directions. Setting different limits for positive " diff --git a/tricycle_controller/test/config/test_tricycle_controller.yaml b/tricycle_controller/test/config/test_tricycle_controller.yaml index efecfe968f..4cef3ee131 100644 --- a/tricycle_controller/test/config/test_tricycle_controller.yaml +++ b/tricycle_controller/test/config/test_tricycle_controller.yaml @@ -5,7 +5,6 @@ test_tricycle_controller: steering_joint_name: steering_joint wheel_radius: 0.125 wheelbase: 1.252 - use_stamped_vel: false enable_odom_tf: false use_sim_time: false pose_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] From f3920d33ebb31d5461939f6f5ed25ed200c2d96f Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 4 Apr 2024 22:33:58 +0000 Subject: [PATCH 3/3] Deprecate also for steering_controllers --- steering_controllers_library/doc/userdoc.rst | 5 +---- .../src/steering_controllers_library.cpp | 3 +++ .../src/steering_controllers_library.yaml | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/steering_controllers_library/doc/userdoc.rst b/steering_controllers_library/doc/userdoc.rst index df3d1529d0..0155bc5fd8 100644 --- a/steering_controllers_library/doc/userdoc.rst +++ b/steering_controllers_library/doc/userdoc.rst @@ -13,7 +13,7 @@ Nomenclature used for the controller is used from `wikipedia /reference [geometry_msgs/msg/TwistStamped] - If parameter ``use_stamped_vel`` is ``true``. -- /reference_unstamped [geometry_msgs/msg/Twist] - If parameter ``use_stamped_vel`` is ``false``. Publishers ,,,,,,,,,,, diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index eb497ebb3d..9bf9fa51d6 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -116,6 +116,9 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( ref_timeout_ = rclcpp::Duration::from_seconds(params_.reference_timeout); if (params_.use_stamped_vel) { + RCLCPP_WARN( + get_node()->get_logger(), + "[Deprecated] Using geometry_msgs::msg::Twist instead of TwistStamped is deprecated."); ref_subscriber_twist_ = get_node()->create_subscription( "~/reference", subscribers_qos, std::bind(&SteeringControllersLibrary::reference_callback, this, std::placeholders::_1)); diff --git a/steering_controllers_library/src/steering_controllers_library.yaml b/steering_controllers_library/src/steering_controllers_library.yaml index a9f7fa75fb..b18cac5ae1 100644 --- a/steering_controllers_library/src/steering_controllers_library.yaml +++ b/steering_controllers_library/src/steering_controllers_library.yaml @@ -116,7 +116,7 @@ steering_controllers_library: use_stamped_vel: { type: bool, default_value: false, - description: "Choice of vel type, if use_stamped_vel is false then ``geometry_msgs::msg::Twist`` is taken as vel msg type, if + description: "(Deprecated) Choice of vel type, if use_stamped_vel is false then ``geometry_msgs::msg::Twist`` is taken as vel msg type, if use_stamped_vel is true then ``geometry_msgs::msg::TwistStamped`` is taken as vel msg type", read_only: false, }