From 51c5c082af4acde0809bda9b8d616507467d7cac Mon Sep 17 00:00:00 2001 From: nitin Date: Wed, 21 May 2025 16:34:47 +0530 Subject: [PATCH 01/19] Add swerve_drive_controller package with tests and documentation --- swerve_drive_controller/CMakeLists.txt | 98 +++ swerve_drive_controller/LICENSE | 202 +++++ swerve_drive_controller/doc/userdoc.rst | 47 ++ .../swerve_drive_controller.hpp | 192 +++++ .../swerve_drive_kinematics.hpp | 99 +++ swerve_drive_controller/package.xml | 40 + .../src/swerve_drive_controller.cpp | 743 ++++++++++++++++++ .../swerve_drive_controller_parameter.yaml | 83 ++ .../src/swerve_drive_kinematics.cpp | 108 +++ .../swerve_drive_plugin.xml | 9 + .../config/test_swerve_drive_controller.yaml | 45 ++ .../test_load_swerve_drive_controller.cpp | 50 ++ .../test/test_swerve_drive_controller.cpp | 457 +++++++++++ .../test/test_swerve_drive_controller.hpp | 58 ++ 14 files changed, 2231 insertions(+) create mode 100644 swerve_drive_controller/CMakeLists.txt create mode 100644 swerve_drive_controller/LICENSE create mode 100644 swerve_drive_controller/doc/userdoc.rst create mode 100644 swerve_drive_controller/include/swerve_drive_controller/swerve_drive_controller.hpp create mode 100644 swerve_drive_controller/include/swerve_drive_controller/swerve_drive_kinematics.hpp create mode 100644 swerve_drive_controller/package.xml create mode 100644 swerve_drive_controller/src/swerve_drive_controller.cpp create mode 100644 swerve_drive_controller/src/swerve_drive_controller_parameter.yaml create mode 100644 swerve_drive_controller/src/swerve_drive_kinematics.cpp create mode 100644 swerve_drive_controller/swerve_drive_plugin.xml create mode 100644 swerve_drive_controller/test/config/test_swerve_drive_controller.yaml create mode 100644 swerve_drive_controller/test/test_load_swerve_drive_controller.cpp create mode 100644 swerve_drive_controller/test/test_swerve_drive_controller.cpp create mode 100644 swerve_drive_controller/test/test_swerve_drive_controller.hpp diff --git a/swerve_drive_controller/CMakeLists.txt b/swerve_drive_controller/CMakeLists.txt new file mode 100644 index 0000000000..f063743e0f --- /dev/null +++ b/swerve_drive_controller/CMakeLists.txt @@ -0,0 +1,98 @@ +cmake_minimum_required(VERSION 3.8) +project(swerve_drive_controller) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(rclcpp REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(tf2_msgs REQUIRED) +find_package(hardware_interface REQUIRED) +find_package(controller_interface REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(realtime_tools REQUIRED) + +pluginlib_export_plugin_description_file(controller_interface swerve_drive_plugin.xml) + +add_library(swerve_drive_controller SHARED + src/swerve_drive_controller.cpp + src/swerve_drive_kinematics.cpp +) + +target_include_directories(swerve_drive_controller PRIVATE include) +ament_target_dependencies(swerve_drive_controller + builtin_interfaces + controller_interface + geometry_msgs + hardware_interface + nav_msgs + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + tf2 + tf2_msgs +) + +install(DIRECTORY include/ + DESTINATION include +) + +install(TARGETS swerve_drive_controller + EXPORT export_swerve_drive_controller + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin +) + +install( + FILES swerve_drive_plugin.xml + DESTINATION share/${PROJECT_NAME} +) + + +if(BUILD_TESTING) + + find_package(controller_manager REQUIRED) + find_package(ament_lint_auto REQUIRED) + find_package(ament_cmake_gmock REQUIRED) + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_cmake_uncrustify REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + ament_add_gtest(test_swerve_drive_controller + test/test_swerve_drive_controller.cpp + ) + + target_include_directories(test_swerve_drive_controller PRIVATE include) + ament_target_dependencies(test_swerve_drive_controller + controller_interface + hardware_interface + rclcpp + geometry_msgs + nav_msgs + ) + target_link_libraries(test_swerve_drive_controller + ${PROJECT_NAME} + ) + + add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") + ament_add_gmock(test_load_swerve_drive_controller test/test_load_swerve_drive_controller.cpp) + ament_target_dependencies(test_load_swerve_drive_controller + controller_manager + ros2_control_test_assets + ) + ament_lint_auto_find_test_dependencies() +endif() + +ament_export_libraries(swerve_drive_controller) +ament_export_dependencies(controller_interface hardware_interface pluginlib rclcpp rclcpp_lifecycle) + + +ament_package() diff --git a/swerve_drive_controller/LICENSE b/swerve_drive_controller/LICENSE new file mode 100644 index 0000000000..d645695673 --- /dev/null +++ b/swerve_drive_controller/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/swerve_drive_controller/doc/userdoc.rst b/swerve_drive_controller/doc/userdoc.rst new file mode 100644 index 0000000000..a568f43f0d --- /dev/null +++ b/swerve_drive_controller/doc/userdoc.rst @@ -0,0 +1,47 @@ +.. _swerve_drive_controller_userdoc: + +swerve_drive_controller +========================= + +Library with shared functionalities for mobile robot controllers with swerve drive (four swerve wheels). +The library implements generic odometry and update methods and defines the main interfaces. + +Execution logic of the controller +---------------------------------- + +The controller uses velocity input, i.e., stamped or non stamped Twist messages where linear ``x``, ``y``, and angular ``z`` components are used. +Values in other components are ignored. + +Note about odometry calculation: +In the DiffDRiveController, the velocity is filtered out, but we prefer to return it raw and let the user perform post-processing at will. +We prefer this way of doing so as filtering introduces delay (which makes it difficult to interpret and compare behavior curves). + + +Description of controller's interfaces +-------------------------------------- + +Subscribers +,,,,,,,,,,,, + +If ``use_stamped_vel=true``: +~/cmd_vel [geometry_msgs/msg/TwistStamped] + Velocity command for the controller. The controller extracts the x and y component of the linear velocity and the z component of the angular velocity. Velocities on other components are ignored. + +If ``use_stamped_vel=false``: +~/cmd_vel [geometry_msgs/msg/Twist] + Velocity command for the controller. The controller extracts the x and y component of the linear velocity and the z component of the angular velocity. Velocities on other components are ignored. + +Publishers +,,,,,,,,,,, +~/odom [nav_msgs::msg::Odometry] + This represents an estimate of the robot's position and velocity in free space. + +/tf [tf2_msgs::msg::TFMessage] + tf tree. Published only if ``enable_odom_tf=true`` + + +Parameters +,,,,,,,,,,, + +.. literalinclude:: ../test/config/test_swerve_drive_controller.yaml + :language: yaml diff --git a/swerve_drive_controller/include/swerve_drive_controller/swerve_drive_controller.hpp b/swerve_drive_controller/include/swerve_drive_controller/swerve_drive_controller.hpp new file mode 100644 index 0000000000..95d23138f6 --- /dev/null +++ b/swerve_drive_controller/include/swerve_drive_controller/swerve_drive_controller.hpp @@ -0,0 +1,192 @@ +// Copyright 2025 (your name or organization) +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SWERVE_DRIVE_CONTROLLER__SWERVE_DRIVE_CONTROLLER_HPP_ +#define SWERVE_DRIVE_CONTROLLER__SWERVE_DRIVE_CONTROLLER_HPP_ + +#include "swerve_drive_controller/swerve_drive_kinematics.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include "controller_interface/controller_interface.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "hardware_interface/handle.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_box.hpp" +#include "realtime_tools/realtime_buffer.hpp" +#include "realtime_tools/realtime_publisher.hpp" +#include "tf2_msgs/msg/tf_message.hpp" + +namespace swerve_drive_controller +{ + +using CallbackReturn = controller_interface::CallbackReturn; +// using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +class Wheel +{ +public: + Wheel( + std::reference_wrapper velocity, + std::reference_wrapper feedback, + std::string name); + + void set_velocity(double velocity); + double get_feedback(); + +private: + std::reference_wrapper velocity_; + std::reference_wrapper feedback_; + std::string name; +}; + +class Axle +{ +public: + Axle( + std::reference_wrapper position, + std::reference_wrapper feedback, + std::string name); + + void set_position(double position); + double get_feedback(); + +private: + std::reference_wrapper position_; + std::reference_wrapper feedback_; + std::string name; +}; + +class SwerveController : public controller_interface::ControllerInterface +{ + using TwistStamped = geometry_msgs::msg::TwistStamped; + using Twist = geometry_msgs::msg::Twist; + +public: + SwerveController(); + + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + CallbackReturn on_init() override; + + CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; + + CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + + CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override; + + CallbackReturn on_error(const rclcpp_lifecycle::State & previous_state) override; + + CallbackReturn on_shutdown(const rclcpp_lifecycle::State & previous_state) override; + +protected: + std::shared_ptr get_wheel(const std::string & wheel_name); + std::shared_ptr get_axle(const std::string & axle_name); + + // Handles for three wheels and their axles + std::shared_ptr front_left_wheel_handle_; + std::shared_ptr front_right_wheel_handle_; + std::shared_ptr rear_left_wheel_handle_; + std::shared_ptr rear_right_wheel_handle_; + + std::shared_ptr front_left_axle_handle_; + std::shared_ptr front_right_axle_handle_; + std::shared_ptr rear_left_axle_handle_; + std::shared_ptr rear_right_axle_handle_; + + // Joint names for wheels and axles + std::string front_left_wheel_joint_name_; + std::string front_right_wheel_joint_name_; + std::string rear_left_wheel_joint_name_; + std::string rear_right_wheel_joint_name_; + + std::string front_left_axle_joint_name_; + std::string front_right_axle_joint_name_; + std::string rear_left_axle_joint_name_; + std::string rear_right_axle_joint_name_; + + std::string cmd_vel_topic_; + std::string odometry_topic_; + std::string base_footprint_; + + bool enable_odom_tf_ = true; + bool open_loop_ = false; + bool use_stamped_vel_ = false; + + double front_left_velocity_threshold_; + double front_right_velocity_threshold_; + double rear_left_velocity_threshold_; + double rear_right_velocity_threshold_; + + SwerveDriveKinematics swerveDriveKinematics_; + + std::queue previous_commands_; // last two commands + + double pose_covariance_diagonal_array_[6]; + double twist_covariance_diagonal_array_[6]; + + double publish_rate_ = 50.0; + rclcpp::Duration publish_period_ = rclcpp::Duration::from_nanoseconds(0); + rclcpp::Time previous_publish_timestamp_{0, 0, RCL_CLOCK_UNINITIALIZED}; + + struct WheelParams + { + double x_offset = 0.0; // Chassis Center to Axle Center + double y_offset = 0.0; // Axle Center to Wheel Center + double radius = 0.0; // Assumed to be the same for all wheels + double center_of_rotation = 0.0; + } wheel_params_; + + // Timeout to consider cmd_vel commands old + std::chrono::milliseconds cmd_vel_timeout_{500}; + rclcpp::Time previous_update_timestamp_{0}; + + // Topic Subscription + bool subscriber_is_active_ = false; + rclcpp::Subscription::SharedPtr velocity_command_subscriber_ = nullptr; + rclcpp::Subscription::SharedPtr velocity_command_unstamped_subscriber_ = nullptr; + + realtime_tools::RealtimeBuffer> received_velocity_msg_ptr_{nullptr}; + + std::shared_ptr> odometry_publisher_ = nullptr; + std::shared_ptr> + realtime_odometry_publisher_ = nullptr; + std::shared_ptr> odometry_transform_publisher_ = + nullptr; + std::shared_ptr> + realtime_odometry_transform_publisher_ = nullptr; + + bool is_halted_ = false; + + bool reset(); + void halt(); +}; + +} // namespace swerve_drive_controller +#endif // SWERVE_DRIVE_CONTROLLER__SWERVE_DRIVE_CONTROLLER_HPP_ diff --git a/swerve_drive_controller/include/swerve_drive_controller/swerve_drive_kinematics.hpp b/swerve_drive_controller/include/swerve_drive_controller/swerve_drive_kinematics.hpp new file mode 100644 index 0000000000..4c1e257f53 --- /dev/null +++ b/swerve_drive_controller/include/swerve_drive_controller/swerve_drive_kinematics.hpp @@ -0,0 +1,99 @@ +// Copyright 2025 (your name or organization) +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SWERVE_DRIVE_CONTROLLER__SWERVE_DRIVE_KINEMATICS_HPP_ +#define SWERVE_DRIVE_CONTROLLER__SWERVE_DRIVE_KINEMATICS_HPP_ + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + + +namespace swerve_drive_controller +{ + +/** + * @brief Struct to represent the kinematics command for a single wheel . + */ + +struct WheelCommand +{ + double steering_angle; // Steering angle in radians + double drive_velocity; // Drive velocity in meters per second +}; + +/** + * @brief Struct to represent the odometry state of the robot. + */ + +struct OdometryState +{ + double x; // X position in meters + double y; // Y position in meters + double theta; // Orientation (yaw) in radians +}; + +class SwerveDriveKinematics +{ +public: + /** + * @brief Constructor for the kinematics solver. + * @param wheel_positions Array of (x, y) positions of the wheels relative to the robot's center. + */ + + explicit SwerveDriveKinematics(const std::array, 4> & wheel_positions); + /** + * @brief Compute the wheel commands based on robot velocity commands. + * @param linear_velocity_x Linear velocity in the x direction (m/s). + * @param linear_velocity_y Linear velocity in the y direction (m/s). + * @param angular_velocity_z Angular velocity about the z-axis (rad/s). + * @return Array of wheel commands (steering angles and drive velocities). + */ + + std::array compute_wheel_commands( + double linear_velocity_x, double linear_velocity_y, double angular_velocity_z); + + /** + * @brief Update the odometry based on wheel velocities and elapsed time. + * @param wheel_velocities Array of measured wheel velocities (m/s). + * @param steering_angles Array of measured steering angles (radians). + * @param dt Time step (seconds). + * @return Updated odometry state. + */ + + OdometryState update_odometry( + const std::array & wheel_velocities_, const std::array & steering_angles_, + double dt); + +private: + std::array, 4> wheel_positions_; // Wheel Positions + OdometryState odometry_; // Current Odometry of the robot + + /** + * @brief Normalize an angle to the range [-pi, pi]. + * @param angle input in radians. + * @return Normalized angle in radians. + */ + + double normalize_angle(double angle); +}; +} // namespace swerve_drive_controller + +#endif // SWERVE_DRIVE_CONTROLLER__SWERVE_DRIVE_KINEMATICS_HPP_ diff --git a/swerve_drive_controller/package.xml b/swerve_drive_controller/package.xml new file mode 100644 index 0000000000..3899b87c03 --- /dev/null +++ b/swerve_drive_controller/package.xml @@ -0,0 +1,40 @@ + + + + swerve_drive_controller + 0.0.0 + TODO: Package description + knight + Apache-2.0 + + ament_cmake + ament_cmake_ros + + + geometry_msgs + hardware_interface + control_toolbox + controller_interface + nav_msgs + rclcpp + rclcpp_lifecycle + realtime_tools + tf2 + tf2_msgs + + ament_lint_auto + ament_lint_common + ament_cmake_uncrustify + ament_cmake_cpplint + ament_cmake_clang_format + controller_manager + ament_cmake_gtest + ament_cmake_gmock + hardware_interface_testing + ros2_control_test_assets + pluginlib + + + ament_cmake + + diff --git a/swerve_drive_controller/src/swerve_drive_controller.cpp b/swerve_drive_controller/src/swerve_drive_controller.cpp new file mode 100644 index 0000000000..d7a0057a9a --- /dev/null +++ b/swerve_drive_controller/src/swerve_drive_controller.cpp @@ -0,0 +1,743 @@ +// Copyright 2025 (your name or organization) +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "swerve_drive_controller/swerve_drive_controller.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/logging.hpp" +#include "tf2/LinearMath/Quaternion.h" + +namespace +{ + +constexpr auto DEFAULT_COMMAND_TOPIC = "~/cmd_vel"; +constexpr auto DEFAULT_COMMAND_UNSTAMPED_TOPIC = "~/cmd_vel_unstamped"; +constexpr auto DEFAULT_ODOMETRY_TOPIC = "~/odom"; +constexpr auto DEFAULT_TRANSFORM_TOPIC = "/tf"; + +} // namespace + +namespace swerve_drive_controller +{ + +using namespace std::chrono_literals; +using controller_interface::interface_configuration_type; +using controller_interface::InterfaceConfiguration; +using hardware_interface::HW_IF_POSITION; +using hardware_interface::HW_IF_VELOCITY; +using lifecycle_msgs::msg::State; + +Wheel::Wheel( + std::reference_wrapper velocity, + std::reference_wrapper feedback, std::string name) +: velocity_(velocity), feedback_(feedback), name(std::move(name)) +{ +} + +void Wheel::set_velocity(double velocity) {velocity_.get().set_value(velocity);} + +double Wheel::get_feedback() {return Wheel::feedback_.get().get_optional().value();} + +Axle::Axle( + std::reference_wrapper position, + std::reference_wrapper feedback, std::string name) +: position_(position), feedback_(feedback), name(std::move(name)) +{ +} + +void Axle::set_position(double position) {position_.get().set_value(position);} + +double Axle::get_feedback() {return Axle::feedback_.get().get_optional().value();} + +std::array, 4> wheel_positions_ = { + std::make_pair(-0.1, 0.175), // front left + std::make_pair(0.1, 0.175), // front right + std::make_pair(-0.1, -0.175), // rear left + std::make_pair(0.1, -0.175) // rear right +}; + +SwerveController::SwerveController() +: controller_interface::ControllerInterface(), swerveDriveKinematics_(wheel_positions_) +{ + auto zero_twist = std::make_shared(); + zero_twist->header.stamp = rclcpp::Time(0); + zero_twist->twist.linear.x = 0.0; + zero_twist->twist.linear.y = 0.0; + zero_twist->twist.angular.z = 0.0; + received_velocity_msg_ptr_.writeFromNonRT(zero_twist); +} + +CallbackReturn SwerveController::on_init() +{ + auto node = get_node(); + if (!node) { + RCLCPP_ERROR(rclcpp::get_logger("SwerveController"), "Node is null in on_init"); + return controller_interface::CallbackReturn::ERROR; + } + try { + // Declare parameters + auto_declare("joint_steering_left_front", front_left_axle_joint_name_); + auto_declare("joint_steering_right_front", front_right_axle_joint_name_); + auto_declare("joint_steering_left_rear", rear_left_axle_joint_name_); + auto_declare("joint_steering_right_rear", rear_right_axle_joint_name_); + auto_declare("joint_wheel_left_front", front_left_wheel_joint_name_); + auto_declare("joint_wheel_right_front", front_right_wheel_joint_name_); + auto_declare("joint_wheel_left_rear", rear_left_wheel_joint_name_); + auto_declare("joint_wheel_right_rear", rear_right_wheel_joint_name_); + auto_declare("chassis_length", wheel_params_.x_offset); + auto_declare("chassis_width", wheel_params_.y_offset); + auto_declare("wheel_radius", wheel_params_.radius); + auto_declare("center_of_rotation", wheel_params_.center_of_rotation); + auto_declare("cmd_vel_timeout", cmd_vel_timeout_.count() / 1000.0); + auto_declare("use_stamped_vel", use_stamped_vel_); + auto_declare("cmd_vel_topic", cmd_vel_topic_); + auto_declare("odom", odometry_topic_); + auto_declare("base_footprint", base_footprint_); + auto_declare("publish_rate", publish_rate_); + auto_declare("enable_odom_tf", enable_odom_tf_); + auto_declare("open_loop", open_loop_); + auto_declare("front_left_velocity_threshold", front_left_velocity_threshold_); + auto_declare("front_right_velocity_threshold", front_right_velocity_threshold_); + auto_declare("rear_left_velocity_threshold", rear_left_velocity_threshold_); + auto_declare("rear_right_velocity_threshold", rear_right_velocity_threshold_); + } catch (const std::exception & e) { + return CallbackReturn::ERROR; + } + return CallbackReturn::SUCCESS; +} + +InterfaceConfiguration SwerveController::command_interface_configuration() const +{ + std::vector conf_names; + + conf_names.push_back(front_left_wheel_joint_name_ + "/" + HW_IF_VELOCITY); + conf_names.push_back(front_right_wheel_joint_name_ + "/" + HW_IF_VELOCITY); + conf_names.push_back(rear_left_wheel_joint_name_ + "/" + HW_IF_VELOCITY); + conf_names.push_back(rear_right_wheel_joint_name_ + "/" + HW_IF_VELOCITY); + + conf_names.push_back(front_left_axle_joint_name_ + "/" + HW_IF_POSITION); + conf_names.push_back(front_right_axle_joint_name_ + "/" + HW_IF_POSITION); + conf_names.push_back(rear_left_axle_joint_name_ + "/" + HW_IF_POSITION); + conf_names.push_back(rear_right_axle_joint_name_ + "/" + HW_IF_POSITION); + + return {interface_configuration_type::INDIVIDUAL, conf_names}; +} + +InterfaceConfiguration SwerveController::state_interface_configuration() const +{ + std::vector conf_names; + conf_names.push_back(front_left_wheel_joint_name_ + "/" + HW_IF_VELOCITY); + conf_names.push_back(front_right_wheel_joint_name_ + "/" + HW_IF_VELOCITY); + conf_names.push_back(rear_left_wheel_joint_name_ + "/" + HW_IF_VELOCITY); + conf_names.push_back(rear_right_wheel_joint_name_ + "/" + HW_IF_VELOCITY); + + conf_names.push_back(front_left_axle_joint_name_ + "/" + HW_IF_POSITION); + conf_names.push_back(front_right_axle_joint_name_ + "/" + HW_IF_POSITION); + conf_names.push_back(rear_left_axle_joint_name_ + "/" + HW_IF_POSITION); + conf_names.push_back(rear_right_axle_joint_name_ + "/" + HW_IF_POSITION); + + return {interface_configuration_type::INDIVIDUAL, conf_names}; +} + +CallbackReturn SwerveController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) +{ + // auto logger = get_node()->get_logger(); + auto logger = rclcpp::get_logger("SwerveController"); + try { + front_left_wheel_joint_name_ = get_node()->get_parameter("joint_wheel_left_front").as_string(); + front_right_wheel_joint_name_ = + get_node()->get_parameter("joint_wheel_right_front").as_string(); + rear_left_wheel_joint_name_ = get_node()->get_parameter("joint_wheel_left_rear").as_string(); + rear_right_wheel_joint_name_ = get_node()->get_parameter("joint_wheel_right_rear").as_string(); + front_left_axle_joint_name_ = + get_node()->get_parameter("joint_steering_left_front").as_string(); + front_right_axle_joint_name_ = + get_node()->get_parameter("joint_steering_right_front").as_string(); + rear_left_axle_joint_name_ = get_node()->get_parameter("joint_steering_left_rear").as_string(); + rear_right_axle_joint_name_ = + get_node()->get_parameter("joint_steering_right_rear").as_string(); + cmd_vel_topic_ = get_node()->get_parameter("cmd_vel_topic").as_string(); + odometry_topic_ = get_node()->get_parameter("odom").as_string(); + base_footprint_ = get_node()->get_parameter("base_footprint").as_string(); + publish_rate_ = get_node()->get_parameter("publish_rate").as_double(); + enable_odom_tf_ = get_node()->get_parameter("enable_odom_tf").as_bool(); + open_loop_ = get_node()->get_parameter("open_loop").as_bool(); + + use_stamped_vel_ = get_node()->get_parameter("use_stamped_vel").as_bool(); + + wheel_params_.x_offset = get_node()->get_parameter("chassis_length").as_double(); + wheel_params_.y_offset = get_node()->get_parameter("chassis_width").as_double(); + wheel_params_.radius = get_node()->get_parameter("wheel_radius").as_double(); + wheel_params_.center_of_rotation = get_node()->get_parameter("center_of_rotation").as_double(); + + front_left_velocity_threshold_ = + get_node()->get_parameter("front_left_velocity_threshold").as_double(); + front_right_velocity_threshold_ = + get_node()->get_parameter("front_right_velocity_threshold").as_double(); + rear_left_velocity_threshold_ = + get_node()->get_parameter("rear_left_velocity_threshold").as_double(); + rear_right_velocity_threshold_ = + get_node()->get_parameter("rear_right_velocity_threshold").as_double(); + + for (std::size_t i = 0; i < 6; ++i) { + pose_covariance_diagonal_array_[i] = 0.01; + } + + for (std::size_t i = 0; i < 6; ++i) { + twist_covariance_diagonal_array_[i] = 0.01; + } + + if (front_left_wheel_joint_name_.empty()) { + RCLCPP_ERROR(logger, "front_wheel_joint_name is not set"); + return CallbackReturn::ERROR; + } + if (front_right_wheel_joint_name_.empty()) { + RCLCPP_ERROR(logger, "front_right_wheel_joint_name is not set"); + return CallbackReturn::ERROR; + } + if (rear_left_wheel_joint_name_.empty()) { + RCLCPP_ERROR(logger, "rear_left_wheel_joint_name is not set"); + return CallbackReturn::ERROR; + } + if (rear_right_wheel_joint_name_.empty()) { + RCLCPP_ERROR(logger, "rear_right_wheel_joint_name is not set"); + return CallbackReturn::ERROR; + } + + if (front_left_axle_joint_name_.empty()) { + RCLCPP_ERROR(logger, "front_axle_joint_name is not set"); + return CallbackReturn::ERROR; + } + if (front_right_axle_joint_name_.empty()) { + RCLCPP_ERROR(logger, "front_right_axle_joint_name is not set"); + return CallbackReturn::ERROR; + } + if (rear_left_axle_joint_name_.empty()) { + RCLCPP_ERROR(logger, "rear_left_axle_joint_name_ is not set"); + return CallbackReturn::ERROR; + } + if (rear_right_axle_joint_name_.empty()) { + RCLCPP_ERROR(logger, "rear_right_axle_joint_name_ is not set"); + return CallbackReturn::ERROR; + } + + cmd_vel_timeout_ = std::chrono::milliseconds( + static_cast(get_node()->get_parameter("cmd_vel_timeout").as_double() * 1000.0)); + + use_stamped_vel_ = get_node()->get_parameter("use_stamped_vel").as_bool(); + + if (!reset()) { + return CallbackReturn::ERROR; + } + + TwistStamped empty_twist; + empty_twist.header.stamp = get_node()->now(); + empty_twist.twist.linear.x = 0.0; + empty_twist.twist.linear.y = 0.0; + empty_twist.twist.angular.z = 0.0; + received_velocity_msg_ptr_.writeFromNonRT(std::make_shared(empty_twist)); + + if (use_stamped_vel_) { + velocity_command_subscriber_ = get_node()->create_subscription( + DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), + [this, logger](const std::shared_ptr msg) -> void + { + if (!subscriber_is_active_) { + RCLCPP_WARN(logger, "Can't accept new commands. subscriber is inactive"); + return; + } + if ((msg->header.stamp.sec == 0) && (msg->header.stamp.nanosec == 0)) { + RCLCPP_WARN_ONCE( + logger, + "Received TwistStamped with zero timestamp, setting it to current " + "time, this message will only be shown once"); + msg->header.stamp = get_node()->get_clock()->now(); + } + received_velocity_msg_ptr_.writeFromNonRT(std::move(msg)); + }); + } else { + velocity_command_unstamped_subscriber_ = get_node()->create_subscription( + DEFAULT_COMMAND_UNSTAMPED_TOPIC, rclcpp::SystemDefaultsQoS(), + [this, logger](const std::shared_ptr msg) -> void + { + if (!subscriber_is_active_) { + RCLCPP_WARN(logger, "Can't accept new commands. subscriber is inactive"); + return; + } + // Write fake header in the stored stamped command + const std::shared_ptr twist_stamped = + *(received_velocity_msg_ptr_.readFromRT()); + twist_stamped->twist = *msg; + twist_stamped->header.stamp = get_node()->get_clock()->now(); + }); + } + + odometry_publisher_ = get_node()->create_publisher( + DEFAULT_ODOMETRY_TOPIC, rclcpp::SystemDefaultsQoS()); + + realtime_odometry_publisher_ = + std::make_shared>( + odometry_publisher_); + + std::string tf_prefix = ""; + tf_prefix = std::string(get_node()->get_namespace()); + if (tf_prefix == "/") { + tf_prefix = ""; + } else { + tf_prefix = tf_prefix + "/"; + } + + const auto odom_frame_id = tf_prefix + odometry_topic_; + const auto base_frame_id = tf_prefix + base_footprint_; + + auto & odometry_message = realtime_odometry_publisher_->msg_; + odometry_message.header.frame_id = odom_frame_id; + odometry_message.child_frame_id = base_frame_id; + + publish_period_ = rclcpp::Duration::from_seconds(1.0 / publish_rate_); + + odometry_message.twist = + geometry_msgs::msg::TwistWithCovariance(rosidl_runtime_cpp::MessageInitialization::ALL); + + constexpr std::size_t NUM_DIMENSIONS = 6; + for (std::size_t index = 0; index < 6; ++index) { + const std::size_t diagonal_index = NUM_DIMENSIONS * index + index; + odometry_message.pose.covariance[diagonal_index] = pose_covariance_diagonal_array_[index]; + odometry_message.twist.covariance[diagonal_index] = twist_covariance_diagonal_array_[index]; + } + + odometry_transform_publisher_ = get_node()->create_publisher( + DEFAULT_TRANSFORM_TOPIC, rclcpp::SystemDefaultsQoS()); + + realtime_odometry_transform_publisher_ = + std::make_shared>( + odometry_transform_publisher_); + + auto & odometry_transform_message = realtime_odometry_transform_publisher_->msg_; + odometry_transform_message.transforms.resize(1); + odometry_transform_message.transforms.front().header.frame_id = odom_frame_id; + odometry_transform_message.transforms.front().child_frame_id = base_frame_id; + + previous_update_timestamp_ = get_node()->get_clock()->now(); + } catch (const std::exception & e) { + RCLCPP_ERROR(logger, "EXCEPTION DURING on_configure: %s", e.what()); + return CallbackReturn::ERROR; + } + + std::chrono::seconds sleep_duration(1); + rclcpp::sleep_for(sleep_duration); + + return CallbackReturn::SUCCESS; +} + +CallbackReturn SwerveController::on_activate(const rclcpp_lifecycle::State &) +{ + auto logger = rclcpp::get_logger("SwerveController"); + front_left_wheel_handle_ = get_wheel(front_left_wheel_joint_name_); + front_right_wheel_handle_ = get_wheel(front_right_wheel_joint_name_); + rear_left_wheel_handle_ = get_wheel(rear_left_wheel_joint_name_); + rear_right_wheel_handle_ = get_wheel(rear_right_wheel_joint_name_); + + front_left_axle_handle_ = get_axle(front_left_axle_joint_name_); + front_right_axle_handle_ = get_axle(front_right_axle_joint_name_); + rear_left_axle_handle_ = get_axle(rear_left_axle_joint_name_); + rear_right_axle_handle_ = get_axle(rear_right_axle_joint_name_); + + if (!front_left_wheel_handle_) { + RCLCPP_ERROR(logger, "ERROR IN FETCHING front_left_wheel_handle_"); + return CallbackReturn::ERROR; + } + + if (!front_right_wheel_handle_) { + RCLCPP_ERROR(logger, "ERROR IN FETCHING front_right_wheel_handle_"); + return CallbackReturn::ERROR; + } + + if (!rear_left_wheel_handle_) { + RCLCPP_ERROR(logger, "ERROR IN FETCHING rear_left_wheel_handle_"); + return CallbackReturn::ERROR; + } + + if (!rear_right_wheel_handle_) { + RCLCPP_ERROR(logger, "ERROR IN FETCHING rear_right_wheel_handle_"); + return CallbackReturn::ERROR; + } + + if (!front_left_axle_handle_) { + RCLCPP_ERROR(logger, "ERROR IN FETCHING front_left_axle_handle_"); + return CallbackReturn::ERROR; + } + + if (!front_right_axle_handle_) { + RCLCPP_ERROR(logger, "ERROR IN FETCHING front_right_axle_handle_"); + return CallbackReturn::ERROR; + } + + if (!rear_left_axle_handle_) { + RCLCPP_ERROR(logger, "ERROR IN FETCHING rear_left_axle_handle_"); + return CallbackReturn::ERROR; + } + + if (!rear_right_axle_handle_) { + RCLCPP_ERROR(logger, "ERROR IN FETCHING rear_right_axle_handle_"); + return CallbackReturn::ERROR; + } + + front_left_wheel_handle_->set_velocity(0.0); + front_right_wheel_handle_->set_velocity(0.0); + rear_left_wheel_handle_->set_velocity(0.0); + rear_right_wheel_handle_->set_velocity(0.0); + front_left_axle_handle_->set_position(0.0); + front_right_axle_handle_->set_position(0.0); + rear_left_axle_handle_->set_position(0.0); + rear_right_axle_handle_->set_position(0.0); + is_halted_ = false; + subscriber_is_active_ = true; + RCLCPP_INFO(logger, "Subscriber and publisher are now active."); + return CallbackReturn::SUCCESS; +} + +controller_interface::return_type SwerveController::update( + const rclcpp::Time & time, const rclcpp::Duration & period) +{ + // auto logger = get_node()->get_logger(); + auto logger = rclcpp::get_logger("SwerveController"); + if (this->get_lifecycle_state().id() == State::PRIMARY_STATE_INACTIVE) { + if (!is_halted_) { + halt(); + is_halted_ = true; + } + + return controller_interface::return_type::OK; + } + + const auto current_time = time; + + std::shared_ptr last_command_msg = *(received_velocity_msg_ptr_.readFromRT()); + + if (last_command_msg == nullptr) { + last_command_msg = std::make_shared(); + last_command_msg->header.stamp = current_time; + last_command_msg->twist.linear.x = 0.0; + last_command_msg->twist.linear.y = 0.0; + last_command_msg->twist.angular.z = 0.0; + + received_velocity_msg_ptr_.writeFromNonRT(last_command_msg); + } + + const auto age_of_last_command = current_time - last_command_msg->header.stamp; + + if (age_of_last_command > cmd_vel_timeout_) { + last_command_msg->twist.linear.x = 0.0; + last_command_msg->twist.linear.y = 0.0; + last_command_msg->twist.angular.z = 0.0; + } + + TwistStamped command = *last_command_msg; + double & linear_x_cmd = command.twist.linear.x; + double & linear_y_cmd = command.twist.linear.y; + double & angular_cmd = command.twist.angular.z; + + double x_offset = wheel_params_.x_offset; + double y_offset = wheel_params_.y_offset; + double radius = wheel_params_.radius; + + auto wheel_command_ = + swerveDriveKinematics_.compute_wheel_commands(linear_x_cmd, linear_y_cmd, angular_cmd); + + std::vector> wheel_data = { + {wheel_command_[0], front_left_velocity_threshold_, "front_left_wheel"}, + {wheel_command_[1], front_right_velocity_threshold_, "front_right_wheel"}, + {wheel_command_[2], rear_left_velocity_threshold_, "rear_left_wheel"}, + {wheel_command_[3], rear_right_velocity_threshold_, "rear_right_wheel"}}; + + for (const auto & [command, threshold, label] : wheel_data) { + if (command.drive_velocity > threshold) { + command.drive_velocity = threshold; + } + } + + const double front_left_velocity = wheel_command_[0].drive_velocity; + const double front_left_angle = wheel_command_[0].steering_angle; + + const double front_right_velocity = wheel_command_[1].drive_velocity; + const double front_right_angle = wheel_command_[1].steering_angle; + + const double rear_left_velocity = wheel_command_[2].drive_velocity; + const double rear_left_angle = wheel_command_[2].steering_angle; + + const double rear_right_velocity = wheel_command_[3].drive_velocity; + const double rear_right_angle = wheel_command_[3].steering_angle; + + try { + if (front_left_axle_handle_ != nullptr && front_left_wheel_handle_ != nullptr) { + front_left_axle_handle_->set_position(front_left_angle); + front_left_wheel_handle_->set_velocity(front_left_velocity); + } else { + RCLCPP_ERROR(logger, "Front Left Axle Handle or Wheel Handle is NULLPTR"); + } + + if (front_right_axle_handle_ != nullptr && front_right_wheel_handle_ != nullptr) { + front_right_axle_handle_->set_position(front_right_angle); + front_right_wheel_handle_->set_velocity(front_right_velocity); + } else { + RCLCPP_ERROR(logger, "Front Right Axle Handle or Wheel Handle is NULLPTR"); + } + + if (rear_left_axle_handle_ != nullptr && rear_left_wheel_handle_ != nullptr) { + rear_left_axle_handle_->set_position(rear_left_angle); + rear_left_wheel_handle_->set_velocity(rear_left_velocity); + } else { + RCLCPP_ERROR(logger, "Rear Left Axle or Wheel Handle is NULLPTR"); + } + + if (rear_right_axle_handle_ != nullptr && rear_right_wheel_handle_ != nullptr) { + rear_right_axle_handle_->set_position(rear_right_angle); + rear_right_wheel_handle_->set_velocity(rear_right_velocity); + } else { + RCLCPP_ERROR(logger, "Rear Right Axle or Wheel Handle is NULLPTR"); + } + } catch (std::exception & e) { + RCLCPP_INFO(logger, "Got Exception: %s", e.what()); + } + + const auto update_dt = current_time - previous_update_timestamp_; + previous_update_timestamp_ = current_time; + + swerve_drive_controller::OdometryState odometry_; + + if (open_loop_) { + std::array velocity_array = { + front_left_velocity, front_right_velocity, rear_left_velocity, rear_right_velocity}; + std::array steering_angles = { + front_left_angle, front_right_angle, rear_left_angle, rear_right_angle}; + odometry_ = + swerveDriveKinematics_.update_odometry(velocity_array, steering_angles, update_dt.seconds()); + } else { + double front_left_wheel_angle = front_left_axle_handle_->get_feedback(); + double front_right_wheel_angle = front_right_axle_handle_->get_feedback(); + double rear_left_wheel_angle = rear_left_axle_handle_->get_feedback(); + double rear_right_wheel_angle = rear_right_axle_handle_->get_feedback(); + + double front_left_wheel_velocity = front_left_wheel_handle_->get_feedback(); + double front_right_wheel_velocity = front_right_wheel_handle_->get_feedback(); + double rear_left_wheel_velocity = rear_left_wheel_handle_->get_feedback(); + double rear_right_wheel_velocity = rear_right_wheel_handle_->get_feedback(); + + std::array velocity_feedback_array = { + front_left_wheel_velocity, front_right_wheel_velocity, rear_left_wheel_velocity, + rear_right_wheel_velocity}; + std::array steering_angles = { + front_left_wheel_angle, front_right_wheel_angle, rear_left_wheel_angle, + rear_right_wheel_angle}; + + odometry_ = swerveDriveKinematics_.update_odometry( + velocity_feedback_array, steering_angles, update_dt.seconds()); + } + + tf2::Quaternion orientation; + orientation.setRPY(0.0, 0.0, odometry_.theta); + + bool should_publish = false; + + try { + if (previous_publish_timestamp_ + publish_period_ < time) { + previous_publish_timestamp_ += publish_period_; + should_publish = true; + } + } catch (const std::runtime_error &) { + previous_publish_timestamp_ = time; + should_publish = true; + } + + if (realtime_odometry_publisher_ && realtime_odometry_publisher_->trylock()) { + auto & odometry_message = realtime_odometry_publisher_->msg_; + odometry_message.header.stamp = time; + odometry_message.pose.pose.position.x = odometry_.x; + odometry_message.pose.pose.position.y = odometry_.y; + odometry_message.pose.pose.orientation.x = orientation.x(); + odometry_message.pose.pose.orientation.y = orientation.y(); + odometry_message.pose.pose.orientation.z = orientation.z(); + odometry_message.pose.pose.orientation.w = orientation.w(); + + realtime_odometry_publisher_->unlockAndPublish(); + } + + if (realtime_odometry_transform_publisher_ && realtime_odometry_transform_publisher_->trylock()) { + auto & transform = realtime_odometry_transform_publisher_->msg_.transforms.front(); + transform.header.stamp = time; + + transform.transform.translation.x = odometry_.x; + transform.transform.translation.y = odometry_.y; + transform.transform.translation.z = 0.0; + + transform.transform.rotation.x = orientation.x(); + transform.transform.rotation.y = orientation.y(); + transform.transform.rotation.z = orientation.z(); + transform.transform.rotation.w = orientation.w(); + realtime_odometry_transform_publisher_->unlockAndPublish(); + } + + return controller_interface::return_type::OK; +} + +CallbackReturn SwerveController::on_deactivate(const rclcpp_lifecycle::State &) +{ + subscriber_is_active_ = false; + return CallbackReturn::SUCCESS; +} + +CallbackReturn SwerveController::on_cleanup(const rclcpp_lifecycle::State &) +{ + if (!reset()) { + return CallbackReturn::ERROR; + } + + received_velocity_msg_ptr_.writeFromNonRT(std::make_shared()); + return CallbackReturn::SUCCESS; +} + +CallbackReturn SwerveController::on_error(const rclcpp_lifecycle::State &) +{ + if (!reset()) { + return CallbackReturn::ERROR; + } + return CallbackReturn::SUCCESS; +} + +bool SwerveController::reset() +{ + subscriber_is_active_ = false; + velocity_command_subscriber_.reset(); + velocity_command_unstamped_subscriber_.reset(); + + auto zero_twist = std::make_shared(); + zero_twist->header.stamp = get_node()->get_clock()->now(); + zero_twist->twist.linear.x = 0.0; + zero_twist->twist.linear.y = 0.0; + zero_twist->twist.angular.z = 0.0; + received_velocity_msg_ptr_.writeFromNonRT(zero_twist); + is_halted_ = false; + return true; +} + +CallbackReturn SwerveController::on_shutdown(const rclcpp_lifecycle::State &) +{ + return CallbackReturn::SUCCESS; +} + +void SwerveController::halt() +{ + front_left_wheel_handle_->set_velocity(0.0); + front_left_axle_handle_->set_position(0.0); + front_right_wheel_handle_->set_velocity(0.0); + front_right_axle_handle_->set_position(0.0); + rear_left_wheel_handle_->set_velocity(0.0); + rear_left_axle_handle_->set_position(0.0); + rear_right_wheel_handle_->set_velocity(0.0); + rear_right_axle_handle_->set_position(0.0); +} + +std::shared_ptr SwerveController::get_wheel(const std::string & wheel_name) +{ + // auto logger = get_node()->get_logger(); + auto logger = rclcpp::get_logger("SwerveController"); + + if (wheel_name.empty()) { + RCLCPP_ERROR(logger, "Wheel joint name not given. Make sure all joints are specified."); + return nullptr; + } + const auto command_handle = std::find_if( + command_interfaces_.begin(), command_interfaces_.end(), + [&wheel_name](const auto & interface) + { + return interface.get_name() == (wheel_name + "/velocity") && + interface.get_interface_name() == HW_IF_VELOCITY; + }); + + if (command_handle == command_interfaces_.end()) { + RCLCPP_ERROR( + get_node()->get_logger(), "Unable to obtain joint command handle for %s", wheel_name.c_str()); + return nullptr; + } + + const auto state_handle = std::find_if( + state_interfaces_.begin(), state_interfaces_.end(), + [&wheel_name](const auto & interface) + { + return interface.get_name() == (wheel_name + "/velocity") && + interface.get_interface_name() == HW_IF_VELOCITY; + }); + + if (state_handle == state_interfaces_.end()) { + RCLCPP_ERROR(logger, "Unable to obtain joint state handle for %s", wheel_name.c_str()); + return nullptr; + } + + return std::make_shared(std::ref(*command_handle), std::ref(*state_handle), wheel_name); +} + +std::shared_ptr SwerveController::get_axle(const std::string & axle_name) +{ + // auto logger = get_node()->get_logger(); + auto logger = rclcpp::get_logger("SwerveController"); + if (axle_name.empty()) { + RCLCPP_ERROR(logger, "Wheel joint name not given. Make sure all joints are specified."); + return nullptr; + } + + // Get Command Handle for joint + const auto command_handle = std::find_if( + command_interfaces_.begin(), command_interfaces_.end(), + [&axle_name](const auto & interface) + { + return interface.get_name() == (axle_name + "/position") && + interface.get_interface_name() == HW_IF_POSITION; + }); + + if (command_handle == command_interfaces_.end()) { + RCLCPP_ERROR(logger, "Unable to find command interface for axle: %s", axle_name.c_str()); + RCLCPP_ERROR(logger, "Expected interface name: %s/position", axle_name.c_str()); + return nullptr; + } + + const auto state_handle = std::find_if( + state_interfaces_.begin(), state_interfaces_.end(), + [&axle_name](const auto & interface) + { + return interface.get_name() == (axle_name + "/position") && + interface.get_interface_name() == HW_IF_POSITION; + }); + + if (state_handle == state_interfaces_.end()) { + RCLCPP_ERROR(logger, "Unable to find the state interface for axle: %s", axle_name.c_str()); + return nullptr; + } + return std::make_shared(std::ref(*command_handle), std::ref(*state_handle), axle_name); +} + +} // namespace swerve_drive_controller + +#include "class_loader/register_macro.hpp" + +CLASS_LOADER_REGISTER_CLASS( + swerve_drive_controller::SwerveController, controller_interface::ControllerInterface) diff --git a/swerve_drive_controller/src/swerve_drive_controller_parameter.yaml b/swerve_drive_controller/src/swerve_drive_controller_parameter.yaml new file mode 100644 index 0000000000..e0ff7259bc --- /dev/null +++ b/swerve_drive_controller/src/swerve_drive_controller_parameter.yaml @@ -0,0 +1,83 @@ +swerve_drive_controller: + ros__parameters: + front_wheel_joint: { + type: string, + default_value: "front_wheel_joint", + description: "Name of the front wheel joint." + } + rear_left_wheel_joint: { + type: string, + default_value: "rear_left_wheel_joint", + description: "Name of the rear left wheel joint." + } + rear_right_wheel_joint: { + type: string, + default_value: "rear_right_wheel_joint", + description: "Name of the rear right wheel joint." + } + + front_axle_joint: { + type: string, + default_value: "front_axle_joint", + description: "Name of the front axle joint." + } + rear_left_axle_joint: { + type: string, + default_value: "rear_left_axle_joint", + description: "Name of the rear left axle joint." + } + rear_right_axle_joint: { + type: string, + default_value: "rear_right_axle_joint", + description: "Name of the rear right axle joint." + } + + chassis_length: { + type: double, + default_value: 0.85, + description: "Length of the robot chassis.", + validation: { + "swerve_drive_controller::gt<>": [0.0] + } + } + chassis_width: { + type: double, + default_value: 0.75, + description: "Width of the robot chassis.", + validation: { + "swerve_drive_controller::gt<>": [0.0] + } + } + wheel_radius: { + type: double, + default_value: 0.0775, + description: "Radius of the wheels.", + validation: { + "swerve_drive_controller::gt<>": [0.0] + } + } + + cmd_vel_timeout: { + type: double, + default_value: 10.0, + description: "Timeout in seconds before velocity commands are considered stale.", + validation: { + "swerve_drive_controller::gt<>": [0.0] + } + } + use_stamped_vel: { + type: bool, + default_value: false, + description: "If true, velocity commands will be expected to have a timestamp." + } + cmd_vel_topic: { + type: string, + default_value: "/cmd_vel", + description: "Topic name for velocity commands." + } + + open_loop: { + type: bool, + default_value: false, + description: "If set to true the odometry of the robot will be calculated from the commanded values and not from feedback.", + } diff --git a/swerve_drive_controller/src/swerve_drive_kinematics.cpp b/swerve_drive_controller/src/swerve_drive_kinematics.cpp new file mode 100644 index 0000000000..c2af9b1c57 --- /dev/null +++ b/swerve_drive_controller/src/swerve_drive_kinematics.cpp @@ -0,0 +1,108 @@ +// Copyright 2025 (your name or organization) +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "swerve_drive_controller/swerve_drive_kinematics.hpp" + +namespace swerve_drive_controller +{ + +SwerveDriveKinematics::SwerveDriveKinematics( + const std::array, 4> & wheel_positions) +: wheel_positions_(wheel_positions), odometry_{0.0, 0.0, 0.0} +{ +} + +std::array SwerveDriveKinematics::compute_wheel_commands( + double linear_velocity_x, double linear_velocity_y, double angular_velocity_z) +{ + std::array wheel_commands; + + // wx = W/2, wy = L/2 + + for (std::size_t i = 0; i < 4; i++) { + const auto & [wx, wy] = wheel_positions_[i]; + + // double vx = linear_velocity_x + angular_velocity_z * wy * ((i<2) ? 1:-1); + // double vy = linear_velocity_y + angular_velocity_z * wx * ((i%2 == 0) ? 1 : -1); + + double vx = linear_velocity_x - angular_velocity_z * wy; + double vy = linear_velocity_y + angular_velocity_z * wx; + + wheel_commands[i].drive_velocity = std::hypot(vx, vy); + wheel_commands[i].steering_angle = std::atan2(vy, vx); + } + + return wheel_commands; +} + +OdometryState SwerveDriveKinematics::update_odometry( + const std::array & wheel_velocities, const std::array & steering_angles, + double dt) +{ + // Compute robot-centric velocity (assuming perfect wheel control) + double vx_sum = 0.0, vy_sum = 0.0, wz_sum = 0.0; + for (std::size_t i = 0; i < 4; i++) { + double vx = wheel_velocities[i] * std::cos(steering_angles[i]); + double vy = wheel_velocities[i] * std::sin(steering_angles[i]); + + // Accumulate contributions to linear and angular velocities + vx_sum += vx; + vy_sum += vy; + + wz_sum += (vy * wheel_positions_[i].first - vx * wheel_positions_[i].second); + } + + double vx_robot = vx_sum / 4.0; + double vy_robot = vy_sum / 4.0; + + double wz_denominator = 0.0; + for (std::size_t i = 0; i < 4; i++) { + wz_denominator += + (wheel_positions_[i].first * wheel_positions_[i].first + + wheel_positions_[i].second * wheel_positions_[i].second); + } + double wz_robot = wz_sum / wz_denominator; + // double wz_robot = wz_sum / 4.0; + + // double wz_robot = wz_sum / (wheel_positions_[0].first * wheel_positions_[0].first + + // wheel_positions_[1].first * wheel_positions_[1].first + + // wheel_positions_[2].first * wheel_positions_[2].first + + // wheel_positions_[0].second * wheel_positions_[0].second + + // wheel_positions_[1].second * wheel_positions_[1].second + + // wheel_positions_[2].second * wheel_positions_[2].second); + + // Transform velocities to global frame + double cos_theta = std::cos(odometry_.theta); + double sin_theta = std::sin(odometry_.theta); + + double vx_global = vx_robot * cos_theta - vy_robot * sin_theta; + double vy_global = vx_robot * sin_theta + vy_robot * cos_theta; + + // Integrate to compute new position and orientation + odometry_.x += vx_global * dt; + odometry_.y += vy_global * dt; + odometry_.theta = normalize_angle(odometry_.theta + wz_robot * dt); + return odometry_; +} + +double SwerveDriveKinematics::normalize_angle(double angle) +{ + while (angle > M_PI) { + angle -= 2.0 * M_PI; + } + while (angle < -M_PI) { + angle += 2.0 * M_PI; + } + return angle; +} +} // namespace swerve_drive_controller diff --git a/swerve_drive_controller/swerve_drive_plugin.xml b/swerve_drive_controller/swerve_drive_plugin.xml new file mode 100644 index 0000000000..08bcf5f4a3 --- /dev/null +++ b/swerve_drive_controller/swerve_drive_plugin.xml @@ -0,0 +1,9 @@ + + + + A controller for a swerve drive robot. + + + diff --git a/swerve_drive_controller/test/config/test_swerve_drive_controller.yaml b/swerve_drive_controller/test/config/test_swerve_drive_controller.yaml new file mode 100644 index 0000000000..48f4e2ccab --- /dev/null +++ b/swerve_drive_controller/test/config/test_swerve_drive_controller.yaml @@ -0,0 +1,45 @@ +test_swerve_drive_controller: + ros__parameters: + update_rate: 10 + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + swerve_drive_controller: + type: swerve_drive_controller/SwerveController + + +swerve_drive_controller: + ros__parameters: + joint_steering_left_front: "joint_steering_left_front" + joint_steering_right_front: "joint_steering_right_front" + joint_steering_left_rear: "joint_steering_left_rear" + joint_steering_right_rear: "joint_steering_right_rear" + + joint_wheel_left_front: "joint_wheel_left_front" + joint_wheel_right_front: "joint_wheel_right_front" + joint_wheel_left_rear: "joint_wheel_left_rear" + joint_wheel_right_rear: "joint_wheel_right_rear" + + chassis_length: 0.35 + chassis_width: 0.2 + wheel_radius: 0.04 + center_of_rotation: 0.1 + + + cmd_vel_timeout: 10.0 + use_stamped_vel: false + cmd_vel_topic: "/cmd_vel" + odom: odom + base_footprint: base_footprint + publishe_rate: 2 + enable_odom_tf: true + open_loop: false + + front_left_velocity_threshold: 2.0 + front_right_velocity_threshold: 2.0 + rear_left_velocity_threshold: 2.0 + rear_right_velocity_threshold: 2.0 + + pose_covariance_diagonal : [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] + twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] diff --git a/swerve_drive_controller/test/test_load_swerve_drive_controller.cpp b/swerve_drive_controller/test/test_load_swerve_drive_controller.cpp new file mode 100644 index 0000000000..e75cf27edf --- /dev/null +++ b/swerve_drive_controller/test/test_load_swerve_drive_controller.cpp @@ -0,0 +1,50 @@ +// Copyright 2025 (your name or organization) +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadSwerveDriveController, load_controller) +{ + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); + + const std::string test_file_path = + std::string(TEST_FILES_DIRECTORY) + "/config/test_swerve_drive_controller.yaml"; + + cm.set_parameter({"test_swerve_drive_controller.params_file", test_file_path}); + cm.set_parameter( + {"test_swerve_drive_controller.type", "swerve_drive_controller/SwerveController"}); + + ASSERT_NE(cm.load_controller("test_swerve_drive_controller"), nullptr); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/swerve_drive_controller/test/test_swerve_drive_controller.cpp b/swerve_drive_controller/test/test_swerve_drive_controller.cpp new file mode 100644 index 0000000000..207788eda2 --- /dev/null +++ b/swerve_drive_controller/test/test_swerve_drive_controller.cpp @@ -0,0 +1,457 @@ +// Copyright 2025 (your name or organization) +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_swerve_drive_controller.hpp" + +#include +#include +#include +#include + +#include "controller_interface/controller_interface.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "rclcpp/logging.hpp" +#include "rclcpp_lifecycle/state.hpp" + +namespace swerve_drive_controller +{ +void SwerveDriveControllerTest::SetUp() +{ + node_ = std::make_shared("test_node"); + RCLCPP_INFO(node_->get_logger(), "Test node initialized"); + + // Declare parameters + node_->declare_parameter("joint_steering_left_front", "front_left_axle_joint"); + node_->declare_parameter("joint_steering_right_front", "front_right_axle_joint"); + node_->declare_parameter("joint_steering_left_rear", "rear_left_axle_joint"); + node_->declare_parameter("joint_steering_right_rear", "rear_right_axle_joint"); + node_->declare_parameter("joint_wheel_left_front", "front_left_wheel_joint"); + node_->declare_parameter("joint_wheel_right_front", "front_right_wheel_joint"); + node_->declare_parameter("joint_wheel_left_rear", "rear_left_wheel_joint"); + node_->declare_parameter("joint_wheel_right_rear", "rear_right_wheel_joint"); + node_->declare_parameter("cmd_vel_topic", "~/cmd_vel"); + node_->declare_parameter("odom", "~/odom"); + node_->declare_parameter("base_footprint", "base_footprint"); + node_->declare_parameter("enable_odom_tf", true); + node_->declare_parameter("open_loop", false); + node_->declare_parameter("use_stamped_vel", true); + node_->declare_parameter("publish_rate", 50.0); + node_->declare_parameter("cmd_vel_timeout", 0.5); + node_->declare_parameter("chassis_length", 0.5); + node_->declare_parameter("chassis_width", 0.5); + node_->declare_parameter("wheel_radius", 0.1); + node_->declare_parameter("center_of_rotation", 0.0); + node_->declare_parameter("front_left_velocity_threshold", 10.0); + node_->declare_parameter("front_right_velocity_threshold", 10.0); + node_->declare_parameter("rear_left_velocity_threshold", 10.0); + node_->declare_parameter("rear_right_velocity_threshold", 10.0); + + // Define expected interfaces + command_interfaces_ = {"front_left_wheel_joint/velocity", "front_right_wheel_joint/velocity", + "rear_left_wheel_joint/velocity", "rear_right_wheel_joint/velocity", + "front_left_axle_joint/position", "front_right_axle_joint/position", + "rear_left_axle_joint/position", "rear_right_axle_joint/position"}; + state_interfaces_ = {"front_left_wheel_joint/velocity", "front_right_wheel_joint/velocity", + "rear_left_wheel_joint/velocity", "rear_right_wheel_joint/velocity", + "front_left_axle_joint/position", "front_right_axle_joint/position", + "rear_left_axle_joint/position", "rear_right_axle_joint/position"}; +} + +void SwerveDriveControllerTest::SetUpController() +{ + RCLCPP_INFO(node_->get_logger(), "Creating SwerveController instance"); + controller_ = std::make_shared(); + if (!controller_) { + RCLCPP_ERROR(node_->get_logger(), "Failed to create SwerveController instance"); + GTEST_FAIL() << "Controller creation failed"; + } + RCLCPP_INFO(node_->get_logger(), "SwerveController instance created"); + + // Initialize the controller's node + RCLCPP_INFO(node_->get_logger(), "Initializing controller node"); + auto init_result = controller_->init( + "swerve_controller_test", // controller_name + "", // urdf (empty for test) + 100, // cm_update_rate (100 Hz) + "", // node_namespace (default) + rclcpp::NodeOptions() // node_options (default) + ); + if (init_result != controller_interface::return_type::OK) { + RCLCPP_ERROR(node_->get_logger(), "Controller node initialization failed"); + GTEST_FAIL() << "Controller node initialization failed"; + } + RCLCPP_INFO(node_->get_logger(), "Controller node initialized"); +} + +void SwerveDriveControllerTest::SetUpInterfaces() +{ + command_values_.resize(command_interfaces_.size(), 0.0); + state_values_.resize(state_interfaces_.size(), 0.0); + command_interfaces_base_.reserve(command_interfaces_.size()); + state_interfaces_base_.reserve(state_interfaces_.size()); + + for (size_t i = 0; i < command_interfaces_.size(); ++i) { + const auto & name = command_interfaces_[i]; + auto interface_type = name.substr(name.find_last_of("/") + 1); + command_interfaces_base_.emplace_back( + hardware_interface::CommandInterface( + name.substr(0, name.find_last_of("/")), interface_type, &command_values_[i])); + command_interface_handles_.emplace_back(command_interfaces_base_.back()); + } + for (size_t i = 0; i < state_interfaces_.size(); ++i) { + const auto & name = state_interfaces_[i]; + auto interface_type = name.substr(name.find_last_of("/") + 1); + state_interfaces_base_.emplace_back( + hardware_interface::StateInterface( + name.substr(0, name.find_last_of("/")), interface_type, &state_values_[i])); + state_interface_handles_.emplace_back(state_interfaces_base_.back()); + } + controller_->assign_interfaces( + std::move(command_interface_handles_), std::move(state_interface_handles_)); +} + +void SwerveDriveControllerTest::PublishTwistStamped( + double linear_x, double linear_y, double angular_z) +{ + auto msg = std::make_shared(); + msg->header.stamp = node_->now(); + msg->twist.linear.x = linear_x; + msg->twist.linear.y = linear_y; + msg->twist.angular.z = angular_z; + twist_stamped_pub_->publish(*msg); +} + +void SwerveDriveControllerTest::PublishTwist(double linear_x, double linear_y, double angular_z) +{ + auto msg = std::make_shared(); + msg->linear.x = linear_x; + msg->linear.y = linear_y; + msg->angular.z = angular_z; + twist_pub_->publish(*msg); +} + +TEST_F(SwerveDriveControllerTest, test_on_init_success) +{ + SetUpController(); + RCLCPP_INFO(node_->get_logger(), "Calling on_init"); + auto result = controller_->on_init(); + EXPECT_EQ(result, controller_interface::CallbackReturn::SUCCESS) + << "on_init returned " << static_cast(result); + RCLCPP_INFO(node_->get_logger(), "on_init completed with result: %d", static_cast(result)); + init_successful_ = (result == controller_interface::CallbackReturn::SUCCESS); +} + +TEST_F(SwerveDriveControllerTest, test_command_interface_configuration) +{ + SetUpController(); + if (!init_successful_) { + GTEST_SKIP() << "Skipping due to on_init failure"; + } + auto config = controller_->command_interface_configuration(); + EXPECT_EQ(config.type, controller_interface::interface_configuration_type::INDIVIDUAL); + EXPECT_EQ(config.names.size(), 8u); + for (const auto & name : command_interfaces_) { + EXPECT_NE(std::find(config.names.begin(), config.names.end(), name), config.names.end()) + << "Expected command interface " << name << " not found"; + } +} + +TEST_F(SwerveDriveControllerTest, test_state_interface_configuration) +{ + SetUpController(); + if (!init_successful_) { + GTEST_SKIP() << "Skipping due to on_init failure"; + } + auto config = controller_->state_interface_configuration(); + EXPECT_EQ(config.type, controller_interface::interface_configuration_type::INDIVIDUAL); + EXPECT_EQ(config.names.size(), 8u); + for (const auto & name : state_interfaces_) { + EXPECT_NE(std::find(config.names.begin(), config.names.end(), name), config.names.end()) + << "Expected state interface " << name << " not found"; + } +} + +TEST_F(SwerveDriveControllerTest, test_on_configure_success) +{ + SetUpController(); + if (!init_successful_) { + GTEST_SKIP() << "Skipping due to on_init failure"; + } + EXPECT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); +} + +TEST_F(SwerveDriveControllerTest, test_on_configure_missing_parameters) +{ + SetUpController(); + if (!init_successful_) { + GTEST_SKIP() << "Skipping due to on_init failure"; + } + node_->undeclare_parameter("joint_wheel_left_front"); + EXPECT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::ERROR); +} + +TEST_F(SwerveDriveControllerTest, test_on_activate_success) +{ + SetUpController(); + if (!init_successful_) { + GTEST_SKIP() << "Skipping due to on_init failure"; + } + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + SetUpInterfaces(); + EXPECT_EQ( + controller_->on_activate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); +} + +TEST_F(SwerveDriveControllerTest, test_on_activate_missing_interfaces) +{ + SetUpController(); + if (!init_successful_) { + GTEST_SKIP() << "Skipping due to on_init failure"; + } + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + EXPECT_EQ( + controller_->on_activate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::ERROR); +} + +TEST_F(SwerveDriveControllerTest, test_on_deactivate_success) +{ + SetUpController(); + if (!init_successful_) { + GTEST_SKIP() << "Skipping due to on_init failure"; + } + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + SetUpInterfaces(); + ASSERT_EQ( + controller_->on_activate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + EXPECT_EQ( + controller_->on_deactivate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); +} + +TEST_F(SwerveDriveControllerTest, test_on_cleanup_success) +{ + SetUpController(); + if (!init_successful_) { + GTEST_SKIP() << "Skipping due to on_init failure"; + } + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + EXPECT_EQ( + controller_->on_cleanup(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); +} + +TEST_F(SwerveDriveControllerTest, test_on_error_success) +{ + SetUpController(); + if (!init_successful_) { + GTEST_SKIP() << "Skipping due to on_init failure"; + } + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + EXPECT_EQ( + controller_->on_error(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); +} + +TEST_F(SwerveDriveControllerTest, test_on_shutdown_success) +{ + SetUpController(); + if (!init_successful_) { + GTEST_SKIP() << "Skipping due to on_init failure"; + } + EXPECT_EQ( + controller_->on_shutdown(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); +} + +TEST_F(SwerveDriveControllerTest, test_update_inactive_state) +{ + SetUpController(); + if (!init_successful_) { + GTEST_SKIP() << "Skipping due to on_init failure"; + } + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + SetUpInterfaces(); + ASSERT_EQ( + controller_->on_activate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + controller_->on_deactivate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + EXPECT_EQ( + controller_->update(node_->now(), rclcpp::Duration::from_seconds(0.02)), + controller_interface::return_type::OK); + for (size_t i = 0; i < 4; ++i) { + EXPECT_EQ(command_values_[i], 0.0) << "Wheel velocity " << i << " not zeroed"; + } + for (size_t i = 4; i < 8; ++i) { + EXPECT_EQ(command_values_[i], 0.0) << "Axle position " << i << " not zeroed"; + } +} + +TEST_F(SwerveDriveControllerTest, test_update_with_velocity_command) +{ + SetUpController(); + if (!init_successful_) { + GTEST_SKIP() << "Skipping due to on_init failure"; + } + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + SetUpInterfaces(); + ASSERT_EQ( + controller_->on_activate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + + // Set up odometry subscriber + odom_sub_ = node_->create_subscription( + "~/odom", 10, [this](const nav_msgs::msg::Odometry::SharedPtr msg) {last_odom_msg_ = msg;}); + + // Set up velocity publisher + twist_stamped_pub_ = node_->create_publisher( + "~/cmd_vel", rclcpp::SystemDefaultsQoS()); + + // Publish a velocity command (1 m/s forward) + PublishTwistStamped(1.0, 0.0, 0.0); + rclcpp::spin_some(node_); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + + // Update controller + auto time = node_->now(); + EXPECT_EQ( + controller_->update(time, rclcpp::Duration::from_seconds(0.02)), + controller_interface::return_type::OK); + + // Spin to receive odometry + rclcpp::spin_some(node_); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + + ASSERT_NE(last_odom_msg_, nullptr); + EXPECT_NEAR(last_odom_msg_->pose.pose.position.x, 0.02, 0.01); // 1 m/s * 0.02 s + EXPECT_NEAR(last_odom_msg_->pose.pose.position.y, 0.0, 0.01); + EXPECT_NEAR(last_odom_msg_->pose.pose.orientation.z, 0.0, 0.01); +} + +TEST_F(SwerveDriveControllerTest, test_update_with_timeout) +{ + SetUpController(); + if (!init_successful_) { + GTEST_SKIP() << "Skipping due to on_init failure"; + } + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + SetUpInterfaces(); + ASSERT_EQ( + controller_->on_activate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + + // Set up velocity publisher + twist_stamped_pub_ = node_->create_publisher( + "~/cmd_vel", rclcpp::SystemDefaultsQoS()); + PublishTwistStamped(1.0, 0.0, 0.0); + rclcpp::spin_some(node_); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + + // Wait beyond timeout (0.5 s) + rclcpp::sleep_for(std::chrono::milliseconds(600)); + + // Update controller + EXPECT_EQ( + controller_->update(node_->now(), rclcpp::Duration::from_seconds(0.02)), + controller_interface::return_type::OK); + + for (size_t i = 0; i < 4; ++i) { + EXPECT_EQ(command_values_[i], 0.0) << "Wheel velocity " << i << " not zeroed"; + } + for (size_t i = 4; i < 8; ++i) { + EXPECT_EQ(command_values_[i], 0.0) << "Axle position " << i << " not zeroed"; + } +} + +TEST_F(SwerveDriveControllerTest, test_unstamped_velocity_command) +{ + SetUpController(); + if (!init_successful_) { + GTEST_SKIP() << "Skipping due to on_init failure"; + } + node_->set_parameter(rclcpp::Parameter("use_stamped_vel", false)); + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + SetUpInterfaces(); + ASSERT_EQ( + controller_->on_activate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + + // Set up publishers and subscribers + twist_pub_ = node_->create_publisher( + "~/cmd_vel_unstamped", rclcpp::SystemDefaultsQoS()); + odom_sub_ = node_->create_subscription( + "~/odom", 10, [this](const nav_msgs::msg::Odometry::SharedPtr msg) {last_odom_msg_ = msg;}); + + // Publish an unstamped velocity command (1 m/s lateral) + PublishTwist(0.0, 1.0, 0.0); + rclcpp::spin_some(node_); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + + // Update controller + auto time = node_->now(); + EXPECT_EQ( + controller_->update(time, rclcpp::Duration::from_seconds(0.02)), + controller_interface::return_type::OK); + + // Spin to receive odometry + rclcpp::spin_some(node_); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + + // Check odometry + ASSERT_NE(last_odom_msg_, nullptr); + EXPECT_NEAR(last_odom_msg_->pose.pose.position.x, 0.0, 0.01); + EXPECT_NEAR(last_odom_msg_->pose.pose.position.y, 0.02, 0.01); // 1 m/s * 0.02 s + EXPECT_NEAR(last_odom_msg_->pose.pose.orientation.z, 0.0, 0.01); +} + +} // namespace swerve_drive_controller + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/swerve_drive_controller/test/test_swerve_drive_controller.hpp b/swerve_drive_controller/test/test_swerve_drive_controller.hpp new file mode 100644 index 0000000000..6d55c0dadc --- /dev/null +++ b/swerve_drive_controller/test/test_swerve_drive_controller.hpp @@ -0,0 +1,58 @@ +// Copyright 2025 (your name or organization) +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_SWERVE_DRIVE_CONTROLLER_HPP_ +#define TEST_SWERVE_DRIVE_CONTROLLER_HPP_ + +#include + +#include +#include +#include + +#include "controller_interface/controller_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/rclcpp.hpp" +#include "swerve_drive_controller/swerve_drive_controller.hpp" + +namespace swerve_drive_controller +{ +class SwerveDriveControllerTest : public ::testing::Test +{ +protected: + void SetUp() override; + void SetUpController(); + void SetUpInterfaces(); + void PublishTwistStamped(double linear_x, double linear_y, double angular_z); + void PublishTwist(double linear_x, double linear_y, double angular_z); + + std::shared_ptr controller_; + std::shared_ptr node_; + std::vector command_interfaces_; + std::vector state_interfaces_; + std::vector command_values_; + std::vector state_values_; + std::vector command_interfaces_base_; + std::vector state_interfaces_base_; + std::vector command_interface_handles_; + std::vector state_interface_handles_; + rclcpp::Publisher::SharedPtr twist_stamped_pub_; + rclcpp::Publisher::SharedPtr twist_pub_; + rclcpp::Subscription::SharedPtr odom_sub_; + nav_msgs::msg::Odometry::SharedPtr last_odom_msg_; + bool init_successful_ = false; +}; + +} // namespace swerve_drive_controller + +#endif // TEST_SWERVE_DRIVE_CONTROLLER_HPP_ From a6b6ac7456dd7a112842a1cd3e416a95ee61f0fc Mon Sep 17 00:00:00 2001 From: nitin Date: Sun, 25 May 2025 22:05:25 +0530 Subject: [PATCH 02/19] Added Swerve Drive In mobile_robot_kinematics.rst and fixed CMakeLists.txt of swerve_drive_controller package Signed-off-by: nitin --- doc/controllers_index.rst | 4 + doc/images/swerve_drive.drawio | 476 +++++++++++++++++ doc/images/swerve_drive.svg | 4 + doc/mobile_robot_kinematics.rst | 498 ++++++++++++++++++ swerve_drive_controller/CMakeLists.txt | 97 ++-- .../swerve_drive_controller.hpp | 6 +- .../swerve_drive_kinematics.hpp | 2 +- swerve_drive_controller/package.xml | 20 +- .../src/swerve_drive_controller.cpp | 47 +- .../src/swerve_drive_kinematics.cpp | 5 +- .../test_load_swerve_drive_controller.cpp | 2 +- .../test/test_swerve_drive_controller.cpp | 2 +- .../test/test_swerve_drive_controller.hpp | 2 +- 13 files changed, 1071 insertions(+), 94 deletions(-) create mode 100644 doc/images/swerve_drive.drawio create mode 100644 doc/images/swerve_drive.svg diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index 235800711b..cf40cd1bf8 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -31,6 +31,10 @@ Controllers for Wheeled Mobile Robots Mecanum Drive Controllers <../mecanum_drive_controller/doc/userdoc.rst> Steering Controllers Library <../steering_controllers_library/doc/userdoc.rst> Tricycle Controller <../tricycle_controller/doc/userdoc.rst> +<<<<<<< HEAD +======= + Swerve Drive Controller <../swerve_drive_controller/doc/userdoc.rst> +>>>>>>> 04f23250 (Added Swerve Drive In mobile_robot_kinematics.rst and fixed CMakeLists.txt of swerve_drive_controller package) Controllers for Manipulators and Other Robots ********************************************* diff --git a/doc/images/swerve_drive.drawio b/doc/images/swerve_drive.drawio new file mode 100644 index 0000000000..cfea2c73a9 --- /dev/null +++ b/doc/images/swerve_drive.drawio @@ -0,0 +1,476 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/doc/images/swerve_drive.svg b/doc/images/swerve_drive.svg new file mode 100644 index 0000000000..72190a4e70 --- /dev/null +++ b/doc/images/swerve_drive.svg @@ -0,0 +1,4 @@ + + + +
Moving Forward
xb
yb
0
2
3
1
vb,x
vb,y

wb,z

xb
yb
vb,x
vb,y
0
1
2
3
Moving Right

wb,z

xb
yb
vb,x
vb,y
0
1
2
3
Moving Left

wb,z

xw
yw
yb
xb
0
1
vb,x
vb,y
3
2

wb,z

Rotating Clockwise
 \ No newline at end of file diff --git a/doc/mobile_robot_kinematics.rst b/doc/mobile_robot_kinematics.rst index b861733ee7..ff0d2526b2 100644 --- a/doc/mobile_robot_kinematics.rst +++ b/doc/mobile_robot_kinematics.rst @@ -306,6 +306,504 @@ holds. But to get a more robust solution, we take the average of both , i.e., v_{b,x} = 0.5 \left(v_{rear,left} \frac{R_b}{R_b - w_r/2} + v_{rear,right} \frac{R_b}{R_b + w_r/2}\right). +Ackermann Steering +,,,,,,,,,,,,,,,,,,,,, + +The following image shows a four-wheeled robot with two independent steering wheels in the front. + +.. image:: images/ackermann_steering.svg + :width: 550 + :align: center + :alt: A car-like robot with two steering wheels at the front + +* :math:`w_f` is the wheel track of the front axle, measured between the two kingpins. + +To prevent the front wheels from slipping, the steering angle of the front wheels cannot be equal. +This is the so-called **Ackermann steering**. + +.. note:: + Ackermann steering can also be achieved by a `mechanical linkage between the two front wheels `__. In this case the robot has only one steering input, and the steering angle of the two front wheels is mechanically coupled. The inverse kinematics of the robot will then be the same as in the car-like model above. + +**Forward Kinematics** + +The forward kinematics is the same as for the car-like model above. + +**Inverse Kinematics** + +The turning radius of the robot is + +.. math:: + R_b = \frac{l}{\tan(\phi)} + +Then the steering angles of the front wheels must satisfy these conditions to avoid skidding + +.. math:: + \phi_{left} &= \arctan\left(\frac{l}{R_b - w_f/2}\right) &= \arctan\left(\frac{2l\sin(\phi)}{2l\cos(\phi) - w_f\sin(\phi)}\right)\\ + \phi_{right} &= \arctan\left(\frac{l}{R_b + w_f/2}\right) &= \arctan\left(\frac{2l\sin(\phi)}{2l\cos(\phi) + w_f\sin(\phi)}\right) + +**Odometry** + +The calculation of :math:`\phi` from two angle measurements of the steering axle is overdetermined. +If there is no slip and the measurements are ideal, + +.. math:: + \phi = \arctan\left(\frac{l\tan(\phi_{left})}{l + w_f/2 \tan(\phi_{left})}\right) = \arctan\left(\frac{l\tan(\phi_{right})}{l - w_f/2 \tan(\phi_{right})}\right) + +holds. But to get a more robust solution, we take the average of both , i.e., + +.. math:: + \phi = 0.5 \left(\arctan\left(\frac{l\tan(\phi_{left})}{l + w_f/2 \tan(\phi_{left})}\right) + \arctan\left(\frac{l\tan(\phi_{right})}{l - w_f/2 \tan(\phi_{right})}\right)\right). + +Ackermann Steering with Traction +,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,, + +The following image shows a four-wheeled car-like robot with two independent steering wheels at the front, which are also driven independently. + +.. image:: images/ackermann_steering_traction.svg + :width: 550 + :align: center + :alt: A car-like robot with two steering wheels at the front, which are also independently driven. + +* :math:`d_{kp}` is the distance from the kingpin to the contact point of the front wheel with the ground. + +**Forward Kinematics** + +The forward kinematics is the same as the car-like model above. + +**Inverse Kinematics** + +To avoid slipping of the front wheels, the velocity of the front wheels cannot be equal and + +.. math:: + \frac{v_{front,left}}{R_{left}} = \frac{v_{front,right}}{R_{right}} = \frac{v_{b,x}}{R_b} + +with turning radius of the robot and the left/right front wheel + +.. math:: + R_b &= \frac{l}{\tan(\phi)} \\ + R_{left} &= \frac{l-d_{kp}\sin(\phi_{left})}{\sin(\phi_{left})}\\ + R_{right} &= \frac{l+d_{kp}\sin(\phi_{right})}{\sin(\phi_{right})}. + +This results in the following inverse kinematics equations + +.. math:: + v_{front,left} &= \frac{v_{b,x}(l-d_{kp}\sin(\phi_{left}))}{R_b\sin(\phi_{left})}\\ + v_{front,right} &= \frac{v_{b,x}(l+d_{kp}\sin(\phi_{right}))}{R_b\sin(\phi_{right})} + +with the steering angles of the front wheels from the Ackermann steering equations above. + +**Odometry** + +The calculation of :math:`v_{b,x}` from two encoder measurements of the traction axle is again overdetermined. +If there is no slip and the encoders are ideal, + +.. math:: + v_{b,x} = v_{front,left} \frac{R_b\sin(\phi_{left})}{l-d_{kp}\sin(\phi_{left})} = v_{front,right} \frac{R_b\sin(\phi_{right})}{l+d_{kp}\sin(\phi_{right})} + +holds. But to get a more robust solution, we take the average of both , i.e., + +.. math:: + v_{b,x} = 0.5 \left( v_{front,left} \frac{R_b\sin(\phi_{left})}{l-d_{kp}\sin(\phi_{left})} + v_{front,right} \frac{R_b\sin(\phi_{right})}{l+d_{kp}\sin(\phi_{right})}\right). +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/doc/mobile_robot_kinematics.rst + +.. _mobile_robot_kinematics: + +Wheeled Mobile Robot Kinematics +-------------------------------------------------------------- + +.. _siciliano: https://link.springer.com/book/10.1007/978-1-84628-642-1 +.. _modern_robotics: http://modernrobotics.org/ + +This page introduces the kinematics of different wheeled mobile robots. For further reference see `Siciliano et.al - Robotics: Modelling, Planning and Control `_ and `Kevin M. Lynch and Frank C. Park - Modern Robotics: Mechanics, Planning, And Control `_. + +Wheeled mobile robots can be classified in two categories: + +Omnidirectional robots + which can move instantaneously in any direction in the plane, and + +Nonholonomic robots + which cannot move instantaneously in any direction in the plane. + +The forward integration of the kinematic model using the encoders of the wheel actuators — is referred to as **odometric localization** or **passive localization** or **dead reckoning**. We will call it just **odometry**. + +Omnidirectional Wheeled Mobile Robots +..................................... + +Omnidirectional Drive Robots using Omni Wheels +,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,, + +The below explains the kinematics of omnidirectional drive robots using 3 or more omni wheels. +It follows the coordinate conventions defined in `ROS REP 103 `__. + +.. image:: images/omni_wheel_omnidirectional_drive.svg + :width: 550 + :align: center + :alt: Omnidirectional Drive Robot using Omni Wheels + +* :math:`x_b,y_b` is the robot's body-frame coordinate system, located at the contact point of the wheel on the ground. +* :math:`x_w,y_w` is the world coordinate system. +* :math:`v_{b,x},` is the robot's linear velocity on the x-axis. +* :math:`v_{b,y}` is the robot's linear velocity on the y-axis. +* :math:`\omega_{b,z}` is the robot's angular velocity on the z-axis. +* :math:`R` is the robot's radius / the distance between the robot's center and the wheels. +* Red arrows on the wheel :math:`i` signify the positive direction of their rotation :math:`\omega_i` +* :math:`\gamma` is the angular offset of the first wheel from :math:`x_b`. +* :math:`\theta` is the angle between each wheel which can be calculated using the below equation where :math:`n` is the number of wheels. + +.. math:: + + θ = \frac{2\pi}{n} + +**Inverse Kinematics** + +The necessary angular velocity of the wheels to achieve a desired body twist can be calculated using the below matrix: + +.. math:: + + A = + \begin{bmatrix} + \sin(\gamma) & -\cos(\gamma) & -R \\ + \sin(\theta + \gamma) & -\cos(\theta + \gamma) & -R\\ + \sin(2\theta + \gamma) & -\cos(2\theta + \gamma) & -R\\ + \sin(3\theta + \gamma) & -\cos(3\theta + \gamma) & -R\\ + \vdots & \vdots & \vdots\\ + \sin((n-1)\theta + \gamma) & -\cos((n-1)\theta + \gamma) & -R\\ + \end{bmatrix} + +.. math:: + + \begin{bmatrix} + \omega_1\\ + \omega_2\\ + \omega_3\\ + \omega_4\\ + \vdots\\ + \omega_n + \end{bmatrix} = + \frac{1}{r} + A + \begin{bmatrix} + v_{b,x}\\ + v_{b,y}\\ + \omega_{b,z}\\ + \end{bmatrix} + +Here :math:`\omega_1,\ldots,\omega_n` are the angular velocities of the wheels and :math:`r` is the radius of the wheels. +These equations can be written in algebraic form for any wheel :math:`i` like this: + +.. math:: + \omega_i = \frac{\sin((i-1)\theta + \gamma) v_{b,x} - \cos((i-1)\theta + \gamma) v_{b,y} - R \omega_{b,z}}{r} + +**Forward Kinematics** + +The body twist of the robot can be obtained from the wheel velocities by using the pseudoinverse of matrix :math:`A`. + +.. math:: + + \begin{bmatrix} + v_{b,x}\\ + v_{b,y}\\ + \omega_{b,z}\\ + \end{bmatrix} = + rA^\dagger + \begin{bmatrix} + \omega_1\\ + \omega_2\\ + \omega_3\\ + \omega_4\\ + \vdots\\ + \omega_n + \end{bmatrix} + + + +Swerve Drive Robots +,,,,,,,,,,,,,,,,,,, + +The below explains the kinematics of omnidirectional drive robots using four swerve modules, each with independently controlled steering and driving motors. It follows the coordinate conventions defined in `ROS REP 103 `__. + +.. image:: images/swerve_drive.svg + :width: 550 + :align: center + :alt: Swerve Drive Robot + +* :math:`x_b, y_b` is the robot's body-frame coordinate system, located at the geometric center of the robot. +* :math:`x_w, y_w` is the world coordinate system. +* :math:`v_{b,x}` is the robot's linear velocity on the x-axis. +* :math:`v_{b,y}` is the robot's linear velocity on the y-axis. +* :math:`\omega_{b,z}` is the robot's angular velocity on the z-axis. +* :math:`l` is the wheelbase (distance between front and rear wheels). +* :math:`w` is the track width (distance between left and right wheels). +* Red arrows on wheel :math:`i` signify the direction of the wheel's velocity :math:`v_i`. + +Each swerve module :math:`i` (for :math:`i = 0, 1, 2, 3`, typically front-left, front-right, back-left, back-right) is located at :math:`(l_{i,x}, l_{i,y})` relative to the center, typically: +- Front-left: :math:`(l/2, w/2)` +- Front-right: :math:`(l/2, -w/2)` +- Back-left: :math:`(-l/2, w/2)` +- Back-right: :math:`(-l/2, -w/2)` + +**Inverse Kinematics** + +The necessary wheel velocities and steering angles to achieve a desired body twist are computed by the `SwerveDriveKinematics` class in the `swerve_drive_controller` package. For each module :math:`i` at position :math:`(l_{i,x}, l_{i,y})`, the velocity vector is: + +.. math:: + + \begin{bmatrix} + v_{i,x} \\ + v_{i,y} + \end{bmatrix} + = + \begin{bmatrix} + v_{b,x} - \omega_{b,z} l_{i,y} \\ + v_{b,y} + \omega_{b,z} l_{i,x} + \end{bmatrix} + +The wheel velocity :math:`v_i` and steering angle :math:`\phi_i` are: + +.. math:: + + v_i = \sqrt{v_{i,x}^2 + v_{i,y}^2} + +.. math:: + + \phi_i = \arctan2(v_{i,y}, v_{i,x}) + + +**Forward Kinematics** + +The body twist of the robot is computed from the wheel velocities :math:`v_i` and steering angles :math:`\phi_i`. Each module’s velocity components in the body frame are: + +.. math:: + + v_{i,x} = v_i \cos(\phi_i), \quad v_{i,y} = v_i \sin(\phi_i) + +The chassis velocities are calculated as: + +.. math:: + + v_{b,x} = \frac{1}{4} \sum_{i=0}^{3} v_{i,x}, \quad v_{b,y} = \frac{1}{4} \sum_{i=0}^{3} v_{i,y} + +.. math:: + + \omega_{b,z} = \frac{\sum_{i=0}^{3} (v_{i,y} l_{i,x} - v_{i,x} l_{i,y})}{\sum_{i=0}^{3} (l_{i,x}^2 + l_{i,y}^2)} + + +**Odometry** + +The `SwerveDriveKinematics` class updates the robot’s pose (:math:`x`, :math:`y`, :math:`\theta`) in the global frame using the computed chassis velocities. The global velocities are: + +.. math:: + + v_{x,\text{global}} = v_{b,x} \cos(\theta) - v_{b,y} \sin(\theta) + +.. math:: + + v_{y,\text{global}} = v_{b,x} \sin(\theta) + v_{b,y} \cos(\theta) + +The pose is updated via Euler integration over time step :math:`\Delta t`: + +.. math:: + + \dot{x} = v_{x,\text{global}}, \quad \dot{y} = v_{y,\text{global}}, \quad \dot{\theta} = \omega_{b,z} + + +Nonholonomic Wheeled Mobile Robots +..................................... + +Unicycle model +,,,,,,,,,,,,,,,, + +To define the coordinate systems (`ROS coordinate frame conventions `__, the coordinate systems follow the right-hand rule), consider the following simple unicycle model + +.. image:: images/unicycle.svg + :width: 550 + :align: center + :alt: Unicycle + +* :math:`x_b,y_b` is the robot's body-frame coordinate system, located at the contact point of the wheel on the ground. +* :math:`x_w,y_w` is the world coordinate system. +* :math:`x,y` are the robot's Cartesian coordinates in the world coordinate system. +* :math:`\theta` is the robot's heading angle, i.e. the orientation of the robot's :math:`x_b`-axis w.r.t. the world's :math:`x_w`-axis. + +In the following, we want to command the robot with a desired body twist + +.. math:: + + \vec{\nu}_b = \begin{bmatrix} + \vec{\omega}_{b} \\ + \vec{v}_{b} + \end{bmatrix}, + +where :math:`\vec{v}_{b}` is the linear velocity of the robot in its body-frame, and :math:`\vec\omega_{b}` is the angular velocity of the robot in its body-frame. As we consider steering robots on a flat surface, it is sufficient to give + +* :math:`v_{b,x}`, i.e. the linear velocity of the robot in direction of the :math:`x_b` axis. +* :math:`\omega_{b,z}`, i.e. the angular velocity of the robot about the :math:`x_z` axis. + +as desired system inputs. The forward kinematics of the unicycle can be calculated with + +.. math:: + \dot{x} &= v_{b,x} \cos(\theta) \\ + \dot{y} &= v_{b,x} \sin(\theta) \\ + \dot{\theta} &= \omega_{b,z} + +We will formulate the inverse kinematics to calculate the desired commands for the robot (wheel speed or steering) from the given body twist. + +Differential Drive Robot +,,,,,,,,,,,,,,,,,,,,,,,, + +Citing `Siciliano et.al - Robotics: Modelling, Planning and Control `_: + +.. code-block:: text + + A unicycle in the strict sense (i.e., a vehicle equipped with a single wheel) + is a robot with a serious problem of balance in static conditions. However, + there exist vehicles that are kinematically equivalent to a unicycle but more + stable from a mechanical viewpoint. + +One of these vehicles is the differential drive robot, which has two wheels, each of which is driven independently. + +.. image:: images/diff_drive.svg + :width: 550 + :align: center + :alt: Differential drive robot + +* :math:`w` is the wheel track (the distance between the wheels). + +**Forward Kinematics** + +The forward kinematics of the differential drive model can be calculated from the unicycle model above using + +.. math:: + v_{b,x} &= \frac{v_{right} + v_{left}}{2} \\ + \omega_{b,z} &= \frac{v_{right} - v_{left}}{w} + +**Inverse Kinematics** + +The necessary wheel speeds to achieve a desired body twist can be calculated with: + +.. math:: + + v_{left} &= v_{b,x} - \omega_{b,z} w / 2 \\ + v_{right} &= v_{b,x} + \omega_{b,z} w / 2 + + +**Odometry** + +We can use the forward kinematics equations above to calculate the robot's odometry directly from the encoder readings. + +Car-Like (Bicycle) Model +,,,,,,,,,,,,,,,,,,,,,,,, + +The following picture shows a car-like robot with two wheels, where the front wheel is steerable. This model is also known as the bicycle model. + +.. image:: images/car_like_robot.svg + :width: 550 + :align: center + :alt: Car-like robot + +* :math:`\phi` is the steering angle of the front wheel, counted positive in direction of rotation around :math:`x_z`-axis. +* :math:`v_{rear}, v_{front}` is the velocity of the rear and front wheel. +* :math:`l` is the wheelbase. + +We assume that the wheels are rolling without slipping. This means that the velocity of the contact point of the wheel with the ground is zero and the wheel's velocity points in the direction perpendicular to the wheel's axis. The **Instantaneous Center of Rotation** (ICR), i.e. the center of the circle around which the robot rotates, is located at the intersection of the lines that are perpendicular to the wheels' axes and pass through the contact points of the wheels with the ground. + +As a consequence of the no-slip condition, the velocity of the two wheels must satisfy the following constraint: + +.. math:: + v_{rear} = v_{front} \cos(\phi) + +**Forward Kinematics** + +The forward kinematics of the car-like model can be calculated with + +.. math:: + \dot{x} &= v_{b,x} \cos(\theta) \\ + \dot{y} &= v_{b,x} \sin(\theta) \\ + \dot{\theta} &= \frac{v_{b,x}}{l} \tan(\phi) + + +**Inverse Kinematics** + +The steering angle is one command input of the robot: + +.. math:: + \phi = \arctan\left(\frac{l w_{b,z}}{v_{b,x}} \right) + + +For the rear-wheel drive, the velocity of the rear wheel is the second input of the robot: + +.. math:: + v_{rear} = v_{b,x} + + +For the front-wheel drive, the velocity of the front wheel is the second input of the robot: + +.. math:: + v_{front} = \frac{v_{b,x}}{\cos(\phi)} + +**Odometry** + +We have to distinguish between two cases: Encoders on the rear wheel or on the front wheel. + +For the rear wheel case: + +.. math:: + \dot{x} &= v_{rear} \cos(\theta) \\ + \dot{y} &= v_{rear} \sin(\theta) \\ + \dot{\theta} &= \frac{v_{rear}}{l} \tan(\phi) + + +For the front wheel case: + +.. math:: + \dot{x} &= v_{front} \cos(\theta) \cos(\phi)\\ + \dot{y} &= v_{front} \sin(\theta) \cos(\phi)\\ + \dot{\theta} &= \frac{v_{front}}{l} \sin(\phi) + + +Double-Traction Axle +,,,,,,,,,,,,,,,,,,,,, + +The following image shows a car-like robot with three wheels, with two independent traction wheels at the rear. + +.. image:: images/double_traction.svg + :width: 550 + :align: center + :alt: A car-like robot with two traction wheels at the rear + +* :math:`w_r` is the wheel track of the rear axle. + +**Forward Kinematics** + +The forward kinematics is the same as the car-like model above. + +**Inverse Kinematics** + +The turning radius of the robot is + +.. math:: + R_b = \frac{l}{\tan(\phi)} + +Then the velocity of the rear wheels must satisfy these conditions to avoid skidding + +.. math:: + v_{rear,left} &= v_{b,x}\frac{R_b - w_r/2}{R_b}\\ + v_{rear,right} &= v_{b,x}\frac{R_b + w_r/2}{R_b} + +**Odometry** + +The calculation of :math:`v_{b,x}` from two encoder measurements of the traction axle is overdetermined. +If there is no slip and the encoders are ideal, + +.. math:: + v_{b,x} = v_{rear,left} \frac{R_b}{R_b - w_r/2} = v_{rear,right} \frac{R_b}{R_b + w_r/2} + +holds. But to get a more robust solution, we take the average of both , i.e., + +.. math:: + v_{b,x} = 0.5 \left(v_{rear,left} \frac{R_b}{R_b - w_r/2} + v_{rear,right} \frac{R_b}{R_b + w_r/2}\right). + + Ackermann Steering ,,,,,,,,,,,,,,,,,,,,, diff --git a/swerve_drive_controller/CMakeLists.txt b/swerve_drive_controller/CMakeLists.txt index f063743e0f..c47e5c16e1 100644 --- a/swerve_drive_controller/CMakeLists.txt +++ b/swerve_drive_controller/CMakeLists.txt @@ -1,85 +1,78 @@ cmake_minimum_required(VERSION 3.8) project(swerve_drive_controller) -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(tf2 REQUIRED) -find_package(rclcpp REQUIRED) -find_package(nav_msgs REQUIRED) -find_package(tf2_msgs REQUIRED) -find_package(hardware_interface REQUIRED) -find_package(controller_interface REQUIRED) -find_package(pluginlib REQUIRED) -find_package(rclcpp_lifecycle REQUIRED) -find_package(realtime_tools REQUIRED) - -pluginlib_export_plugin_description_file(controller_interface swerve_drive_plugin.xml) - -add_library(swerve_drive_controller SHARED - src/swerve_drive_controller.cpp - src/swerve_drive_kinematics.cpp -) +find_package(ros2_control_cmake REQUIRED) +set_compiler_options() +export_windows_symbols() -target_include_directories(swerve_drive_controller PRIVATE include) -ament_target_dependencies(swerve_drive_controller - builtin_interfaces +set(THIS_PACKAGE_INCLUDE_DEPENDS controller_interface + generate_parameter_library geometry_msgs hardware_interface nav_msgs pluginlib rclcpp rclcpp_lifecycle + rcpputils realtime_tools tf2 tf2_msgs ) -install(DIRECTORY include/ - DESTINATION include +find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() +add_compile_definitions(RCPPUTILS_VERSION_MAJOR=${rcpputils_VERSION_MAJOR}) +add_compile_definitions(RCPPUTILS_VERSION_MINOR=${rcpputils_VERSION_MINOR}) + +generate_parameter_library(swerve_drive_controller_parameters + src/swerve_drive_controller_parameter.yaml ) -install(TARGETS swerve_drive_controller - EXPORT export_swerve_drive_controller - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib - RUNTIME DESTINATION bin +add_library(swerve_drive_controller SHARED + src/swerve_drive_controller.cpp + src/swerve_drive_kinematics.cpp ) -install( - FILES swerve_drive_plugin.xml - DESTINATION share/${PROJECT_NAME} +target_compile_features(swerve_drive_controller PUBLIC cxx_std_17) +target_include_directories(swerve_drive_controller PUBLIC + $ + $ ) +target_link_libraries(swerve_drive_controller + PUBLIC + swerve_drive_controller_parameters) +ament_target_dependencies(swerve_drive_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) +pluginlib_export_plugin_description_file(controller_interface swerve_drive_plugin.xml) if(BUILD_TESTING) find_package(controller_manager REQUIRED) - find_package(ament_lint_auto REQUIRED) find_package(ament_cmake_gmock REQUIRED) find_package(ament_cmake_gtest REQUIRED) - find_package(ament_cmake_uncrustify REQUIRED) find_package(ros2_control_test_assets REQUIRED) ament_add_gtest(test_swerve_drive_controller test/test_swerve_drive_controller.cpp ) - target_include_directories(test_swerve_drive_controller PRIVATE include) + target_link_libraries(test_swerve_drive_controller + swerve_drive_controller + ) ament_target_dependencies(test_swerve_drive_controller + geometry_msgs controller_interface hardware_interface - rclcpp - geometry_msgs nav_msgs - ) - target_link_libraries(test_swerve_drive_controller - ${PROJECT_NAME} + rclcpp + rclcpp_lifecycle + realtime_tools + tf2 + tf2_msgs ) add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") @@ -88,11 +81,21 @@ if(BUILD_TESTING) controller_manager ros2_control_test_assets ) - ament_lint_auto_find_test_dependencies() endif() -ament_export_libraries(swerve_drive_controller) -ament_export_dependencies(controller_interface hardware_interface pluginlib rclcpp rclcpp_lifecycle) +install( + DIRECTORY include/ + DESTINATION include/swerve_drive_controller +) +install(TARGETS swerve_drive_controller swerve_drive_controller_parameters + EXPORT export_swerve_drive_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +ament_export_libraries(swerve_drive_controller) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_package() diff --git a/swerve_drive_controller/include/swerve_drive_controller/swerve_drive_controller.hpp b/swerve_drive_controller/include/swerve_drive_controller/swerve_drive_controller.hpp index 95d23138f6..c08743b57a 100644 --- a/swerve_drive_controller/include/swerve_drive_controller/swerve_drive_controller.hpp +++ b/swerve_drive_controller/include/swerve_drive_controller/swerve_drive_controller.hpp @@ -1,4 +1,4 @@ -// Copyright 2025 (your name or organization) +// Copyright 2025 ros2_control development team // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at @@ -56,7 +56,7 @@ class Wheel private: std::reference_wrapper velocity_; std::reference_wrapper feedback_; - std::string name; + std::string name_; }; class Axle @@ -73,7 +73,7 @@ class Axle private: std::reference_wrapper position_; std::reference_wrapper feedback_; - std::string name; + std::string name_; }; class SwerveController : public controller_interface::ControllerInterface diff --git a/swerve_drive_controller/include/swerve_drive_controller/swerve_drive_kinematics.hpp b/swerve_drive_controller/include/swerve_drive_controller/swerve_drive_kinematics.hpp index 4c1e257f53..38c2559218 100644 --- a/swerve_drive_controller/include/swerve_drive_controller/swerve_drive_kinematics.hpp +++ b/swerve_drive_controller/include/swerve_drive_controller/swerve_drive_kinematics.hpp @@ -1,4 +1,4 @@ -// Copyright 2025 (your name or organization) +// Copyright 2025 ros2_control development team // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at diff --git a/swerve_drive_controller/package.xml b/swerve_drive_controller/package.xml index 3899b87c03..e64969948b 100644 --- a/swerve_drive_controller/package.xml +++ b/swerve_drive_controller/package.xml @@ -2,37 +2,35 @@ swerve_drive_controller - 0.0.0 - TODO: Package description - knight + 5.0.0 + Controller for a 4 wheel swerve drive robot base. + Nitin Maurya Apache-2.0 ament_cmake - ament_cmake_ros - + generate_parameter_library + ros2_control_cmake + + backward_ros geometry_msgs hardware_interface control_toolbox controller_interface nav_msgs + pluginlib rclcpp rclcpp_lifecycle + rcpputils realtime_tools tf2 tf2_msgs - ament_lint_auto - ament_lint_common - ament_cmake_uncrustify - ament_cmake_cpplint - ament_cmake_clang_format controller_manager ament_cmake_gtest ament_cmake_gmock hardware_interface_testing ros2_control_test_assets - pluginlib ament_cmake diff --git a/swerve_drive_controller/src/swerve_drive_controller.cpp b/swerve_drive_controller/src/swerve_drive_controller.cpp index d7a0057a9a..2b5f607025 100644 --- a/swerve_drive_controller/src/swerve_drive_controller.cpp +++ b/swerve_drive_controller/src/swerve_drive_controller.cpp @@ -1,4 +1,4 @@ -// Copyright 2025 (your name or organization) +// Copyright 2025 ros2_control development team // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at @@ -49,7 +49,7 @@ using lifecycle_msgs::msg::State; Wheel::Wheel( std::reference_wrapper velocity, std::reference_wrapper feedback, std::string name) -: velocity_(velocity), feedback_(feedback), name(std::move(name)) +: velocity_(velocity), feedback_(feedback), name_(std::move(name)) { } @@ -60,7 +60,7 @@ double Wheel::get_feedback() {return Wheel::feedback_.get().get_optional().value Axle::Axle( std::reference_wrapper position, std::reference_wrapper feedback, std::string name) -: position_(position), feedback_(feedback), name(std::move(name)) +: position_(position), feedback_(feedback), name_(std::move(name)) { } @@ -68,12 +68,12 @@ void Axle::set_position(double position) {position_.get().set_value(position);} double Axle::get_feedback() {return Axle::feedback_.get().get_optional().value();} -std::array, 4> wheel_positions_ = { - std::make_pair(-0.1, 0.175), // front left - std::make_pair(0.1, 0.175), // front right - std::make_pair(-0.1, -0.175), // rear left - std::make_pair(0.1, -0.175) // rear right -}; +std::array, 4> wheel_positions_ = {{ + {-0.1, 0.175}, // front left + {0.1, 0.175}, // front right + {-0.1, -0.175}, // rear left + {0.1, -0.175} // rear right +}}; SwerveController::SwerveController() : controller_interface::ControllerInterface(), swerveDriveKinematics_(wheel_positions_) @@ -107,7 +107,8 @@ CallbackReturn SwerveController::on_init() auto_declare("chassis_width", wheel_params_.y_offset); auto_declare("wheel_radius", wheel_params_.radius); auto_declare("center_of_rotation", wheel_params_.center_of_rotation); - auto_declare("cmd_vel_timeout", cmd_vel_timeout_.count() / 1000.0); + // auto_declare("cmd_vel_timeout", cmd_vel_timeout_.count() / 1000.0); + auto_declare("cmd_vel_timeout", static_cast(cmd_vel_timeout_.count()) / 1000.0); auto_declare("use_stamped_vel", use_stamped_vel_); auto_declare("cmd_vel_topic", cmd_vel_topic_); auto_declare("odom", odometry_topic_); @@ -470,9 +471,9 @@ controller_interface::return_type SwerveController::update( {wheel_command_[2], rear_left_velocity_threshold_, "rear_left_wheel"}, {wheel_command_[3], rear_right_velocity_threshold_, "rear_right_wheel"}}; - for (const auto & [command, threshold, label] : wheel_data) { - if (command.drive_velocity > threshold) { - command.drive_velocity = threshold; + for (const auto & [wheel_command, threshold, label] : wheel_data) { + if (wheel_command.drive_velocity > threshold) { + wheel_command.drive_velocity = threshold; } } @@ -526,10 +527,10 @@ controller_interface::return_type SwerveController::update( swerve_drive_controller::OdometryState odometry_; if (open_loop_) { - std::array velocity_array = { - front_left_velocity, front_right_velocity, rear_left_velocity, rear_right_velocity}; - std::array steering_angles = { - front_left_angle, front_right_angle, rear_left_angle, rear_right_angle}; + std::array velocity_array = {{ + front_left_velocity, front_right_velocity, rear_left_velocity, rear_right_velocity}}; + std::array steering_angles = {{ + front_left_angle, front_right_angle, rear_left_angle, rear_right_angle}}; odometry_ = swerveDriveKinematics_.update_odometry(velocity_array, steering_angles, update_dt.seconds()); } else { @@ -543,12 +544,12 @@ controller_interface::return_type SwerveController::update( double rear_left_wheel_velocity = rear_left_wheel_handle_->get_feedback(); double rear_right_wheel_velocity = rear_right_wheel_handle_->get_feedback(); - std::array velocity_feedback_array = { + std::array velocity_feedback_array = {{ front_left_wheel_velocity, front_right_wheel_velocity, rear_left_wheel_velocity, - rear_right_wheel_velocity}; - std::array steering_angles = { + rear_right_wheel_velocity}}; + std::array steering_angles = {{ front_left_wheel_angle, front_right_wheel_angle, rear_left_wheel_angle, - rear_right_wheel_angle}; + rear_right_wheel_angle}}; odometry_ = swerveDriveKinematics_.update_odometry( velocity_feedback_array, steering_angles, update_dt.seconds()); @@ -557,16 +558,12 @@ controller_interface::return_type SwerveController::update( tf2::Quaternion orientation; orientation.setRPY(0.0, 0.0, odometry_.theta); - bool should_publish = false; - try { if (previous_publish_timestamp_ + publish_period_ < time) { previous_publish_timestamp_ += publish_period_; - should_publish = true; } } catch (const std::runtime_error &) { previous_publish_timestamp_ = time; - should_publish = true; } if (realtime_odometry_publisher_ && realtime_odometry_publisher_->trylock()) { diff --git a/swerve_drive_controller/src/swerve_drive_kinematics.cpp b/swerve_drive_controller/src/swerve_drive_kinematics.cpp index c2af9b1c57..de6034000a 100644 --- a/swerve_drive_controller/src/swerve_drive_kinematics.cpp +++ b/swerve_drive_controller/src/swerve_drive_kinematics.cpp @@ -1,4 +1,4 @@ -// Copyright 2025 (your name or organization) +// Copyright 2025 ros2_control development team // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at @@ -32,9 +32,6 @@ std::array SwerveDriveKinematics::compute_wheel_commands( for (std::size_t i = 0; i < 4; i++) { const auto & [wx, wy] = wheel_positions_[i]; - // double vx = linear_velocity_x + angular_velocity_z * wy * ((i<2) ? 1:-1); - // double vy = linear_velocity_y + angular_velocity_z * wx * ((i%2 == 0) ? 1 : -1); - double vx = linear_velocity_x - angular_velocity_z * wy; double vy = linear_velocity_y + angular_velocity_z * wx; diff --git a/swerve_drive_controller/test/test_load_swerve_drive_controller.cpp b/swerve_drive_controller/test/test_load_swerve_drive_controller.cpp index e75cf27edf..cd73a06815 100644 --- a/swerve_drive_controller/test/test_load_swerve_drive_controller.cpp +++ b/swerve_drive_controller/test/test_load_swerve_drive_controller.cpp @@ -1,4 +1,4 @@ -// Copyright 2025 (your name or organization) +// Copyright 2025 ros2_control development team // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at diff --git a/swerve_drive_controller/test/test_swerve_drive_controller.cpp b/swerve_drive_controller/test/test_swerve_drive_controller.cpp index 207788eda2..b698b19f1c 100644 --- a/swerve_drive_controller/test/test_swerve_drive_controller.cpp +++ b/swerve_drive_controller/test/test_swerve_drive_controller.cpp @@ -1,4 +1,4 @@ -// Copyright 2025 (your name or organization) +// Copyright 2025 ros2_control development team // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at diff --git a/swerve_drive_controller/test/test_swerve_drive_controller.hpp b/swerve_drive_controller/test/test_swerve_drive_controller.hpp index 6d55c0dadc..2bb21daf8a 100644 --- a/swerve_drive_controller/test/test_swerve_drive_controller.hpp +++ b/swerve_drive_controller/test/test_swerve_drive_controller.hpp @@ -1,4 +1,4 @@ -// Copyright 2025 (your name or organization) +// Copyright 2025 ros2_control development team // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at From 647b161905f6af571b9d6f0c1e5cf681d65ebc33 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 5 Jun 2025 13:20:53 +0100 Subject: [PATCH 03/19] Apply suggestions from code review --- doc/controllers_index.rst | 3 --- 1 file changed, 3 deletions(-) diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index cf40cd1bf8..2b77b9b55f 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -31,10 +31,7 @@ Controllers for Wheeled Mobile Robots Mecanum Drive Controllers <../mecanum_drive_controller/doc/userdoc.rst> Steering Controllers Library <../steering_controllers_library/doc/userdoc.rst> Tricycle Controller <../tricycle_controller/doc/userdoc.rst> -<<<<<<< HEAD -======= Swerve Drive Controller <../swerve_drive_controller/doc/userdoc.rst> ->>>>>>> 04f23250 (Added Swerve Drive In mobile_robot_kinematics.rst and fixed CMakeLists.txt of swerve_drive_controller package) Controllers for Manipulators and Other Robots ********************************************* From 8b72f06b441f94a752a5b4c0e7b66f0afa12780a Mon Sep 17 00:00:00 2001 From: Nitin Maurya <97148529+nitin2606@users.noreply.github.com> Date: Tue, 24 Jun 2025 22:32:05 +0530 Subject: [PATCH 04/19] Update swerve_drive_controller/src/swerve_drive_controller.cpp Co-authored-by: Zheng Qu --- .../src/swerve_drive_controller.cpp | 91 ++++++++----------- 1 file changed, 37 insertions(+), 54 deletions(-) diff --git a/swerve_drive_controller/src/swerve_drive_controller.cpp b/swerve_drive_controller/src/swerve_drive_controller.cpp index 2b5f607025..83c6e70f44 100644 --- a/swerve_drive_controller/src/swerve_drive_controller.cpp +++ b/swerve_drive_controller/src/swerve_drive_controller.cpp @@ -654,84 +654,67 @@ void SwerveController::halt() rear_right_axle_handle_->set_position(0.0); } -std::shared_ptr SwerveController::get_wheel(const std::string & wheel_name) + +template +std::shared_ptr get_interface_object( + std::vector& command_interfaces, + const std::vector& state_interfaces, + const std::string& name, + const std::string& interface_suffix, + const std::string& hw_if_type) { - // auto logger = get_node()->get_logger(); auto logger = rclcpp::get_logger("SwerveController"); - if (wheel_name.empty()) { - RCLCPP_ERROR(logger, "Wheel joint name not given. Make sure all joints are specified."); + if (name.empty()) { + RCLCPP_ERROR(logger, "Joint name not given. Make sure all joints are specified."); return nullptr; } - const auto command_handle = std::find_if( - command_interfaces_.begin(), command_interfaces_.end(), - [&wheel_name](const auto & interface) + + const std::string expected_interface_name = name + interface_suffix; + + auto command_handle = std::find_if( + command_interfaces.begin(), command_interfaces.end(), + [&expected_interface_name, &hw_if_type](const auto & interface) { - return interface.get_name() == (wheel_name + "/velocity") && - interface.get_interface_name() == HW_IF_VELOCITY; + return interface.get_name() == expected_interface_name && + interface.get_interface_name() == hw_if_type; }); - if (command_handle == command_interfaces_.end()) { - RCLCPP_ERROR( - get_node()->get_logger(), "Unable to obtain joint command handle for %s", wheel_name.c_str()); + if (command_handle == command_interfaces.end()) { + RCLCPP_ERROR(logger, "Unable to find command interface for: %s", name.c_str()); + RCLCPP_ERROR(logger, "Expected interface name: %s", expected_interface_name.c_str()); return nullptr; } const auto state_handle = std::find_if( - state_interfaces_.begin(), state_interfaces_.end(), - [&wheel_name](const auto & interface) + state_interfaces.begin(), state_interfaces.end(), + [&expected_interface_name, &hw_if_type](const auto & interface) { - return interface.get_name() == (wheel_name + "/velocity") && - interface.get_interface_name() == HW_IF_VELOCITY; + return interface.get_name() == expected_interface_name && + interface.get_interface_name() == hw_if_type; }); - if (state_handle == state_interfaces_.end()) { - RCLCPP_ERROR(logger, "Unable to obtain joint state handle for %s", wheel_name.c_str()); + if (state_handle == state_interfaces.end()) { + RCLCPP_ERROR(logger, "Unable to find the state interface for: %s", name.c_str()); return nullptr; } + static_assert(!std::is_const_v>, "Command handle is const!"); + return std::make_shared(std::ref(*command_handle), std::ref(*state_handle), name); +} - return std::make_shared(std::ref(*command_handle), std::ref(*state_handle), wheel_name); +std::shared_ptr SwerveController::get_wheel(const std::string & wheel_name) +{ + return get_interface_object( + command_interfaces_, state_interfaces_, wheel_name, "/velocity", HW_IF_VELOCITY); } std::shared_ptr SwerveController::get_axle(const std::string & axle_name) { - // auto logger = get_node()->get_logger(); - auto logger = rclcpp::get_logger("SwerveController"); - if (axle_name.empty()) { - RCLCPP_ERROR(logger, "Wheel joint name not given. Make sure all joints are specified."); - return nullptr; - } - - // Get Command Handle for joint - const auto command_handle = std::find_if( - command_interfaces_.begin(), command_interfaces_.end(), - [&axle_name](const auto & interface) - { - return interface.get_name() == (axle_name + "/position") && - interface.get_interface_name() == HW_IF_POSITION; - }); - - if (command_handle == command_interfaces_.end()) { - RCLCPP_ERROR(logger, "Unable to find command interface for axle: %s", axle_name.c_str()); - RCLCPP_ERROR(logger, "Expected interface name: %s/position", axle_name.c_str()); - return nullptr; - } - - const auto state_handle = std::find_if( - state_interfaces_.begin(), state_interfaces_.end(), - [&axle_name](const auto & interface) - { - return interface.get_name() == (axle_name + "/position") && - interface.get_interface_name() == HW_IF_POSITION; - }); - - if (state_handle == state_interfaces_.end()) { - RCLCPP_ERROR(logger, "Unable to find the state interface for axle: %s", axle_name.c_str()); - return nullptr; - } - return std::make_shared(std::ref(*command_handle), std::ref(*state_handle), axle_name); + return get_interface_object( + command_interfaces_, state_interfaces_, axle_name, "/position", HW_IF_POSITION); } + } // namespace swerve_drive_controller #include "class_loader/register_macro.hpp" From ee8d704a5a5b38c168cd0dc175fd6ba0bc11bcf9 Mon Sep 17 00:00:00 2001 From: nitin Date: Wed, 28 May 2025 03:29:52 +0530 Subject: [PATCH 05/19] Added Swerve Drive Kinematics in mobile_robot_kinematics.rst Signed-off-by: nitin --- doc/mobile_robot_kinematics.rst | 406 -------------------------------- 1 file changed, 406 deletions(-) diff --git a/doc/mobile_robot_kinematics.rst b/doc/mobile_robot_kinematics.rst index ff0d2526b2..27cdfa0efe 100644 --- a/doc/mobile_robot_kinematics.rst +++ b/doc/mobile_robot_kinematics.rst @@ -109,412 +109,6 @@ The body twist of the robot can be obtained from the wheel velocities by using t \omega_n \end{bmatrix} -Nonholonomic Wheeled Mobile Robots -..................................... - -Unicycle model -,,,,,,,,,,,,,,,, - -To define the coordinate systems (`ROS coordinate frame conventions `__, the coordinate systems follow the right-hand rule), consider the following simple unicycle model - -.. image:: images/unicycle.svg - :width: 550 - :align: center - :alt: Unicycle - -* :math:`x_b,y_b` is the robot's body-frame coordinate system, located at the contact point of the wheel on the ground. -* :math:`x_w,y_w` is the world coordinate system. -* :math:`x,y` are the robot's Cartesian coordinates in the world coordinate system. -* :math:`\theta` is the robot's heading angle, i.e. the orientation of the robot's :math:`x_b`-axis w.r.t. the world's :math:`x_w`-axis. - -In the following, we want to command the robot with a desired body twist - -.. math:: - - \vec{\nu}_b = \begin{bmatrix} - \vec{\omega}_{b} \\ - \vec{v}_{b} - \end{bmatrix}, - -where :math:`\vec{v}_{b}` is the linear velocity of the robot in its body-frame, and :math:`\vec\omega_{b}` is the angular velocity of the robot in its body-frame. As we consider steering robots on a flat surface, it is sufficient to give - -* :math:`v_{b,x}`, i.e. the linear velocity of the robot in direction of the :math:`x_b` axis. -* :math:`\omega_{b,z}`, i.e. the angular velocity of the robot about the :math:`x_z` axis. - -as desired system inputs. The forward kinematics of the unicycle can be calculated with - -.. math:: - \dot{x} &= v_{b,x} \cos(\theta) \\ - \dot{y} &= v_{b,x} \sin(\theta) \\ - \dot{\theta} &= \omega_{b,z} - -We will formulate the inverse kinematics to calculate the desired commands for the robot (wheel speed or steering) from the given body twist. - -Differential Drive Robot -,,,,,,,,,,,,,,,,,,,,,,,, - -Citing `Siciliano et.al - Robotics: Modelling, Planning and Control `_: - -.. code-block:: text - - A unicycle in the strict sense (i.e., a vehicle equipped with a single wheel) - is a robot with a serious problem of balance in static conditions. However, - there exist vehicles that are kinematically equivalent to a unicycle but more - stable from a mechanical viewpoint. - -One of these vehicles is the differential drive robot, which has two wheels, each of which is driven independently. - -.. image:: images/diff_drive.svg - :width: 550 - :align: center - :alt: Differential drive robot - -* :math:`w` is the wheel track (the distance between the wheels). - -**Forward Kinematics** - -The forward kinematics of the differential drive model can be calculated from the unicycle model above using - -.. math:: - v_{b,x} &= \frac{v_{right} + v_{left}}{2} \\ - \omega_{b,z} &= \frac{v_{right} - v_{left}}{w} - -**Inverse Kinematics** - -The necessary wheel speeds to achieve a desired body twist can be calculated with: - -.. math:: - - v_{left} &= v_{b,x} - \omega_{b,z} w / 2 \\ - v_{right} &= v_{b,x} + \omega_{b,z} w / 2 - - -**Odometry** - -We can use the forward kinematics equations above to calculate the robot's odometry directly from the encoder readings. - -Car-Like (Bicycle) Model -,,,,,,,,,,,,,,,,,,,,,,,, - -The following picture shows a car-like robot with two wheels, where the front wheel is steerable. This model is also known as the bicycle model. - -.. image:: images/car_like_robot.svg - :width: 550 - :align: center - :alt: Car-like robot - -* :math:`\phi` is the steering angle of the front wheel, counted positive in direction of rotation around :math:`x_z`-axis. -* :math:`v_{rear}, v_{front}` is the velocity of the rear and front wheel. -* :math:`l` is the wheelbase. - -We assume that the wheels are rolling without slipping. This means that the velocity of the contact point of the wheel with the ground is zero and the wheel's velocity points in the direction perpendicular to the wheel's axis. The **Instantaneous Center of Rotation** (ICR), i.e. the center of the circle around which the robot rotates, is located at the intersection of the lines that are perpendicular to the wheels' axes and pass through the contact points of the wheels with the ground. - -As a consequence of the no-slip condition, the velocity of the two wheels must satisfy the following constraint: - -.. math:: - v_{rear} = v_{front} \cos(\phi) - -**Forward Kinematics** - -The forward kinematics of the car-like model can be calculated with - -.. math:: - \dot{x} &= v_{b,x} \cos(\theta) \\ - \dot{y} &= v_{b,x} \sin(\theta) \\ - \dot{\theta} &= \frac{v_{b,x}}{l} \tan(\phi) - - -**Inverse Kinematics** - -The steering angle is one command input of the robot: - -.. math:: - \phi = \arctan\left(\frac{l w_{b,z}}{v_{b,x}} \right) - - -For the rear-wheel drive, the velocity of the rear wheel is the second input of the robot: - -.. math:: - v_{rear} = v_{b,x} - - -For the front-wheel drive, the velocity of the front wheel is the second input of the robot: - -.. math:: - v_{front} = \frac{v_{b,x}}{\cos(\phi)} - -**Odometry** - -We have to distinguish between two cases: Encoders on the rear wheel or on the front wheel. - -For the rear wheel case: - -.. math:: - \dot{x} &= v_{rear} \cos(\theta) \\ - \dot{y} &= v_{rear} \sin(\theta) \\ - \dot{\theta} &= \frac{v_{rear}}{l} \tan(\phi) - - -For the front wheel case: - -.. math:: - \dot{x} &= v_{front} \cos(\theta) \cos(\phi)\\ - \dot{y} &= v_{front} \sin(\theta) \cos(\phi)\\ - \dot{\theta} &= \frac{v_{front}}{l} \sin(\phi) - - -Double-Traction Axle -,,,,,,,,,,,,,,,,,,,,, - -The following image shows a car-like robot with three wheels, with two independent traction wheels at the rear. - -.. image:: images/double_traction.svg - :width: 550 - :align: center - :alt: A car-like robot with two traction wheels at the rear - -* :math:`w_r` is the wheel track of the rear axle. - -**Forward Kinematics** - -The forward kinematics is the same as the car-like model above. - -**Inverse Kinematics** - -The turning radius of the robot is - -.. math:: - R_b = \frac{l}{\tan(\phi)} - -Then the velocity of the rear wheels must satisfy these conditions to avoid skidding - -.. math:: - v_{rear,left} &= v_{b,x}\frac{R_b - w_r/2}{R_b}\\ - v_{rear,right} &= v_{b,x}\frac{R_b + w_r/2}{R_b} - -**Odometry** - -The calculation of :math:`v_{b,x}` from two encoder measurements of the traction axle is overdetermined. -If there is no slip and the encoders are ideal, - -.. math:: - v_{b,x} = v_{rear,left} \frac{R_b}{R_b - w_r/2} = v_{rear,right} \frac{R_b}{R_b + w_r/2} - -holds. But to get a more robust solution, we take the average of both , i.e., - -.. math:: - v_{b,x} = 0.5 \left(v_{rear,left} \frac{R_b}{R_b - w_r/2} + v_{rear,right} \frac{R_b}{R_b + w_r/2}\right). - - -Ackermann Steering -,,,,,,,,,,,,,,,,,,,,, - -The following image shows a four-wheeled robot with two independent steering wheels in the front. - -.. image:: images/ackermann_steering.svg - :width: 550 - :align: center - :alt: A car-like robot with two steering wheels at the front - -* :math:`w_f` is the wheel track of the front axle, measured between the two kingpins. - -To prevent the front wheels from slipping, the steering angle of the front wheels cannot be equal. -This is the so-called **Ackermann steering**. - -.. note:: - Ackermann steering can also be achieved by a `mechanical linkage between the two front wheels `__. In this case the robot has only one steering input, and the steering angle of the two front wheels is mechanically coupled. The inverse kinematics of the robot will then be the same as in the car-like model above. - -**Forward Kinematics** - -The forward kinematics is the same as for the car-like model above. - -**Inverse Kinematics** - -The turning radius of the robot is - -.. math:: - R_b = \frac{l}{\tan(\phi)} - -Then the steering angles of the front wheels must satisfy these conditions to avoid skidding - -.. math:: - \phi_{left} &= \arctan\left(\frac{l}{R_b - w_f/2}\right) &= \arctan\left(\frac{2l\sin(\phi)}{2l\cos(\phi) - w_f\sin(\phi)}\right)\\ - \phi_{right} &= \arctan\left(\frac{l}{R_b + w_f/2}\right) &= \arctan\left(\frac{2l\sin(\phi)}{2l\cos(\phi) + w_f\sin(\phi)}\right) - -**Odometry** - -The calculation of :math:`\phi` from two angle measurements of the steering axle is overdetermined. -If there is no slip and the measurements are ideal, - -.. math:: - \phi = \arctan\left(\frac{l\tan(\phi_{left})}{l + w_f/2 \tan(\phi_{left})}\right) = \arctan\left(\frac{l\tan(\phi_{right})}{l - w_f/2 \tan(\phi_{right})}\right) - -holds. But to get a more robust solution, we take the average of both , i.e., - -.. math:: - \phi = 0.5 \left(\arctan\left(\frac{l\tan(\phi_{left})}{l + w_f/2 \tan(\phi_{left})}\right) + \arctan\left(\frac{l\tan(\phi_{right})}{l - w_f/2 \tan(\phi_{right})}\right)\right). - -Ackermann Steering with Traction -,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,, - -The following image shows a four-wheeled car-like robot with two independent steering wheels at the front, which are also driven independently. - -.. image:: images/ackermann_steering_traction.svg - :width: 550 - :align: center - :alt: A car-like robot with two steering wheels at the front, which are also independently driven. - -* :math:`d_{kp}` is the distance from the kingpin to the contact point of the front wheel with the ground. - -**Forward Kinematics** - -The forward kinematics is the same as the car-like model above. - -**Inverse Kinematics** - -To avoid slipping of the front wheels, the velocity of the front wheels cannot be equal and - -.. math:: - \frac{v_{front,left}}{R_{left}} = \frac{v_{front,right}}{R_{right}} = \frac{v_{b,x}}{R_b} - -with turning radius of the robot and the left/right front wheel - -.. math:: - R_b &= \frac{l}{\tan(\phi)} \\ - R_{left} &= \frac{l-d_{kp}\sin(\phi_{left})}{\sin(\phi_{left})}\\ - R_{right} &= \frac{l+d_{kp}\sin(\phi_{right})}{\sin(\phi_{right})}. - -This results in the following inverse kinematics equations - -.. math:: - v_{front,left} &= \frac{v_{b,x}(l-d_{kp}\sin(\phi_{left}))}{R_b\sin(\phi_{left})}\\ - v_{front,right} &= \frac{v_{b,x}(l+d_{kp}\sin(\phi_{right}))}{R_b\sin(\phi_{right})} - -with the steering angles of the front wheels from the Ackermann steering equations above. - -**Odometry** - -The calculation of :math:`v_{b,x}` from two encoder measurements of the traction axle is again overdetermined. -If there is no slip and the encoders are ideal, - -.. math:: - v_{b,x} = v_{front,left} \frac{R_b\sin(\phi_{left})}{l-d_{kp}\sin(\phi_{left})} = v_{front,right} \frac{R_b\sin(\phi_{right})}{l+d_{kp}\sin(\phi_{right})} - -holds. But to get a more robust solution, we take the average of both , i.e., - -.. math:: - v_{b,x} = 0.5 \left( v_{front,left} \frac{R_b\sin(\phi_{left})}{l-d_{kp}\sin(\phi_{left})} + v_{front,right} \frac{R_b\sin(\phi_{right})}{l+d_{kp}\sin(\phi_{right})}\right). -:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/doc/mobile_robot_kinematics.rst - -.. _mobile_robot_kinematics: - -Wheeled Mobile Robot Kinematics --------------------------------------------------------------- - -.. _siciliano: https://link.springer.com/book/10.1007/978-1-84628-642-1 -.. _modern_robotics: http://modernrobotics.org/ - -This page introduces the kinematics of different wheeled mobile robots. For further reference see `Siciliano et.al - Robotics: Modelling, Planning and Control `_ and `Kevin M. Lynch and Frank C. Park - Modern Robotics: Mechanics, Planning, And Control `_. - -Wheeled mobile robots can be classified in two categories: - -Omnidirectional robots - which can move instantaneously in any direction in the plane, and - -Nonholonomic robots - which cannot move instantaneously in any direction in the plane. - -The forward integration of the kinematic model using the encoders of the wheel actuators — is referred to as **odometric localization** or **passive localization** or **dead reckoning**. We will call it just **odometry**. - -Omnidirectional Wheeled Mobile Robots -..................................... - -Omnidirectional Drive Robots using Omni Wheels -,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,, - -The below explains the kinematics of omnidirectional drive robots using 3 or more omni wheels. -It follows the coordinate conventions defined in `ROS REP 103 `__. - -.. image:: images/omni_wheel_omnidirectional_drive.svg - :width: 550 - :align: center - :alt: Omnidirectional Drive Robot using Omni Wheels - -* :math:`x_b,y_b` is the robot's body-frame coordinate system, located at the contact point of the wheel on the ground. -* :math:`x_w,y_w` is the world coordinate system. -* :math:`v_{b,x},` is the robot's linear velocity on the x-axis. -* :math:`v_{b,y}` is the robot's linear velocity on the y-axis. -* :math:`\omega_{b,z}` is the robot's angular velocity on the z-axis. -* :math:`R` is the robot's radius / the distance between the robot's center and the wheels. -* Red arrows on the wheel :math:`i` signify the positive direction of their rotation :math:`\omega_i` -* :math:`\gamma` is the angular offset of the first wheel from :math:`x_b`. -* :math:`\theta` is the angle between each wheel which can be calculated using the below equation where :math:`n` is the number of wheels. - -.. math:: - - θ = \frac{2\pi}{n} - -**Inverse Kinematics** - -The necessary angular velocity of the wheels to achieve a desired body twist can be calculated using the below matrix: - -.. math:: - - A = - \begin{bmatrix} - \sin(\gamma) & -\cos(\gamma) & -R \\ - \sin(\theta + \gamma) & -\cos(\theta + \gamma) & -R\\ - \sin(2\theta + \gamma) & -\cos(2\theta + \gamma) & -R\\ - \sin(3\theta + \gamma) & -\cos(3\theta + \gamma) & -R\\ - \vdots & \vdots & \vdots\\ - \sin((n-1)\theta + \gamma) & -\cos((n-1)\theta + \gamma) & -R\\ - \end{bmatrix} - -.. math:: - - \begin{bmatrix} - \omega_1\\ - \omega_2\\ - \omega_3\\ - \omega_4\\ - \vdots\\ - \omega_n - \end{bmatrix} = - \frac{1}{r} - A - \begin{bmatrix} - v_{b,x}\\ - v_{b,y}\\ - \omega_{b,z}\\ - \end{bmatrix} - -Here :math:`\omega_1,\ldots,\omega_n` are the angular velocities of the wheels and :math:`r` is the radius of the wheels. -These equations can be written in algebraic form for any wheel :math:`i` like this: - -.. math:: - \omega_i = \frac{\sin((i-1)\theta + \gamma) v_{b,x} - \cos((i-1)\theta + \gamma) v_{b,y} - R \omega_{b,z}}{r} - -**Forward Kinematics** - -The body twist of the robot can be obtained from the wheel velocities by using the pseudoinverse of matrix :math:`A`. - -.. math:: - - \begin{bmatrix} - v_{b,x}\\ - v_{b,y}\\ - \omega_{b,z}\\ - \end{bmatrix} = - rA^\dagger - \begin{bmatrix} - \omega_1\\ - \omega_2\\ - \omega_3\\ - \omega_4\\ - \vdots\\ - \omega_n - \end{bmatrix} - Swerve Drive Robots From 5b7b0779c62512b218f16b787f08317cbf153a28 Mon Sep 17 00:00:00 2001 From: nitin Date: Tue, 24 Jun 2025 22:33:25 +0530 Subject: [PATCH 06/19] Changed Wheel and Axle handles to unique_ptr Signed-off-by: nitin --- .../swerve_drive_controller.hpp | 26 +++---- .../src/swerve_drive_controller.cpp | 75 +++++++++---------- .../src/swerve_drive_kinematics.cpp | 8 -- 3 files changed, 47 insertions(+), 62 deletions(-) diff --git a/swerve_drive_controller/include/swerve_drive_controller/swerve_drive_controller.hpp b/swerve_drive_controller/include/swerve_drive_controller/swerve_drive_controller.hpp index c08743b57a..252e64b014 100644 --- a/swerve_drive_controller/include/swerve_drive_controller/swerve_drive_controller.hpp +++ b/swerve_drive_controller/include/swerve_drive_controller/swerve_drive_controller.hpp @@ -106,19 +106,19 @@ class SwerveController : public controller_interface::ControllerInterface CallbackReturn on_shutdown(const rclcpp_lifecycle::State & previous_state) override; protected: - std::shared_ptr get_wheel(const std::string & wheel_name); - std::shared_ptr get_axle(const std::string & axle_name); - - // Handles for three wheels and their axles - std::shared_ptr front_left_wheel_handle_; - std::shared_ptr front_right_wheel_handle_; - std::shared_ptr rear_left_wheel_handle_; - std::shared_ptr rear_right_wheel_handle_; - - std::shared_ptr front_left_axle_handle_; - std::shared_ptr front_right_axle_handle_; - std::shared_ptr rear_left_axle_handle_; - std::shared_ptr rear_right_axle_handle_; + std::unique_ptr get_wheel(const std::string & wheel_name); + std::unique_ptr get_axle(const std::string & axle_name); + + // Handles for four wheels and their axles + std::unique_ptr front_left_wheel_handle_; + std::unique_ptr front_right_wheel_handle_; + std::unique_ptr rear_left_wheel_handle_; + std::unique_ptr rear_right_wheel_handle_; + + std::unique_ptr front_left_axle_handle_; + std::unique_ptr front_right_axle_handle_; + std::unique_ptr rear_left_axle_handle_; + std::unique_ptr rear_right_axle_handle_; // Joint names for wheels and axles std::string front_left_wheel_joint_name_; diff --git a/swerve_drive_controller/src/swerve_drive_controller.cpp b/swerve_drive_controller/src/swerve_drive_controller.cpp index 83c6e70f44..4c58d05574 100644 --- a/swerve_drive_controller/src/swerve_drive_controller.cpp +++ b/swerve_drive_controller/src/swerve_drive_controller.cpp @@ -462,14 +462,14 @@ controller_interface::return_type SwerveController::update( double y_offset = wheel_params_.y_offset; double radius = wheel_params_.radius; - auto wheel_command_ = + auto wheel_command = swerveDriveKinematics_.compute_wheel_commands(linear_x_cmd, linear_y_cmd, angular_cmd); std::vector> wheel_data = { - {wheel_command_[0], front_left_velocity_threshold_, "front_left_wheel"}, - {wheel_command_[1], front_right_velocity_threshold_, "front_right_wheel"}, - {wheel_command_[2], rear_left_velocity_threshold_, "rear_left_wheel"}, - {wheel_command_[3], rear_right_velocity_threshold_, "rear_right_wheel"}}; + {wheel_command[0], front_left_velocity_threshold_, "front_left_wheel"}, + {wheel_command[1], front_right_velocity_threshold_, "front_right_wheel"}, + {wheel_command[2], rear_left_velocity_threshold_, "rear_left_wheel"}, + {wheel_command[3], rear_right_velocity_threshold_, "rear_right_wheel"}}; for (const auto & [wheel_command, threshold, label] : wheel_data) { if (wheel_command.drive_velocity > threshold) { @@ -477,49 +477,42 @@ controller_interface::return_type SwerveController::update( } } - const double front_left_velocity = wheel_command_[0].drive_velocity; - const double front_left_angle = wheel_command_[0].steering_angle; + const double front_left_velocity = wheel_command[0].drive_velocity; + const double front_left_angle = wheel_command[0].steering_angle; - const double front_right_velocity = wheel_command_[1].drive_velocity; - const double front_right_angle = wheel_command_[1].steering_angle; + const double front_right_velocity = wheel_command[1].drive_velocity; + const double front_right_angle = wheel_command[1].steering_angle; - const double rear_left_velocity = wheel_command_[2].drive_velocity; - const double rear_left_angle = wheel_command_[2].steering_angle; + const double rear_left_velocity = wheel_command[2].drive_velocity; + const double rear_left_angle = wheel_command[2].steering_angle; - const double rear_right_velocity = wheel_command_[3].drive_velocity; - const double rear_right_angle = wheel_command_[3].steering_angle; + const double rear_right_velocity = wheel_command[3].drive_velocity; + const double rear_right_angle = wheel_command[3].steering_angle; - try { - if (front_left_axle_handle_ != nullptr && front_left_wheel_handle_ != nullptr) { - front_left_axle_handle_->set_position(front_left_angle); - front_left_wheel_handle_->set_velocity(front_left_velocity); - } else { - RCLCPP_ERROR(logger, "Front Left Axle Handle or Wheel Handle is NULLPTR"); - } + if(front_left_axle_handle_ == nullptr || front_left_wheel_handle_ == nullptr){ + throw std::runtime_error("Front Left Axle or Wheel handle is nullptr"); + } + front_left_axle_handle_->set_position(front_left_angle); + front_left_wheel_handle_->set_velocity(front_left_velocity); - if (front_right_axle_handle_ != nullptr && front_right_wheel_handle_ != nullptr) { - front_right_axle_handle_->set_position(front_right_angle); - front_right_wheel_handle_->set_velocity(front_right_velocity); - } else { - RCLCPP_ERROR(logger, "Front Right Axle Handle or Wheel Handle is NULLPTR"); - } + if (front_right_axle_handle_ == nullptr || front_right_wheel_handle_ == nullptr) { + throw std::runtime_error("Front Right Axle or Wheel handle is nullptr"); + } + front_right_axle_handle_->set_position(front_right_angle); + front_right_wheel_handle_->set_velocity(front_right_velocity); - if (rear_left_axle_handle_ != nullptr && rear_left_wheel_handle_ != nullptr) { - rear_left_axle_handle_->set_position(rear_left_angle); - rear_left_wheel_handle_->set_velocity(rear_left_velocity); - } else { - RCLCPP_ERROR(logger, "Rear Left Axle or Wheel Handle is NULLPTR"); - } + if (rear_left_axle_handle_ == nullptr || rear_left_wheel_handle_ == nullptr) { + throw std::runtime_error("Rear Left Axle or Wheel handle is nullptr"); + } + rear_left_axle_handle_->set_position(rear_left_angle); + rear_left_wheel_handle_->set_velocity(rear_left_velocity); - if (rear_right_axle_handle_ != nullptr && rear_right_wheel_handle_ != nullptr) { - rear_right_axle_handle_->set_position(rear_right_angle); - rear_right_wheel_handle_->set_velocity(rear_right_velocity); - } else { - RCLCPP_ERROR(logger, "Rear Right Axle or Wheel Handle is NULLPTR"); - } - } catch (std::exception & e) { - RCLCPP_INFO(logger, "Got Exception: %s", e.what()); + if (rear_right_axle_handle_ == nullptr || rear_right_wheel_handle_ == nullptr) { + throw std::runtime_error("Rear Right Axle or Wheel handle is nullptr"); } + rear_right_axle_handle_->set_position(rear_right_angle); + rear_right_wheel_handle_->set_velocity(rear_right_velocity); + const auto update_dt = current_time - previous_update_timestamp_; previous_update_timestamp_ = current_time; @@ -708,7 +701,7 @@ std::shared_ptr SwerveController::get_wheel(const std::string & wheel_nam command_interfaces_, state_interfaces_, wheel_name, "/velocity", HW_IF_VELOCITY); } -std::shared_ptr SwerveController::get_axle(const std::string & axle_name) +std::unique_ptr SwerveController::get_axle(const std::string & axle_name) { return get_interface_object( command_interfaces_, state_interfaces_, axle_name, "/position", HW_IF_POSITION); diff --git a/swerve_drive_controller/src/swerve_drive_kinematics.cpp b/swerve_drive_controller/src/swerve_drive_kinematics.cpp index de6034000a..d6bd248a1f 100644 --- a/swerve_drive_controller/src/swerve_drive_kinematics.cpp +++ b/swerve_drive_controller/src/swerve_drive_kinematics.cpp @@ -69,14 +69,6 @@ OdometryState SwerveDriveKinematics::update_odometry( wheel_positions_[i].second * wheel_positions_[i].second); } double wz_robot = wz_sum / wz_denominator; - // double wz_robot = wz_sum / 4.0; - - // double wz_robot = wz_sum / (wheel_positions_[0].first * wheel_positions_[0].first + - // wheel_positions_[1].first * wheel_positions_[1].first + - // wheel_positions_[2].first * wheel_positions_[2].first + - // wheel_positions_[0].second * wheel_positions_[0].second + - // wheel_positions_[1].second * wheel_positions_[1].second + - // wheel_positions_[2].second * wheel_positions_[2].second); // Transform velocities to global frame double cos_theta = std::cos(odometry_.theta); From 7c7c1495cc34a42943fdc3dec035dae9e84fd4bc Mon Sep 17 00:00:00 2001 From: nitin Date: Tue, 24 Jun 2025 23:00:08 +0530 Subject: [PATCH 07/19] Code Refactoring Signed-off-by: nitin --- .../src/swerve_drive_controller.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/swerve_drive_controller/src/swerve_drive_controller.cpp b/swerve_drive_controller/src/swerve_drive_controller.cpp index 4c58d05574..f1eb5465e9 100644 --- a/swerve_drive_controller/src/swerve_drive_controller.cpp +++ b/swerve_drive_controller/src/swerve_drive_controller.cpp @@ -471,9 +471,9 @@ controller_interface::return_type SwerveController::update( {wheel_command[2], rear_left_velocity_threshold_, "rear_left_wheel"}, {wheel_command[3], rear_right_velocity_threshold_, "rear_right_wheel"}}; - for (const auto & [wheel_command, threshold, label] : wheel_data) { - if (wheel_command.drive_velocity > threshold) { - wheel_command.drive_velocity = threshold; + for (const auto & [wheel_command_, threshold, label] : wheel_data) { + if (wheel_command_.drive_velocity > threshold) { + wheel_command_.drive_velocity = threshold; } } @@ -649,7 +649,7 @@ void SwerveController::halt() template -std::shared_ptr get_interface_object( +std::unique_ptr get_interface_object( std::vector& command_interfaces, const std::vector& state_interfaces, const std::string& name, @@ -692,10 +692,10 @@ std::shared_ptr get_interface_object( return nullptr; } static_assert(!std::is_const_v>, "Command handle is const!"); - return std::make_shared(std::ref(*command_handle), std::ref(*state_handle), name); + return std::make_unique(std::ref(*command_handle), std::ref(*state_handle), name); } -std::shared_ptr SwerveController::get_wheel(const std::string & wheel_name) +std::unique_ptr SwerveController::get_wheel(const std::string & wheel_name) { return get_interface_object( command_interfaces_, state_interfaces_, wheel_name, "/velocity", HW_IF_VELOCITY); From d4386948fbac8a05935ef67014d96827c0bb5ec2 Mon Sep 17 00:00:00 2001 From: nitin Date: Sun, 29 Jun 2025 19:33:07 +0530 Subject: [PATCH 08/19] refactor(swervecontroller): optimize SwerveController code structure and performance Improved code quality, reduced redundancy, and enhanced maintainability: - Removed blocking sleep from on_configure lifecycle callback - Ensured all wheel and axle interface accesses are null-checked - Removed redundant set_velocity/set_position initializations - Simplified initialization and cleanup logic - Improved readability with consistent naming and formatting Signed-off-by: nitin --- .../swerve_drive_controller.hpp | 23 +- .../src/swerve_drive_controller.cpp | 196 ++++++------------ 2 files changed, 77 insertions(+), 142 deletions(-) diff --git a/swerve_drive_controller/include/swerve_drive_controller/swerve_drive_controller.hpp b/swerve_drive_controller/include/swerve_drive_controller/swerve_drive_controller.hpp index 252e64b014..2c9b9ac34d 100644 --- a/swerve_drive_controller/include/swerve_drive_controller/swerve_drive_controller.hpp +++ b/swerve_drive_controller/include/swerve_drive_controller/swerve_drive_controller.hpp @@ -39,8 +39,15 @@ namespace swerve_drive_controller { +enum class WheelAxleIndex : std::size_t +{ + FRONT_LEFT = 0, + FRONT_RIGHT = 1, + REAR_LEFT = 2, + REAR_RIGHT = 3 +}; + using CallbackReturn = controller_interface::CallbackReturn; -// using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; class Wheel { @@ -110,15 +117,8 @@ class SwerveController : public controller_interface::ControllerInterface std::unique_ptr get_axle(const std::string & axle_name); // Handles for four wheels and their axles - std::unique_ptr front_left_wheel_handle_; - std::unique_ptr front_right_wheel_handle_; - std::unique_ptr rear_left_wheel_handle_; - std::unique_ptr rear_right_wheel_handle_; - - std::unique_ptr front_left_axle_handle_; - std::unique_ptr front_right_axle_handle_; - std::unique_ptr rear_left_axle_handle_; - std::unique_ptr rear_right_axle_handle_; + std::vector> wheel_handles_; + std::vector> axle_handles_; // Joint names for wheels and axles std::string front_left_wheel_joint_name_; @@ -131,6 +131,9 @@ class SwerveController : public controller_interface::ControllerInterface std::string rear_left_axle_joint_name_; std::string rear_right_axle_joint_name_; + std::array wheel_joint_names{}; + std::array axle_joint_names{}; + std::string cmd_vel_topic_; std::string odometry_topic_; std::string base_footprint_; diff --git a/swerve_drive_controller/src/swerve_drive_controller.cpp b/swerve_drive_controller/src/swerve_drive_controller.cpp index f1eb5465e9..6a3d4cbc90 100644 --- a/swerve_drive_controller/src/swerve_drive_controller.cpp +++ b/swerve_drive_controller/src/swerve_drive_controller.cpp @@ -241,11 +241,19 @@ CallbackReturn SwerveController::on_configure(const rclcpp_lifecycle::State & /* return CallbackReturn::ERROR; } + wheel_joint_names[0] = front_left_wheel_joint_name_; + wheel_joint_names[1] = front_right_wheel_joint_name_; + wheel_joint_names[2] = rear_left_wheel_joint_name_; + wheel_joint_names[3] = rear_right_wheel_joint_name_; + + axle_joint_names[0] = front_left_axle_joint_name_; + axle_joint_names[1] = front_right_axle_joint_name_; + axle_joint_names[2] = rear_left_axle_joint_name_; + axle_joint_names[3] = rear_right_axle_joint_name_; + cmd_vel_timeout_ = std::chrono::milliseconds( static_cast(get_node()->get_parameter("cmd_vel_timeout").as_double() * 1000.0)); - use_stamped_vel_ = get_node()->get_parameter("use_stamped_vel").as_bool(); - if (!reset()) { return CallbackReturn::ERROR; } @@ -344,73 +352,39 @@ CallbackReturn SwerveController::on_configure(const rclcpp_lifecycle::State & /* return CallbackReturn::ERROR; } - std::chrono::seconds sleep_duration(1); - rclcpp::sleep_for(sleep_duration); - return CallbackReturn::SUCCESS; } CallbackReturn SwerveController::on_activate(const rclcpp_lifecycle::State &) { auto logger = rclcpp::get_logger("SwerveController"); - front_left_wheel_handle_ = get_wheel(front_left_wheel_joint_name_); - front_right_wheel_handle_ = get_wheel(front_right_wheel_joint_name_); - rear_left_wheel_handle_ = get_wheel(rear_left_wheel_joint_name_); - rear_right_wheel_handle_ = get_wheel(rear_right_wheel_joint_name_); - - front_left_axle_handle_ = get_axle(front_left_axle_joint_name_); - front_right_axle_handle_ = get_axle(front_right_axle_joint_name_); - rear_left_axle_handle_ = get_axle(rear_left_axle_joint_name_); - rear_right_axle_handle_ = get_axle(rear_right_axle_joint_name_); - - if (!front_left_wheel_handle_) { - RCLCPP_ERROR(logger, "ERROR IN FETCHING front_left_wheel_handle_"); - return CallbackReturn::ERROR; - } - - if (!front_right_wheel_handle_) { - RCLCPP_ERROR(logger, "ERROR IN FETCHING front_right_wheel_handle_"); - return CallbackReturn::ERROR; - } - - if (!rear_left_wheel_handle_) { - RCLCPP_ERROR(logger, "ERROR IN FETCHING rear_left_wheel_handle_"); - return CallbackReturn::ERROR; - } - if (!rear_right_wheel_handle_) { - RCLCPP_ERROR(logger, "ERROR IN FETCHING rear_right_wheel_handle_"); - return CallbackReturn::ERROR; + wheel_handles_.resize(4); + for(std::size_t i = 0; i < 4; i++) { + wheel_handles_[i] = get_wheel(wheel_joint_names[i]); } - if (!front_left_axle_handle_) { - RCLCPP_ERROR(logger, "ERROR IN FETCHING front_left_axle_handle_"); - return CallbackReturn::ERROR; + axle_handles_.resize(4); + for(std::size_t i = 0; i < 4; i++) { + axle_handles_[i] = get_axle(axle_joint_names[i]); } - if (!front_right_axle_handle_) { - RCLCPP_ERROR(logger, "ERROR IN FETCHING front_right_axle_handle_"); - return CallbackReturn::ERROR; - } - - if (!rear_left_axle_handle_) { - RCLCPP_ERROR(logger, "ERROR IN FETCHING rear_left_axle_handle_"); - return CallbackReturn::ERROR; + for(std::size_t i = 0; i < wheel_handles_.size(); ++i) { + if(!wheel_handles_[i]) { + RCLCPP_ERROR(logger, "ERROR IN FETCHING wheel handle for: %s", wheel_joint_names[i].c_str()); + return CallbackReturn::ERROR; + } + wheel_handles_[i]->set_velocity(0.0); } - if (!rear_right_axle_handle_) { - RCLCPP_ERROR(logger, "ERROR IN FETCHING rear_right_axle_handle_"); - return CallbackReturn::ERROR; + for(std::size_t i = 0; i < axle_handles_.size(); ++i) { + if(!axle_handles_[i]) { + RCLCPP_ERROR(logger, "ERROR IN FETCHING axle handle for: %s", axle_joint_names[i].c_str()); + return CallbackReturn::ERROR; + } + axle_handles_[i]->set_position(0.0); } - front_left_wheel_handle_->set_velocity(0.0); - front_right_wheel_handle_->set_velocity(0.0); - rear_left_wheel_handle_->set_velocity(0.0); - rear_right_wheel_handle_->set_velocity(0.0); - front_left_axle_handle_->set_position(0.0); - front_right_axle_handle_->set_position(0.0); - rear_left_axle_handle_->set_position(0.0); - rear_right_axle_handle_->set_position(0.0); is_halted_ = false; subscriber_is_active_ = true; RCLCPP_INFO(logger, "Subscriber and publisher are now active."); @@ -420,8 +394,8 @@ CallbackReturn SwerveController::on_activate(const rclcpp_lifecycle::State &) controller_interface::return_type SwerveController::update( const rclcpp::Time & time, const rclcpp::Duration & period) { - // auto logger = get_node()->get_logger(); auto logger = rclcpp::get_logger("SwerveController"); + if (this->get_lifecycle_state().id() == State::PRIMARY_STATE_INACTIVE) { if (!is_halted_) { halt(); @@ -457,7 +431,6 @@ controller_interface::return_type SwerveController::update( double & linear_x_cmd = command.twist.linear.x; double & linear_y_cmd = command.twist.linear.y; double & angular_cmd = command.twist.angular.z; - double x_offset = wheel_params_.x_offset; double y_offset = wheel_params_.y_offset; double radius = wheel_params_.radius; @@ -477,41 +450,14 @@ controller_interface::return_type SwerveController::update( } } - const double front_left_velocity = wheel_command[0].drive_velocity; - const double front_left_angle = wheel_command[0].steering_angle; - - const double front_right_velocity = wheel_command[1].drive_velocity; - const double front_right_angle = wheel_command[1].steering_angle; - - const double rear_left_velocity = wheel_command[2].drive_velocity; - const double rear_left_angle = wheel_command[2].steering_angle; - - const double rear_right_velocity = wheel_command[3].drive_velocity; - const double rear_right_angle = wheel_command[3].steering_angle; - - if(front_left_axle_handle_ == nullptr || front_left_wheel_handle_ == nullptr){ - throw std::runtime_error("Front Left Axle or Wheel handle is nullptr"); - } - front_left_axle_handle_->set_position(front_left_angle); - front_left_wheel_handle_->set_velocity(front_left_velocity); - - if (front_right_axle_handle_ == nullptr || front_right_wheel_handle_ == nullptr) { - throw std::runtime_error("Front Right Axle or Wheel handle is nullptr"); - } - front_right_axle_handle_->set_position(front_right_angle); - front_right_wheel_handle_->set_velocity(front_right_velocity); - - if (rear_left_axle_handle_ == nullptr || rear_left_wheel_handle_ == nullptr) { - throw std::runtime_error("Rear Left Axle or Wheel handle is nullptr"); - } - rear_left_axle_handle_->set_position(rear_left_angle); - rear_left_wheel_handle_->set_velocity(rear_left_velocity); - - if (rear_right_axle_handle_ == nullptr || rear_right_wheel_handle_ == nullptr) { - throw std::runtime_error("Rear Right Axle or Wheel handle is nullptr"); + for(std::size_t i = 0; i < 4; i++) { + if(!axle_handles_[i] || !wheel_handles_[i]) { + throw std::runtime_error("Axle or Wheel handle is nullptr for: " + + axle_joint_names[i] + " / " + wheel_joint_names[i]); + } + axle_handles_[i]->set_position(wheel_command[i].steering_angle); + wheel_handles_[i]->set_velocity(wheel_command[i].drive_velocity); } - rear_right_axle_handle_->set_position(rear_right_angle); - rear_right_wheel_handle_->set_velocity(rear_right_velocity); const auto update_dt = current_time - previous_update_timestamp_; @@ -519,34 +465,20 @@ controller_interface::return_type SwerveController::update( swerve_drive_controller::OdometryState odometry_; - if (open_loop_) { - std::array velocity_array = {{ - front_left_velocity, front_right_velocity, rear_left_velocity, rear_right_velocity}}; - std::array steering_angles = {{ - front_left_angle, front_right_angle, rear_left_angle, rear_right_angle}}; - odometry_ = - swerveDriveKinematics_.update_odometry(velocity_array, steering_angles, update_dt.seconds()); - } else { - double front_left_wheel_angle = front_left_axle_handle_->get_feedback(); - double front_right_wheel_angle = front_right_axle_handle_->get_feedback(); - double rear_left_wheel_angle = rear_left_axle_handle_->get_feedback(); - double rear_right_wheel_angle = rear_right_axle_handle_->get_feedback(); - - double front_left_wheel_velocity = front_left_wheel_handle_->get_feedback(); - double front_right_wheel_velocity = front_right_wheel_handle_->get_feedback(); - double rear_left_wheel_velocity = rear_left_wheel_handle_->get_feedback(); - double rear_right_wheel_velocity = rear_right_wheel_handle_->get_feedback(); - - std::array velocity_feedback_array = {{ - front_left_wheel_velocity, front_right_wheel_velocity, rear_left_wheel_velocity, - rear_right_wheel_velocity}}; - std::array steering_angles = {{ - front_left_wheel_angle, front_right_wheel_angle, rear_left_wheel_angle, - rear_right_wheel_angle}}; - - odometry_ = swerveDriveKinematics_.update_odometry( - velocity_feedback_array, steering_angles, update_dt.seconds()); + std::array velocity_array{}; + std::array steering_angle_array{}; + + for(std::size_t i = 0; i < 4; ++i) { + if(open_loop_) { + velocity_array[i] = wheel_command[i].drive_velocity; + steering_angle_array[i] = wheel_command[i].steering_angle; + } else { + velocity_array[i] = wheel_handles_[i]->get_feedback(); + steering_angle_array[i] = axle_handles_[i]->get_feedback(); + } } + odometry_ = swerveDriveKinematics_.update_odometry( + velocity_array, steering_angle_array, update_dt.seconds()); tf2::Quaternion orientation; orientation.setRPY(0.0, 0.0, odometry_.theta); @@ -637,24 +569,22 @@ CallbackReturn SwerveController::on_shutdown(const rclcpp_lifecycle::State &) void SwerveController::halt() { - front_left_wheel_handle_->set_velocity(0.0); - front_left_axle_handle_->set_position(0.0); - front_right_wheel_handle_->set_velocity(0.0); - front_right_axle_handle_->set_position(0.0); - rear_left_wheel_handle_->set_velocity(0.0); - rear_left_axle_handle_->set_position(0.0); - rear_right_wheel_handle_->set_velocity(0.0); - rear_right_axle_handle_->set_position(0.0); + for(std::size_t i = 0; i < wheel_handles_.size(); ++i) { + wheel_handles_[i]->set_velocity(0.0); + } + for(std::size_t i = 0; i < axle_handles_.size(); ++i) { + axle_handles_[i]->set_position(0.0); + } } template std::unique_ptr get_interface_object( - std::vector& command_interfaces, - const std::vector& state_interfaces, - const std::string& name, - const std::string& interface_suffix, - const std::string& hw_if_type) + std::vector & command_interfaces, + const std::vector & state_interfaces, + const std::string & name, + const std::string & interface_suffix, + const std::string & hw_if_type) { auto logger = rclcpp::get_logger("SwerveController"); @@ -688,10 +618,12 @@ std::unique_ptr get_interface_object( }); if (state_handle == state_interfaces.end()) { - RCLCPP_ERROR(logger, "Unable to find the state interface for: %s", name.c_str()); + RCLCPP_ERROR(logger, "Unable to find the state interface for: %s", + name.c_str()); return nullptr; } - static_assert(!std::is_const_v>, "Command handle is const!"); + static_assert(!std::is_const_v>, + "Command handle is const!"); return std::make_unique(std::ref(*command_handle), std::ref(*state_handle), name); } From e4d6f4b7b950248155775f8cb479699ad3d74bbb Mon Sep 17 00:00:00 2001 From: nitin Date: Sat, 19 Jul 2025 04:01:45 +0530 Subject: [PATCH 09/19] Resolved conflict in documentation Signed-off-by: nitin --- doc/controllers_index.rst | 3 +- doc/images/swerve_drive.drawio | 836 +++++++++++++++++++------------- doc/images/swerve_drive.svg | 2 +- doc/migration.rst | 6 + doc/mobile_robot_kinematics.rst | 29 +- doc/release_notes.rst | 13 +- doc/writing_new_controller.rst | 2 +- 7 files changed, 544 insertions(+), 347 deletions(-) diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index 2b77b9b55f..01a3e127a9 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -31,7 +31,6 @@ Controllers for Wheeled Mobile Robots Mecanum Drive Controllers <../mecanum_drive_controller/doc/userdoc.rst> Steering Controllers Library <../steering_controllers_library/doc/userdoc.rst> Tricycle Controller <../tricycle_controller/doc/userdoc.rst> - Swerve Drive Controller <../swerve_drive_controller/doc/userdoc.rst> Controllers for Manipulators and Other Robots ********************************************* @@ -82,4 +81,4 @@ Common Controller Parameters Every controller and broadcaster has a few common parameters. They are optional, but if needed they have to be set before ``onConfigure`` transition to ``inactive`` state, see `lifecycle documents `__. Once the controllers are already loaded, this transition is done using the service ``configure_controller`` of the controller_manager. * ``update_rate``: An unsigned integer parameter representing the rate at which every controller/broadcaster runs its update cycle. When unspecified, they run at the same frequency as the controller_manager. -* ``is_async``: A boolean parameter that is needed to specify if the controller update needs to run asynchronously. +* ``is_async``: A boolean parameter that is needed to specify if the controller update needs to run asynchronously. \ No newline at end of file diff --git a/doc/images/swerve_drive.drawio b/doc/images/swerve_drive.drawio index cfea2c73a9..7f79cad6a5 100644 --- a/doc/images/swerve_drive.drawio +++ b/doc/images/swerve_drive.drawio @@ -1,474 +1,662 @@ - - - + + + - - + + - - + + - - + + - - - - - - + + - - - - + + - - - - - - + + - - - - + + - - + + - - + + - - + + - - + + - - - - - + + + + - - - - - + + + + - - - - - - + + - - - - - + + + + - - - - - + + + + - - - + + + + - - - - - + + + + - - - + + + + - + + + + - - + + - + - - - - + + - - + + - - + + - - - - - + + + + - - - + + + + - - - - - + + + + - - - - + + - - - - - - + + - - - - - - + + - - - - + + + + + + + + - - - - - + + + + + + + + + + + + + - - - + + + + - - + + - - + + - - + + - - + + - - - - - + + + + - - - - - + + + + - - - - - + + + + - - - - + - - - - - + + - - + + + + + + + + - - + + - - + + - - - - - + + + + + + + - - - + + + + - - - - - + + + + + + + + + + + + + - - - + + + + - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - + + + + - - - - - + + + + - - - + + + + - - + + + + + - - + + - - + + - - + + - - - - - + + + + + + + - - - - - + + + + - - - - - + + + + + + + + + + + + + - - - - - + + + + - - + + + + + + + + - + - - + + - + - - + + - - + + - - - - - + + + + + + + + + + - - - + + + + - - - - - + + + + + + + + + + + + + + + + + + + + + + - - + + + + + + + + + + + - - + + - - - - - + + + + + + + - - - + + + + - - + + - - + + - - - - - + + + + - - - + + + + - - + + - - + + + + + - - - - - + + + + - - - + + + + - - - - - + + + + + + + + + + - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - + + + + + + + + + + + + + + + + + + + + + + + - - + + - - - - - + + + + - - - - - + + + + - - - - - + + + + + + + + + + + + + - - - - - + + + + - + + + + - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - + - - + + + + + + + + + + + + + + - - + + diff --git a/doc/images/swerve_drive.svg b/doc/images/swerve_drive.svg index 72190a4e70..7afefd3c18 100644 --- a/doc/images/swerve_drive.svg +++ b/doc/images/swerve_drive.svg @@ -1,4 +1,4 @@ -
Moving Forward
xb
yb
0
2
3
1
vb,x
vb,y

wb,z

xb
yb
vb,x
vb,y
0
1
2
3
Moving Right

wb,z

xb
yb
vb,x
vb,y
0
1
2
3
Moving Left

wb,z

xw
yw
yb
xb
0
1
vb,x
vb,y
3
2

wb,z

Rotating Clockwise
 \ No newline at end of file +
0
1
2
3
l
0
1
2
3
Rotating Clockwise
w
0
1
2
3
Moving Forward
%3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22%26lt%3Bfont%20style%3D%26quot%3Bcolor%3A%20light-dark(rgb(255%2C%20255%2C%20255)%2C%20rgb(255%2C%20255%2C%20255))%3B%26quot%3B%26gt%3B0%26lt%3B%2Ffont%26gt%3B%22%20style%3D%22text%3Bhtml%3D1%3Balign%3Dcenter%3BverticalAlign%3Dmiddle%3Bresizable%3D0%3Bpoints%3D%5B%5D%3Bautosize%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3B%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%2282%22%20y%3D%22185%22%20width%3D%2230%22%20height%3D%2230%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E%3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22%26lt%3Bfont%20style%3D%26quot%3Bcolor%3A%20light-dark(rgb(255%2C%20255%2C%20255)%2C%20rgb(255%2C%20255%2C%20255))%3B%26quot%3B%26gt%3B0%26lt%3B%2Ffont%26gt%3B%22%20style%3D%22text%3Bhtml%3D1%3Balign%3Dcenter%3BverticalAlign%3Dmiddle%3Bresizable%3D0%3Bpoints%3D%5B%5D%3Bautosize%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3B%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%2282%22%20y%3D%22185%22%20width%3D%2230%22%20height%3D%2230%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E
0
1
2
3
Moving Right
0
1
2
3
Moving Left
 \ No newline at end of file diff --git a/doc/migration.rst b/doc/migration.rst index db48cc9355..ba2c690e1b 100644 --- a/doc/migration.rst +++ b/doc/migration.rst @@ -11,3 +11,9 @@ The ``effort_controllers/GripperActionController`` and ``position_controllers/Gr diff_drive_controller ***************************** * Parameters ``has_velocity_limits``, ``has_acceleration_limits``, and ``has_jerk_limits`` are removed. Instead, set the respective limits to ``.NAN``. (`#1653 `_). + +pid_controller +***************************** +* Parameters ``enable_feedforward`` and service ``set_feedforward_control`` are removed. Instead, set the feedforward_gain to zero or a non-zero value. (`#1553 `_). +* The legacy ``antiwindup`` boolean and integral clamp parameters ``i_clamp_max``/``i_clamp_min`` have + been deprecated in favor of the new ``antiwindup_strategy`` parameter (`#1585 `__). Choose a suitable anti-windup strategy and set the parameters accordingly. \ No newline at end of file diff --git a/doc/mobile_robot_kinematics.rst b/doc/mobile_robot_kinematics.rst index 27cdfa0efe..28c077b675 100644 --- a/doc/mobile_robot_kinematics.rst +++ b/doc/mobile_robot_kinematics.rst @@ -114,10 +114,9 @@ The body twist of the robot can be obtained from the wheel velocities by using t Swerve Drive Robots ,,,,,,,,,,,,,,,,,,, -The below explains the kinematics of omnidirectional drive robots using four swerve modules, each with independently controlled steering and driving motors. It follows the coordinate conventions defined in `ROS REP 103 `__. +The below explains the kinematics of omnidirectional drive robots using four swerve modules, each with independently controlled steering and driving motors. It follows the coordinate conventions defined in `REP-103 `__. .. image:: images/swerve_drive.svg - :width: 550 :align: center :alt: Swerve Drive Robot @@ -130,15 +129,16 @@ The below explains the kinematics of omnidirectional drive robots using four swe * :math:`w` is the track width (distance between left and right wheels). * Red arrows on wheel :math:`i` signify the direction of the wheel's velocity :math:`v_i`. -Each swerve module :math:`i` (for :math:`i = 0, 1, 2, 3`, typically front-left, front-right, back-left, back-right) is located at :math:`(l_{i,x}, l_{i,y})` relative to the center, typically: -- Front-left: :math:`(l/2, w/2)` -- Front-right: :math:`(l/2, -w/2)` -- Back-left: :math:`(-l/2, w/2)` -- Back-right: :math:`(-l/2, -w/2)` +Each swerve module :math:`i`, for :math:`i = 0, 1, 2, 3` (typically front-left, front-right, back-left, back-right) is located at :math:`(l_{i,x}, l_{i,y})` relative to the center, typically: + +* Front-left: :math:`(l/2, w/2)` +* Front-right: :math:`(l/2, -w/2)` +* Back-left: :math:`(-l/2, w/2)` +* Back-right: :math:`(-l/2, -w/2)` **Inverse Kinematics** -The necessary wheel velocities and steering angles to achieve a desired body twist are computed by the `SwerveDriveKinematics` class in the `swerve_drive_controller` package. For each module :math:`i` at position :math:`(l_{i,x}, l_{i,y})`, the velocity vector is: +For each module :math:`i` at position :math:`(l_{i,x}, l_{i,y})`, the velocity vector is: .. math:: @@ -163,7 +163,7 @@ The wheel velocity :math:`v_i` and steering angle :math:`\phi_i` are: \phi_i = \arctan2(v_{i,y}, v_{i,x}) -**Forward Kinematics** +**Odometry** The body twist of the robot is computed from the wheel velocities :math:`v_i` and steering angles :math:`\phi_i`. Each module’s velocity components in the body frame are: @@ -182,9 +182,8 @@ The chassis velocities are calculated as: \omega_{b,z} = \frac{\sum_{i=0}^{3} (v_{i,y} l_{i,x} - v_{i,x} l_{i,y})}{\sum_{i=0}^{3} (l_{i,x}^2 + l_{i,y}^2)} -**Odometry** -The `SwerveDriveKinematics` class updates the robot’s pose (:math:`x`, :math:`y`, :math:`\theta`) in the global frame using the computed chassis velocities. The global velocities are: +Odometry updates the robot’s pose (:math:`x`, :math:`y`, :math:`\theta`) in the global frame using the computed chassis velocities. The global velocities are: .. math:: @@ -194,12 +193,6 @@ The `SwerveDriveKinematics` class updates the robot’s pose (:math:`x`, :math:` v_{y,\text{global}} = v_{b,x} \sin(\theta) + v_{b,y} \cos(\theta) -The pose is updated via Euler integration over time step :math:`\Delta t`: - -.. math:: - - \dot{x} = v_{x,\text{global}}, \quad \dot{y} = v_{y,\text{global}}, \quad \dot{\theta} = \omega_{b,z} - Nonholonomic Wheeled Mobile Robots ..................................... @@ -495,4 +488,4 @@ If there is no slip and the encoders are ideal, holds. But to get a more robust solution, we take the average of both , i.e., .. math:: - v_{b,x} = 0.5 \left( v_{front,left} \frac{R_b\sin(\phi_{left})}{l-d_{kp}\sin(\phi_{left})} + v_{front,right} \frac{R_b\sin(\phi_{right})}{l+d_{kp}\sin(\phi_{right})}\right). + v_{b,x} = 0.5 \left( v_{front,left} \frac{R_b\sin(\phi_{left})}{l-d_{kp}\sin(\phi_{left})} + v_{front,right} \frac{R_b\sin(\phi_{right})}{l+d_{kp}\sin(\phi_{right})}\right). \ No newline at end of file diff --git a/doc/release_notes.rst b/doc/release_notes.rst index f9df286eb3..abc46fffdc 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -6,4 +6,15 @@ This list summarizes the changes between Jazzy (previous) and Kilted (current) r force_torque_sensor_broadcaster ******************************* -* Multiplier support was added. Users can now specify per–axis scaling factors for both force and torque readings, applied after the existing offset logic. (`#1647 `__. +* Multiplier support was added. Users can now specify per–axis scaling factors for both force and torque readings, applied after the existing offset logic. (`#1647 `__). + +joint_trajectory_controller +******************************* +* The controller now supports the new anti-windup strategy of the PID class, which allows for more flexible control of the anti-windup behavior. (`#1759 `__). + +pid_controller +******************************* +* The controller now supports the new anti-windup strategy of the PID class, which allows for more flexible control of the anti-windup behavior (`#1585 `__). + * Output clamping via ``u_clamp_max`` and ``u_clamp_min`` was added, allowing users to bound the controller output. + * The legacy ``antiwindup`` boolean and integral clamp parameters ``i_clamp_max``/``i_clamp_min`` have been deprecated in favor of the new ``antiwindup_strategy`` parameter. A ``tracking_time_constant`` parameter has also been introduced to configure the back-calculation strategy. + * A new ``error_deadband`` parameter stops integration when the error is within a specified range. \ No newline at end of file diff --git a/doc/writing_new_controller.rst b/doc/writing_new_controller.rst index 0874448461..41b3414cf0 100644 --- a/doc/writing_new_controller.rst +++ b/doc/writing_new_controller.rst @@ -144,4 +144,4 @@ Useful External References - `Templates and scripts for generating controllers shell `_ - .. NOTE:: The script is currently only recommended to use with Humble, not compatible with the API from Jazzy and onwards. + .. NOTE:: The script is currently only recommended to use with Humble, not compatible with the API from Jazzy and onwards. \ No newline at end of file From 42c3328a1b210a38009d2eeae00eec696f9ee097 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 22 Jul 2025 17:46:03 +0100 Subject: [PATCH 10/19] Fix format --- doc/controllers_index.rst | 2 +- doc/mobile_robot_kinematics.rst | 2 +- doc/release_notes.rst | 1 - doc/writing_new_controller.rst | 2 +- .../swerve_drive_controller.hpp | 4 +- .../swerve_drive_kinematics.hpp | 1 - .../src/swerve_drive_controller.cpp | 205 +++++++++++------- .../src/swerve_drive_kinematics.cpp | 17 +- .../test/test_swerve_drive_controller.cpp | 88 +++++--- 9 files changed, 202 insertions(+), 120 deletions(-) diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index 01a3e127a9..235800711b 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -81,4 +81,4 @@ Common Controller Parameters Every controller and broadcaster has a few common parameters. They are optional, but if needed they have to be set before ``onConfigure`` transition to ``inactive`` state, see `lifecycle documents `__. Once the controllers are already loaded, this transition is done using the service ``configure_controller`` of the controller_manager. * ``update_rate``: An unsigned integer parameter representing the rate at which every controller/broadcaster runs its update cycle. When unspecified, they run at the same frequency as the controller_manager. -* ``is_async``: A boolean parameter that is needed to specify if the controller update needs to run asynchronously. \ No newline at end of file +* ``is_async``: A boolean parameter that is needed to specify if the controller update needs to run asynchronously. diff --git a/doc/mobile_robot_kinematics.rst b/doc/mobile_robot_kinematics.rst index 28c077b675..4bf285972e 100644 --- a/doc/mobile_robot_kinematics.rst +++ b/doc/mobile_robot_kinematics.rst @@ -488,4 +488,4 @@ If there is no slip and the encoders are ideal, holds. But to get a more robust solution, we take the average of both , i.e., .. math:: - v_{b,x} = 0.5 \left( v_{front,left} \frac{R_b\sin(\phi_{left})}{l-d_{kp}\sin(\phi_{left})} + v_{front,right} \frac{R_b\sin(\phi_{right})}{l+d_{kp}\sin(\phi_{right})}\right). \ No newline at end of file + v_{b,x} = 0.5 \left( v_{front,left} \frac{R_b\sin(\phi_{left})}{l-d_{kp}\sin(\phi_{left})} + v_{front,right} \frac{R_b\sin(\phi_{right})}{l+d_{kp}\sin(\phi_{right})}\right). diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 9c5729ba16..e831cf6f49 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -24,4 +24,3 @@ pid_controller * Output clamping via ``u_clamp_max`` and ``u_clamp_min`` was added, allowing users to bound the controller output. * The legacy ``antiwindup`` boolean and integral clamp parameters ``i_clamp_max``/``i_clamp_min`` have been deprecated in favor of the new ``antiwindup_strategy`` parameter. A ``tracking_time_constant`` parameter has also been introduced to configure the back-calculation strategy. * A new ``error_deadband`` parameter stops integration when the error is within a specified range. - diff --git a/doc/writing_new_controller.rst b/doc/writing_new_controller.rst index 41b3414cf0..0874448461 100644 --- a/doc/writing_new_controller.rst +++ b/doc/writing_new_controller.rst @@ -144,4 +144,4 @@ Useful External References - `Templates and scripts for generating controllers shell `_ - .. NOTE:: The script is currently only recommended to use with Humble, not compatible with the API from Jazzy and onwards. \ No newline at end of file + .. NOTE:: The script is currently only recommended to use with Humble, not compatible with the API from Jazzy and onwards. diff --git a/swerve_drive_controller/include/swerve_drive_controller/swerve_drive_controller.hpp b/swerve_drive_controller/include/swerve_drive_controller/swerve_drive_controller.hpp index 2c9b9ac34d..36c774bdba 100644 --- a/swerve_drive_controller/include/swerve_drive_controller/swerve_drive_controller.hpp +++ b/swerve_drive_controller/include/swerve_drive_controller/swerve_drive_controller.hpp @@ -179,11 +179,11 @@ class SwerveController : public controller_interface::ControllerInterface std::shared_ptr> odometry_publisher_ = nullptr; std::shared_ptr> - realtime_odometry_publisher_ = nullptr; + realtime_odometry_publisher_ = nullptr; std::shared_ptr> odometry_transform_publisher_ = nullptr; std::shared_ptr> - realtime_odometry_transform_publisher_ = nullptr; + realtime_odometry_transform_publisher_ = nullptr; bool is_halted_ = false; diff --git a/swerve_drive_controller/include/swerve_drive_controller/swerve_drive_kinematics.hpp b/swerve_drive_controller/include/swerve_drive_controller/swerve_drive_kinematics.hpp index 38c2559218..dd4580e620 100644 --- a/swerve_drive_controller/include/swerve_drive_controller/swerve_drive_kinematics.hpp +++ b/swerve_drive_controller/include/swerve_drive_controller/swerve_drive_kinematics.hpp @@ -25,7 +25,6 @@ #include #include - namespace swerve_drive_controller { diff --git a/swerve_drive_controller/src/swerve_drive_controller.cpp b/swerve_drive_controller/src/swerve_drive_controller.cpp index 6a3d4cbc90..bb73e3fc77 100644 --- a/swerve_drive_controller/src/swerve_drive_controller.cpp +++ b/swerve_drive_controller/src/swerve_drive_controller.cpp @@ -53,9 +53,9 @@ Wheel::Wheel( { } -void Wheel::set_velocity(double velocity) {velocity_.get().set_value(velocity);} +void Wheel::set_velocity(double velocity) { velocity_.get().set_value(velocity); } -double Wheel::get_feedback() {return Wheel::feedback_.get().get_optional().value();} +double Wheel::get_feedback() { return Wheel::feedback_.get().get_optional().value(); } Axle::Axle( std::reference_wrapper position, @@ -64,15 +64,15 @@ Axle::Axle( { } -void Axle::set_position(double position) {position_.get().set_value(position);} +void Axle::set_position(double position) { position_.get().set_value(position); } -double Axle::get_feedback() {return Axle::feedback_.get().get_optional().value();} +double Axle::get_feedback() { return Axle::feedback_.get().get_optional().value(); } std::array, 4> wheel_positions_ = {{ - {-0.1, 0.175}, // front left - {0.1, 0.175}, // front right - {-0.1, -0.175}, // rear left - {0.1, -0.175} // rear right + {-0.1, 0.175}, // front left + {0.1, 0.175}, // front right + {-0.1, -0.175}, // rear left + {0.1, -0.175} // rear right }}; SwerveController::SwerveController() @@ -89,11 +89,13 @@ SwerveController::SwerveController() CallbackReturn SwerveController::on_init() { auto node = get_node(); - if (!node) { + if (!node) + { RCLCPP_ERROR(rclcpp::get_logger("SwerveController"), "Node is null in on_init"); return controller_interface::CallbackReturn::ERROR; } - try { + try + { // Declare parameters auto_declare("joint_steering_left_front", front_left_axle_joint_name_); auto_declare("joint_steering_right_front", front_right_axle_joint_name_); @@ -120,7 +122,9 @@ CallbackReturn SwerveController::on_init() auto_declare("front_right_velocity_threshold", front_right_velocity_threshold_); auto_declare("rear_left_velocity_threshold", rear_left_velocity_threshold_); auto_declare("rear_right_velocity_threshold", rear_right_velocity_threshold_); - } catch (const std::exception & e) { + } + catch (const std::exception & e) + { return CallbackReturn::ERROR; } return CallbackReturn::SUCCESS; @@ -163,7 +167,8 @@ CallbackReturn SwerveController::on_configure(const rclcpp_lifecycle::State & /* { // auto logger = get_node()->get_logger(); auto logger = rclcpp::get_logger("SwerveController"); - try { + try + { front_left_wheel_joint_name_ = get_node()->get_parameter("joint_wheel_left_front").as_string(); front_right_wheel_joint_name_ = get_node()->get_parameter("joint_wheel_right_front").as_string(); @@ -199,44 +204,54 @@ CallbackReturn SwerveController::on_configure(const rclcpp_lifecycle::State & /* rear_right_velocity_threshold_ = get_node()->get_parameter("rear_right_velocity_threshold").as_double(); - for (std::size_t i = 0; i < 6; ++i) { + for (std::size_t i = 0; i < 6; ++i) + { pose_covariance_diagonal_array_[i] = 0.01; } - for (std::size_t i = 0; i < 6; ++i) { + for (std::size_t i = 0; i < 6; ++i) + { twist_covariance_diagonal_array_[i] = 0.01; } - if (front_left_wheel_joint_name_.empty()) { + if (front_left_wheel_joint_name_.empty()) + { RCLCPP_ERROR(logger, "front_wheel_joint_name is not set"); return CallbackReturn::ERROR; } - if (front_right_wheel_joint_name_.empty()) { + if (front_right_wheel_joint_name_.empty()) + { RCLCPP_ERROR(logger, "front_right_wheel_joint_name is not set"); return CallbackReturn::ERROR; } - if (rear_left_wheel_joint_name_.empty()) { + if (rear_left_wheel_joint_name_.empty()) + { RCLCPP_ERROR(logger, "rear_left_wheel_joint_name is not set"); return CallbackReturn::ERROR; } - if (rear_right_wheel_joint_name_.empty()) { + if (rear_right_wheel_joint_name_.empty()) + { RCLCPP_ERROR(logger, "rear_right_wheel_joint_name is not set"); return CallbackReturn::ERROR; } - if (front_left_axle_joint_name_.empty()) { + if (front_left_axle_joint_name_.empty()) + { RCLCPP_ERROR(logger, "front_axle_joint_name is not set"); return CallbackReturn::ERROR; } - if (front_right_axle_joint_name_.empty()) { + if (front_right_axle_joint_name_.empty()) + { RCLCPP_ERROR(logger, "front_right_axle_joint_name is not set"); return CallbackReturn::ERROR; } - if (rear_left_axle_joint_name_.empty()) { + if (rear_left_axle_joint_name_.empty()) + { RCLCPP_ERROR(logger, "rear_left_axle_joint_name_ is not set"); return CallbackReturn::ERROR; } - if (rear_right_axle_joint_name_.empty()) { + if (rear_right_axle_joint_name_.empty()) + { RCLCPP_ERROR(logger, "rear_right_axle_joint_name_ is not set"); return CallbackReturn::ERROR; } @@ -254,7 +269,8 @@ CallbackReturn SwerveController::on_configure(const rclcpp_lifecycle::State & /* cmd_vel_timeout_ = std::chrono::milliseconds( static_cast(get_node()->get_parameter("cmd_vel_timeout").as_double() * 1000.0)); - if (!reset()) { + if (!reset()) + { return CallbackReturn::ERROR; } @@ -265,16 +281,19 @@ CallbackReturn SwerveController::on_configure(const rclcpp_lifecycle::State & /* empty_twist.twist.angular.z = 0.0; received_velocity_msg_ptr_.writeFromNonRT(std::make_shared(empty_twist)); - if (use_stamped_vel_) { + if (use_stamped_vel_) + { velocity_command_subscriber_ = get_node()->create_subscription( DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), [this, logger](const std::shared_ptr msg) -> void { - if (!subscriber_is_active_) { + if (!subscriber_is_active_) + { RCLCPP_WARN(logger, "Can't accept new commands. subscriber is inactive"); return; } - if ((msg->header.stamp.sec == 0) && (msg->header.stamp.nanosec == 0)) { + if ((msg->header.stamp.sec == 0) && (msg->header.stamp.nanosec == 0)) + { RCLCPP_WARN_ONCE( logger, "Received TwistStamped with zero timestamp, setting it to current " @@ -283,18 +302,21 @@ CallbackReturn SwerveController::on_configure(const rclcpp_lifecycle::State & /* } received_velocity_msg_ptr_.writeFromNonRT(std::move(msg)); }); - } else { + } + else + { velocity_command_unstamped_subscriber_ = get_node()->create_subscription( DEFAULT_COMMAND_UNSTAMPED_TOPIC, rclcpp::SystemDefaultsQoS(), [this, logger](const std::shared_ptr msg) -> void { - if (!subscriber_is_active_) { + if (!subscriber_is_active_) + { RCLCPP_WARN(logger, "Can't accept new commands. subscriber is inactive"); return; } // Write fake header in the stored stamped command const std::shared_ptr twist_stamped = - *(received_velocity_msg_ptr_.readFromRT()); + *(received_velocity_msg_ptr_.readFromRT()); twist_stamped->twist = *msg; twist_stamped->header.stamp = get_node()->get_clock()->now(); }); @@ -309,9 +331,12 @@ CallbackReturn SwerveController::on_configure(const rclcpp_lifecycle::State & /* std::string tf_prefix = ""; tf_prefix = std::string(get_node()->get_namespace()); - if (tf_prefix == "/") { + if (tf_prefix == "/") + { tf_prefix = ""; - } else { + } + else + { tf_prefix = tf_prefix + "/"; } @@ -328,7 +353,8 @@ CallbackReturn SwerveController::on_configure(const rclcpp_lifecycle::State & /* geometry_msgs::msg::TwistWithCovariance(rosidl_runtime_cpp::MessageInitialization::ALL); constexpr std::size_t NUM_DIMENSIONS = 6; - for (std::size_t index = 0; index < 6; ++index) { + for (std::size_t index = 0; index < 6; ++index) + { const std::size_t diagonal_index = NUM_DIMENSIONS * index + index; odometry_message.pose.covariance[diagonal_index] = pose_covariance_diagonal_array_[index]; odometry_message.twist.covariance[diagonal_index] = twist_covariance_diagonal_array_[index]; @@ -347,7 +373,9 @@ CallbackReturn SwerveController::on_configure(const rclcpp_lifecycle::State & /* odometry_transform_message.transforms.front().child_frame_id = base_frame_id; previous_update_timestamp_ = get_node()->get_clock()->now(); - } catch (const std::exception & e) { + } + catch (const std::exception & e) + { RCLCPP_ERROR(logger, "EXCEPTION DURING on_configure: %s", e.what()); return CallbackReturn::ERROR; } @@ -360,25 +388,31 @@ CallbackReturn SwerveController::on_activate(const rclcpp_lifecycle::State &) auto logger = rclcpp::get_logger("SwerveController"); wheel_handles_.resize(4); - for(std::size_t i = 0; i < 4; i++) { + for (std::size_t i = 0; i < 4; i++) + { wheel_handles_[i] = get_wheel(wheel_joint_names[i]); } axle_handles_.resize(4); - for(std::size_t i = 0; i < 4; i++) { + for (std::size_t i = 0; i < 4; i++) + { axle_handles_[i] = get_axle(axle_joint_names[i]); } - for(std::size_t i = 0; i < wheel_handles_.size(); ++i) { - if(!wheel_handles_[i]) { + for (std::size_t i = 0; i < wheel_handles_.size(); ++i) + { + if (!wheel_handles_[i]) + { RCLCPP_ERROR(logger, "ERROR IN FETCHING wheel handle for: %s", wheel_joint_names[i].c_str()); return CallbackReturn::ERROR; } wheel_handles_[i]->set_velocity(0.0); } - for(std::size_t i = 0; i < axle_handles_.size(); ++i) { - if(!axle_handles_[i]) { + for (std::size_t i = 0; i < axle_handles_.size(); ++i) + { + if (!axle_handles_[i]) + { RCLCPP_ERROR(logger, "ERROR IN FETCHING axle handle for: %s", axle_joint_names[i].c_str()); return CallbackReturn::ERROR; } @@ -396,8 +430,10 @@ controller_interface::return_type SwerveController::update( { auto logger = rclcpp::get_logger("SwerveController"); - if (this->get_lifecycle_state().id() == State::PRIMARY_STATE_INACTIVE) { - if (!is_halted_) { + if (this->get_lifecycle_state().id() == State::PRIMARY_STATE_INACTIVE) + { + if (!is_halted_) + { halt(); is_halted_ = true; } @@ -409,7 +445,8 @@ controller_interface::return_type SwerveController::update( std::shared_ptr last_command_msg = *(received_velocity_msg_ptr_.readFromRT()); - if (last_command_msg == nullptr) { + if (last_command_msg == nullptr) + { last_command_msg = std::make_shared(); last_command_msg->header.stamp = current_time; last_command_msg->twist.linear.x = 0.0; @@ -421,7 +458,8 @@ controller_interface::return_type SwerveController::update( const auto age_of_last_command = current_time - last_command_msg->header.stamp; - if (age_of_last_command > cmd_vel_timeout_) { + if (age_of_last_command > cmd_vel_timeout_) + { last_command_msg->twist.linear.x = 0.0; last_command_msg->twist.linear.y = 0.0; last_command_msg->twist.angular.z = 0.0; @@ -444,22 +482,26 @@ controller_interface::return_type SwerveController::update( {wheel_command[2], rear_left_velocity_threshold_, "rear_left_wheel"}, {wheel_command[3], rear_right_velocity_threshold_, "rear_right_wheel"}}; - for (const auto & [wheel_command_, threshold, label] : wheel_data) { - if (wheel_command_.drive_velocity > threshold) { + for (const auto & [wheel_command_, threshold, label] : wheel_data) + { + if (wheel_command_.drive_velocity > threshold) + { wheel_command_.drive_velocity = threshold; } } - for(std::size_t i = 0; i < 4; i++) { - if(!axle_handles_[i] || !wheel_handles_[i]) { - throw std::runtime_error("Axle or Wheel handle is nullptr for: " + - axle_joint_names[i] + " / " + wheel_joint_names[i]); + for (std::size_t i = 0; i < 4; i++) + { + if (!axle_handles_[i] || !wheel_handles_[i]) + { + throw std::runtime_error( + "Axle or Wheel handle is nullptr for: " + axle_joint_names[i] + " / " + + wheel_joint_names[i]); } axle_handles_[i]->set_position(wheel_command[i].steering_angle); wheel_handles_[i]->set_velocity(wheel_command[i].drive_velocity); } - const auto update_dt = current_time - previous_update_timestamp_; previous_update_timestamp_ = current_time; @@ -468,11 +510,15 @@ controller_interface::return_type SwerveController::update( std::array velocity_array{}; std::array steering_angle_array{}; - for(std::size_t i = 0; i < 4; ++i) { - if(open_loop_) { + for (std::size_t i = 0; i < 4; ++i) + { + if (open_loop_) + { velocity_array[i] = wheel_command[i].drive_velocity; steering_angle_array[i] = wheel_command[i].steering_angle; - } else { + } + else + { velocity_array[i] = wheel_handles_[i]->get_feedback(); steering_angle_array[i] = axle_handles_[i]->get_feedback(); } @@ -483,15 +529,20 @@ controller_interface::return_type SwerveController::update( tf2::Quaternion orientation; orientation.setRPY(0.0, 0.0, odometry_.theta); - try { - if (previous_publish_timestamp_ + publish_period_ < time) { + try + { + if (previous_publish_timestamp_ + publish_period_ < time) + { previous_publish_timestamp_ += publish_period_; } - } catch (const std::runtime_error &) { + } + catch (const std::runtime_error &) + { previous_publish_timestamp_ = time; } - if (realtime_odometry_publisher_ && realtime_odometry_publisher_->trylock()) { + if (realtime_odometry_publisher_ && realtime_odometry_publisher_->trylock()) + { auto & odometry_message = realtime_odometry_publisher_->msg_; odometry_message.header.stamp = time; odometry_message.pose.pose.position.x = odometry_.x; @@ -504,7 +555,8 @@ controller_interface::return_type SwerveController::update( realtime_odometry_publisher_->unlockAndPublish(); } - if (realtime_odometry_transform_publisher_ && realtime_odometry_transform_publisher_->trylock()) { + if (realtime_odometry_transform_publisher_ && realtime_odometry_transform_publisher_->trylock()) + { auto & transform = realtime_odometry_transform_publisher_->msg_.transforms.front(); transform.header.stamp = time; @@ -530,7 +582,8 @@ CallbackReturn SwerveController::on_deactivate(const rclcpp_lifecycle::State &) CallbackReturn SwerveController::on_cleanup(const rclcpp_lifecycle::State &) { - if (!reset()) { + if (!reset()) + { return CallbackReturn::ERROR; } @@ -540,7 +593,8 @@ CallbackReturn SwerveController::on_cleanup(const rclcpp_lifecycle::State &) CallbackReturn SwerveController::on_error(const rclcpp_lifecycle::State &) { - if (!reset()) { + if (!reset()) + { return CallbackReturn::ERROR; } return CallbackReturn::SUCCESS; @@ -569,26 +623,26 @@ CallbackReturn SwerveController::on_shutdown(const rclcpp_lifecycle::State &) void SwerveController::halt() { - for(std::size_t i = 0; i < wheel_handles_.size(); ++i) { + for (std::size_t i = 0; i < wheel_handles_.size(); ++i) + { wheel_handles_[i]->set_velocity(0.0); } - for(std::size_t i = 0; i < axle_handles_.size(); ++i) { + for (std::size_t i = 0; i < axle_handles_.size(); ++i) + { axle_handles_[i]->set_position(0.0); } } - -template +template std::unique_ptr get_interface_object( std::vector & command_interfaces, const std::vector & state_interfaces, - const std::string & name, - const std::string & interface_suffix, - const std::string & hw_if_type) + const std::string & name, const std::string & interface_suffix, const std::string & hw_if_type) { auto logger = rclcpp::get_logger("SwerveController"); - if (name.empty()) { + if (name.empty()) + { RCLCPP_ERROR(logger, "Joint name not given. Make sure all joints are specified."); return nullptr; } @@ -603,7 +657,8 @@ std::unique_ptr get_interface_object( interface.get_interface_name() == hw_if_type; }); - if (command_handle == command_interfaces.end()) { + if (command_handle == command_interfaces.end()) + { RCLCPP_ERROR(logger, "Unable to find command interface for: %s", name.c_str()); RCLCPP_ERROR(logger, "Expected interface name: %s", expected_interface_name.c_str()); return nullptr; @@ -617,13 +672,14 @@ std::unique_ptr get_interface_object( interface.get_interface_name() == hw_if_type; }); - if (state_handle == state_interfaces.end()) { - RCLCPP_ERROR(logger, "Unable to find the state interface for: %s", - name.c_str()); + if (state_handle == state_interfaces.end()) + { + RCLCPP_ERROR(logger, "Unable to find the state interface for: %s", name.c_str()); return nullptr; } - static_assert(!std::is_const_v>, - "Command handle is const!"); + static_assert( + !std::is_const_v>, + "Command handle is const!"); return std::make_unique(std::ref(*command_handle), std::ref(*state_handle), name); } @@ -639,7 +695,6 @@ std::unique_ptr SwerveController::get_axle(const std::string & axle_name) command_interfaces_, state_interfaces_, axle_name, "/position", HW_IF_POSITION); } - } // namespace swerve_drive_controller #include "class_loader/register_macro.hpp" diff --git a/swerve_drive_controller/src/swerve_drive_kinematics.cpp b/swerve_drive_controller/src/swerve_drive_kinematics.cpp index d6bd248a1f..eebc2bfc3c 100644 --- a/swerve_drive_controller/src/swerve_drive_kinematics.cpp +++ b/swerve_drive_controller/src/swerve_drive_kinematics.cpp @@ -29,7 +29,8 @@ std::array SwerveDriveKinematics::compute_wheel_commands( // wx = W/2, wy = L/2 - for (std::size_t i = 0; i < 4; i++) { + for (std::size_t i = 0; i < 4; i++) + { const auto & [wx, wy] = wheel_positions_[i]; double vx = linear_velocity_x - angular_velocity_z * wy; @@ -48,7 +49,8 @@ OdometryState SwerveDriveKinematics::update_odometry( { // Compute robot-centric velocity (assuming perfect wheel control) double vx_sum = 0.0, vy_sum = 0.0, wz_sum = 0.0; - for (std::size_t i = 0; i < 4; i++) { + for (std::size_t i = 0; i < 4; i++) + { double vx = wheel_velocities[i] * std::cos(steering_angles[i]); double vy = wheel_velocities[i] * std::sin(steering_angles[i]); @@ -63,10 +65,11 @@ OdometryState SwerveDriveKinematics::update_odometry( double vy_robot = vy_sum / 4.0; double wz_denominator = 0.0; - for (std::size_t i = 0; i < 4; i++) { + for (std::size_t i = 0; i < 4; i++) + { wz_denominator += (wheel_positions_[i].first * wheel_positions_[i].first + - wheel_positions_[i].second * wheel_positions_[i].second); + wheel_positions_[i].second * wheel_positions_[i].second); } double wz_robot = wz_sum / wz_denominator; @@ -86,10 +89,12 @@ OdometryState SwerveDriveKinematics::update_odometry( double SwerveDriveKinematics::normalize_angle(double angle) { - while (angle > M_PI) { + while (angle > M_PI) + { angle -= 2.0 * M_PI; } - while (angle < -M_PI) { + while (angle < -M_PI) + { angle += 2.0 * M_PI; } return angle; diff --git a/swerve_drive_controller/test/test_swerve_drive_controller.cpp b/swerve_drive_controller/test/test_swerve_drive_controller.cpp index b698b19f1c..05b93fcbf6 100644 --- a/swerve_drive_controller/test/test_swerve_drive_controller.cpp +++ b/swerve_drive_controller/test/test_swerve_drive_controller.cpp @@ -63,20 +63,21 @@ void SwerveDriveControllerTest::SetUp() // Define expected interfaces command_interfaces_ = {"front_left_wheel_joint/velocity", "front_right_wheel_joint/velocity", - "rear_left_wheel_joint/velocity", "rear_right_wheel_joint/velocity", - "front_left_axle_joint/position", "front_right_axle_joint/position", - "rear_left_axle_joint/position", "rear_right_axle_joint/position"}; + "rear_left_wheel_joint/velocity", "rear_right_wheel_joint/velocity", + "front_left_axle_joint/position", "front_right_axle_joint/position", + "rear_left_axle_joint/position", "rear_right_axle_joint/position"}; state_interfaces_ = {"front_left_wheel_joint/velocity", "front_right_wheel_joint/velocity", - "rear_left_wheel_joint/velocity", "rear_right_wheel_joint/velocity", - "front_left_axle_joint/position", "front_right_axle_joint/position", - "rear_left_axle_joint/position", "rear_right_axle_joint/position"}; + "rear_left_wheel_joint/velocity", "rear_right_wheel_joint/velocity", + "front_left_axle_joint/position", "front_right_axle_joint/position", + "rear_left_axle_joint/position", "rear_right_axle_joint/position"}; } void SwerveDriveControllerTest::SetUpController() { RCLCPP_INFO(node_->get_logger(), "Creating SwerveController instance"); controller_ = std::make_shared(); - if (!controller_) { + if (!controller_) + { RCLCPP_ERROR(node_->get_logger(), "Failed to create SwerveController instance"); GTEST_FAIL() << "Controller creation failed"; } @@ -91,7 +92,8 @@ void SwerveDriveControllerTest::SetUpController() "", // node_namespace (default) rclcpp::NodeOptions() // node_options (default) ); - if (init_result != controller_interface::return_type::OK) { + if (init_result != controller_interface::return_type::OK) + { RCLCPP_ERROR(node_->get_logger(), "Controller node initialization failed"); GTEST_FAIL() << "Controller node initialization failed"; } @@ -105,7 +107,8 @@ void SwerveDriveControllerTest::SetUpInterfaces() command_interfaces_base_.reserve(command_interfaces_.size()); state_interfaces_base_.reserve(state_interfaces_.size()); - for (size_t i = 0; i < command_interfaces_.size(); ++i) { + for (size_t i = 0; i < command_interfaces_.size(); ++i) + { const auto & name = command_interfaces_[i]; auto interface_type = name.substr(name.find_last_of("/") + 1); command_interfaces_base_.emplace_back( @@ -113,7 +116,8 @@ void SwerveDriveControllerTest::SetUpInterfaces() name.substr(0, name.find_last_of("/")), interface_type, &command_values_[i])); command_interface_handles_.emplace_back(command_interfaces_base_.back()); } - for (size_t i = 0; i < state_interfaces_.size(); ++i) { + for (size_t i = 0; i < state_interfaces_.size(); ++i) + { const auto & name = state_interfaces_[i]; auto interface_type = name.substr(name.find_last_of("/") + 1); state_interfaces_base_.emplace_back( @@ -159,13 +163,15 @@ TEST_F(SwerveDriveControllerTest, test_on_init_success) TEST_F(SwerveDriveControllerTest, test_command_interface_configuration) { SetUpController(); - if (!init_successful_) { + if (!init_successful_) + { GTEST_SKIP() << "Skipping due to on_init failure"; } auto config = controller_->command_interface_configuration(); EXPECT_EQ(config.type, controller_interface::interface_configuration_type::INDIVIDUAL); EXPECT_EQ(config.names.size(), 8u); - for (const auto & name : command_interfaces_) { + for (const auto & name : command_interfaces_) + { EXPECT_NE(std::find(config.names.begin(), config.names.end(), name), config.names.end()) << "Expected command interface " << name << " not found"; } @@ -174,13 +180,15 @@ TEST_F(SwerveDriveControllerTest, test_command_interface_configuration) TEST_F(SwerveDriveControllerTest, test_state_interface_configuration) { SetUpController(); - if (!init_successful_) { + if (!init_successful_) + { GTEST_SKIP() << "Skipping due to on_init failure"; } auto config = controller_->state_interface_configuration(); EXPECT_EQ(config.type, controller_interface::interface_configuration_type::INDIVIDUAL); EXPECT_EQ(config.names.size(), 8u); - for (const auto & name : state_interfaces_) { + for (const auto & name : state_interfaces_) + { EXPECT_NE(std::find(config.names.begin(), config.names.end(), name), config.names.end()) << "Expected state interface " << name << " not found"; } @@ -189,7 +197,8 @@ TEST_F(SwerveDriveControllerTest, test_state_interface_configuration) TEST_F(SwerveDriveControllerTest, test_on_configure_success) { SetUpController(); - if (!init_successful_) { + if (!init_successful_) + { GTEST_SKIP() << "Skipping due to on_init failure"; } EXPECT_EQ( @@ -200,7 +209,8 @@ TEST_F(SwerveDriveControllerTest, test_on_configure_success) TEST_F(SwerveDriveControllerTest, test_on_configure_missing_parameters) { SetUpController(); - if (!init_successful_) { + if (!init_successful_) + { GTEST_SKIP() << "Skipping due to on_init failure"; } node_->undeclare_parameter("joint_wheel_left_front"); @@ -212,7 +222,8 @@ TEST_F(SwerveDriveControllerTest, test_on_configure_missing_parameters) TEST_F(SwerveDriveControllerTest, test_on_activate_success) { SetUpController(); - if (!init_successful_) { + if (!init_successful_) + { GTEST_SKIP() << "Skipping due to on_init failure"; } ASSERT_EQ( @@ -227,7 +238,8 @@ TEST_F(SwerveDriveControllerTest, test_on_activate_success) TEST_F(SwerveDriveControllerTest, test_on_activate_missing_interfaces) { SetUpController(); - if (!init_successful_) { + if (!init_successful_) + { GTEST_SKIP() << "Skipping due to on_init failure"; } ASSERT_EQ( @@ -241,7 +253,8 @@ TEST_F(SwerveDriveControllerTest, test_on_activate_missing_interfaces) TEST_F(SwerveDriveControllerTest, test_on_deactivate_success) { SetUpController(); - if (!init_successful_) { + if (!init_successful_) + { GTEST_SKIP() << "Skipping due to on_init failure"; } ASSERT_EQ( @@ -259,7 +272,8 @@ TEST_F(SwerveDriveControllerTest, test_on_deactivate_success) TEST_F(SwerveDriveControllerTest, test_on_cleanup_success) { SetUpController(); - if (!init_successful_) { + if (!init_successful_) + { GTEST_SKIP() << "Skipping due to on_init failure"; } ASSERT_EQ( @@ -273,7 +287,8 @@ TEST_F(SwerveDriveControllerTest, test_on_cleanup_success) TEST_F(SwerveDriveControllerTest, test_on_error_success) { SetUpController(); - if (!init_successful_) { + if (!init_successful_) + { GTEST_SKIP() << "Skipping due to on_init failure"; } ASSERT_EQ( @@ -287,7 +302,8 @@ TEST_F(SwerveDriveControllerTest, test_on_error_success) TEST_F(SwerveDriveControllerTest, test_on_shutdown_success) { SetUpController(); - if (!init_successful_) { + if (!init_successful_) + { GTEST_SKIP() << "Skipping due to on_init failure"; } EXPECT_EQ( @@ -298,7 +314,8 @@ TEST_F(SwerveDriveControllerTest, test_on_shutdown_success) TEST_F(SwerveDriveControllerTest, test_update_inactive_state) { SetUpController(); - if (!init_successful_) { + if (!init_successful_) + { GTEST_SKIP() << "Skipping due to on_init failure"; } ASSERT_EQ( @@ -314,10 +331,12 @@ TEST_F(SwerveDriveControllerTest, test_update_inactive_state) EXPECT_EQ( controller_->update(node_->now(), rclcpp::Duration::from_seconds(0.02)), controller_interface::return_type::OK); - for (size_t i = 0; i < 4; ++i) { + for (size_t i = 0; i < 4; ++i) + { EXPECT_EQ(command_values_[i], 0.0) << "Wheel velocity " << i << " not zeroed"; } - for (size_t i = 4; i < 8; ++i) { + for (size_t i = 4; i < 8; ++i) + { EXPECT_EQ(command_values_[i], 0.0) << "Axle position " << i << " not zeroed"; } } @@ -325,7 +344,8 @@ TEST_F(SwerveDriveControllerTest, test_update_inactive_state) TEST_F(SwerveDriveControllerTest, test_update_with_velocity_command) { SetUpController(); - if (!init_successful_) { + if (!init_successful_) + { GTEST_SKIP() << "Skipping due to on_init failure"; } ASSERT_EQ( @@ -338,7 +358,7 @@ TEST_F(SwerveDriveControllerTest, test_update_with_velocity_command) // Set up odometry subscriber odom_sub_ = node_->create_subscription( - "~/odom", 10, [this](const nav_msgs::msg::Odometry::SharedPtr msg) {last_odom_msg_ = msg;}); + "~/odom", 10, [this](const nav_msgs::msg::Odometry::SharedPtr msg) { last_odom_msg_ = msg; }); // Set up velocity publisher twist_stamped_pub_ = node_->create_publisher( @@ -368,7 +388,8 @@ TEST_F(SwerveDriveControllerTest, test_update_with_velocity_command) TEST_F(SwerveDriveControllerTest, test_update_with_timeout) { SetUpController(); - if (!init_successful_) { + if (!init_successful_) + { GTEST_SKIP() << "Skipping due to on_init failure"; } ASSERT_EQ( @@ -394,10 +415,12 @@ TEST_F(SwerveDriveControllerTest, test_update_with_timeout) controller_->update(node_->now(), rclcpp::Duration::from_seconds(0.02)), controller_interface::return_type::OK); - for (size_t i = 0; i < 4; ++i) { + for (size_t i = 0; i < 4; ++i) + { EXPECT_EQ(command_values_[i], 0.0) << "Wheel velocity " << i << " not zeroed"; } - for (size_t i = 4; i < 8; ++i) { + for (size_t i = 4; i < 8; ++i) + { EXPECT_EQ(command_values_[i], 0.0) << "Axle position " << i << " not zeroed"; } } @@ -405,7 +428,8 @@ TEST_F(SwerveDriveControllerTest, test_update_with_timeout) TEST_F(SwerveDriveControllerTest, test_unstamped_velocity_command) { SetUpController(); - if (!init_successful_) { + if (!init_successful_) + { GTEST_SKIP() << "Skipping due to on_init failure"; } node_->set_parameter(rclcpp::Parameter("use_stamped_vel", false)); @@ -421,7 +445,7 @@ TEST_F(SwerveDriveControllerTest, test_unstamped_velocity_command) twist_pub_ = node_->create_publisher( "~/cmd_vel_unstamped", rclcpp::SystemDefaultsQoS()); odom_sub_ = node_->create_subscription( - "~/odom", 10, [this](const nav_msgs::msg::Odometry::SharedPtr msg) {last_odom_msg_ = msg;}); + "~/odom", 10, [this](const nav_msgs::msg::Odometry::SharedPtr msg) { last_odom_msg_ = msg; }); // Publish an unstamped velocity command (1 m/s lateral) PublishTwist(0.0, 1.0, 0.0); From 879ecb2db0f5fd0d88f892bb15cf03cd5352f32a Mon Sep 17 00:00:00 2001 From: nitin Date: Wed, 23 Jul 2025 01:23:13 +0530 Subject: [PATCH 11/19] Mofied Joint And Axle Name Variables Signed-off-by: nitin --- .../swerve_drive_controller.hpp | 24 ++-- .../src/swerve_drive_controller.cpp | 136 +++++++++--------- 2 files changed, 80 insertions(+), 80 deletions(-) diff --git a/swerve_drive_controller/include/swerve_drive_controller/swerve_drive_controller.hpp b/swerve_drive_controller/include/swerve_drive_controller/swerve_drive_controller.hpp index 36c774bdba..f599851ed1 100644 --- a/swerve_drive_controller/include/swerve_drive_controller/swerve_drive_controller.hpp +++ b/swerve_drive_controller/include/swerve_drive_controller/swerve_drive_controller.hpp @@ -121,15 +121,15 @@ class SwerveController : public controller_interface::ControllerInterface std::vector> axle_handles_; // Joint names for wheels and axles - std::string front_left_wheel_joint_name_; - std::string front_right_wheel_joint_name_; - std::string rear_left_wheel_joint_name_; - std::string rear_right_wheel_joint_name_; + std::string left_front_wheel_joint_name_; + std::string right_front_wheel_joint_name_; + std::string left_rear_wheel_joint_name_; + std::string right_rear_wheel_joint_name_; - std::string front_left_axle_joint_name_; - std::string front_right_axle_joint_name_; - std::string rear_left_axle_joint_name_; - std::string rear_right_axle_joint_name_; + std::string left_front_axle_joint_name_; + std::string right_front_axle_joint_name_; + std::string left_rear_axle_joint_name_; + std::string right_rear_axle_joint_name_; std::array wheel_joint_names{}; std::array axle_joint_names{}; @@ -142,10 +142,10 @@ class SwerveController : public controller_interface::ControllerInterface bool open_loop_ = false; bool use_stamped_vel_ = false; - double front_left_velocity_threshold_; - double front_right_velocity_threshold_; - double rear_left_velocity_threshold_; - double rear_right_velocity_threshold_; + double left_front_velocity_threshold_; + double right_front_velocity_threshold_; + double left_rear_velocity_threshold_; + double right_rear_velocity_threshold_; SwerveDriveKinematics swerveDriveKinematics_; diff --git a/swerve_drive_controller/src/swerve_drive_controller.cpp b/swerve_drive_controller/src/swerve_drive_controller.cpp index bb73e3fc77..cadc6c3d09 100644 --- a/swerve_drive_controller/src/swerve_drive_controller.cpp +++ b/swerve_drive_controller/src/swerve_drive_controller.cpp @@ -97,14 +97,14 @@ CallbackReturn SwerveController::on_init() try { // Declare parameters - auto_declare("joint_steering_left_front", front_left_axle_joint_name_); - auto_declare("joint_steering_right_front", front_right_axle_joint_name_); - auto_declare("joint_steering_left_rear", rear_left_axle_joint_name_); - auto_declare("joint_steering_right_rear", rear_right_axle_joint_name_); - auto_declare("joint_wheel_left_front", front_left_wheel_joint_name_); - auto_declare("joint_wheel_right_front", front_right_wheel_joint_name_); - auto_declare("joint_wheel_left_rear", rear_left_wheel_joint_name_); - auto_declare("joint_wheel_right_rear", rear_right_wheel_joint_name_); + auto_declare("joint_steering_left_front", left_front_axle_joint_name_); + auto_declare("joint_steering_right_front", right_front_axle_joint_name_); + auto_declare("joint_steering_left_rear", left_rear_axle_joint_name_); + auto_declare("joint_steering_right_rear", right_rear_axle_joint_name_); + auto_declare("joint_wheel_left_front", left_front_wheel_joint_name_); + auto_declare("joint_wheel_right_front", right_front_wheel_joint_name_); + auto_declare("joint_wheel_left_rear", left_rear_wheel_joint_name_); + auto_declare("joint_wheel_right_rear", right_rear_wheel_joint_name_); auto_declare("chassis_length", wheel_params_.x_offset); auto_declare("chassis_width", wheel_params_.y_offset); auto_declare("wheel_radius", wheel_params_.radius); @@ -118,10 +118,10 @@ CallbackReturn SwerveController::on_init() auto_declare("publish_rate", publish_rate_); auto_declare("enable_odom_tf", enable_odom_tf_); auto_declare("open_loop", open_loop_); - auto_declare("front_left_velocity_threshold", front_left_velocity_threshold_); - auto_declare("front_right_velocity_threshold", front_right_velocity_threshold_); - auto_declare("rear_left_velocity_threshold", rear_left_velocity_threshold_); - auto_declare("rear_right_velocity_threshold", rear_right_velocity_threshold_); + auto_declare("front_left_velocity_threshold", left_front_velocity_threshold_); + auto_declare("front_right_velocity_threshold", right_front_velocity_threshold_); + auto_declare("rear_left_velocity_threshold", left_rear_velocity_threshold_); + auto_declare("rear_right_velocity_threshold", right_rear_velocity_threshold_); } catch (const std::exception & e) { @@ -134,15 +134,15 @@ InterfaceConfiguration SwerveController::command_interface_configuration() const { std::vector conf_names; - conf_names.push_back(front_left_wheel_joint_name_ + "/" + HW_IF_VELOCITY); - conf_names.push_back(front_right_wheel_joint_name_ + "/" + HW_IF_VELOCITY); - conf_names.push_back(rear_left_wheel_joint_name_ + "/" + HW_IF_VELOCITY); - conf_names.push_back(rear_right_wheel_joint_name_ + "/" + HW_IF_VELOCITY); + conf_names.push_back(left_front_wheel_joint_name_ + "/" + HW_IF_VELOCITY); + conf_names.push_back(right_front_wheel_joint_name_ + "/" + HW_IF_VELOCITY); + conf_names.push_back(left_rear_wheel_joint_name_ + "/" + HW_IF_VELOCITY); + conf_names.push_back(right_rear_wheel_joint_name_ + "/" + HW_IF_VELOCITY); - conf_names.push_back(front_left_axle_joint_name_ + "/" + HW_IF_POSITION); - conf_names.push_back(front_right_axle_joint_name_ + "/" + HW_IF_POSITION); - conf_names.push_back(rear_left_axle_joint_name_ + "/" + HW_IF_POSITION); - conf_names.push_back(rear_right_axle_joint_name_ + "/" + HW_IF_POSITION); + conf_names.push_back(left_front_axle_joint_name_ + "/" + HW_IF_POSITION); + conf_names.push_back(right_front_axle_joint_name_ + "/" + HW_IF_POSITION); + conf_names.push_back(left_rear_axle_joint_name_ + "/" + HW_IF_POSITION); + conf_names.push_back(right_rear_axle_joint_name_ + "/" + HW_IF_POSITION); return {interface_configuration_type::INDIVIDUAL, conf_names}; } @@ -150,15 +150,15 @@ InterfaceConfiguration SwerveController::command_interface_configuration() const InterfaceConfiguration SwerveController::state_interface_configuration() const { std::vector conf_names; - conf_names.push_back(front_left_wheel_joint_name_ + "/" + HW_IF_VELOCITY); - conf_names.push_back(front_right_wheel_joint_name_ + "/" + HW_IF_VELOCITY); - conf_names.push_back(rear_left_wheel_joint_name_ + "/" + HW_IF_VELOCITY); - conf_names.push_back(rear_right_wheel_joint_name_ + "/" + HW_IF_VELOCITY); + conf_names.push_back(left_front_wheel_joint_name_ + "/" + HW_IF_VELOCITY); + conf_names.push_back(right_front_wheel_joint_name_ + "/" + HW_IF_VELOCITY); + conf_names.push_back(left_rear_wheel_joint_name_ + "/" + HW_IF_VELOCITY); + conf_names.push_back(right_rear_wheel_joint_name_ + "/" + HW_IF_VELOCITY); - conf_names.push_back(front_left_axle_joint_name_ + "/" + HW_IF_POSITION); - conf_names.push_back(front_right_axle_joint_name_ + "/" + HW_IF_POSITION); - conf_names.push_back(rear_left_axle_joint_name_ + "/" + HW_IF_POSITION); - conf_names.push_back(rear_right_axle_joint_name_ + "/" + HW_IF_POSITION); + conf_names.push_back(left_front_axle_joint_name_ + "/" + HW_IF_POSITION); + conf_names.push_back(right_front_axle_joint_name_ + "/" + HW_IF_POSITION); + conf_names.push_back(left_rear_axle_joint_name_ + "/" + HW_IF_POSITION); + conf_names.push_back(right_rear_axle_joint_name_ + "/" + HW_IF_POSITION); return {interface_configuration_type::INDIVIDUAL, conf_names}; } @@ -169,17 +169,17 @@ CallbackReturn SwerveController::on_configure(const rclcpp_lifecycle::State & /* auto logger = rclcpp::get_logger("SwerveController"); try { - front_left_wheel_joint_name_ = get_node()->get_parameter("joint_wheel_left_front").as_string(); - front_right_wheel_joint_name_ = + left_front_wheel_joint_name_ = get_node()->get_parameter("joint_wheel_left_front").as_string(); + right_front_wheel_joint_name_ = get_node()->get_parameter("joint_wheel_right_front").as_string(); - rear_left_wheel_joint_name_ = get_node()->get_parameter("joint_wheel_left_rear").as_string(); - rear_right_wheel_joint_name_ = get_node()->get_parameter("joint_wheel_right_rear").as_string(); - front_left_axle_joint_name_ = + left_rear_wheel_joint_name_ = get_node()->get_parameter("joint_wheel_left_rear").as_string(); + right_rear_wheel_joint_name_ = get_node()->get_parameter("joint_wheel_right_rear").as_string(); + left_front_axle_joint_name_ = get_node()->get_parameter("joint_steering_left_front").as_string(); - front_right_axle_joint_name_ = + right_front_axle_joint_name_ = get_node()->get_parameter("joint_steering_right_front").as_string(); - rear_left_axle_joint_name_ = get_node()->get_parameter("joint_steering_left_rear").as_string(); - rear_right_axle_joint_name_ = + left_rear_axle_joint_name_ = get_node()->get_parameter("joint_steering_left_rear").as_string(); + right_rear_axle_joint_name_ = get_node()->get_parameter("joint_steering_right_rear").as_string(); cmd_vel_topic_ = get_node()->get_parameter("cmd_vel_topic").as_string(); odometry_topic_ = get_node()->get_parameter("odom").as_string(); @@ -195,13 +195,13 @@ CallbackReturn SwerveController::on_configure(const rclcpp_lifecycle::State & /* wheel_params_.radius = get_node()->get_parameter("wheel_radius").as_double(); wheel_params_.center_of_rotation = get_node()->get_parameter("center_of_rotation").as_double(); - front_left_velocity_threshold_ = + left_front_velocity_threshold_ = get_node()->get_parameter("front_left_velocity_threshold").as_double(); - front_right_velocity_threshold_ = + right_front_velocity_threshold_ = get_node()->get_parameter("front_right_velocity_threshold").as_double(); - rear_left_velocity_threshold_ = + left_rear_velocity_threshold_ = get_node()->get_parameter("rear_left_velocity_threshold").as_double(); - rear_right_velocity_threshold_ = + right_rear_velocity_threshold_ = get_node()->get_parameter("rear_right_velocity_threshold").as_double(); for (std::size_t i = 0; i < 6; ++i) @@ -214,57 +214,57 @@ CallbackReturn SwerveController::on_configure(const rclcpp_lifecycle::State & /* twist_covariance_diagonal_array_[i] = 0.01; } - if (front_left_wheel_joint_name_.empty()) + if (left_front_wheel_joint_name_.empty()) { - RCLCPP_ERROR(logger, "front_wheel_joint_name is not set"); + RCLCPP_ERROR(logger, "left_front_wheel_joint_name is not set"); return CallbackReturn::ERROR; } - if (front_right_wheel_joint_name_.empty()) + if (right_front_wheel_joint_name_.empty()) { - RCLCPP_ERROR(logger, "front_right_wheel_joint_name is not set"); + RCLCPP_ERROR(logger, "right_front_wheel_joint_name is not set"); return CallbackReturn::ERROR; } - if (rear_left_wheel_joint_name_.empty()) + if (left_rear_wheel_joint_name_.empty()) { - RCLCPP_ERROR(logger, "rear_left_wheel_joint_name is not set"); + RCLCPP_ERROR(logger, "left_rear_wheel_joint_name is not set"); return CallbackReturn::ERROR; } - if (rear_right_wheel_joint_name_.empty()) + if (right_rear_wheel_joint_name_.empty()) { - RCLCPP_ERROR(logger, "rear_right_wheel_joint_name is not set"); + RCLCPP_ERROR(logger, "right_rear_wheel_joint_name is not set"); return CallbackReturn::ERROR; } - if (front_left_axle_joint_name_.empty()) + if (left_front_axle_joint_name_.empty()) { - RCLCPP_ERROR(logger, "front_axle_joint_name is not set"); + RCLCPP_ERROR(logger, "left_front_axle_joint_name is not set"); return CallbackReturn::ERROR; } - if (front_right_axle_joint_name_.empty()) + if (right_front_axle_joint_name_.empty()) { - RCLCPP_ERROR(logger, "front_right_axle_joint_name is not set"); + RCLCPP_ERROR(logger, "right_front_axle_joint_name is not set"); return CallbackReturn::ERROR; } - if (rear_left_axle_joint_name_.empty()) + if (left_rear_axle_joint_name_.empty()) { - RCLCPP_ERROR(logger, "rear_left_axle_joint_name_ is not set"); + RCLCPP_ERROR(logger, "left_rear_axle_joint_name_ is not set"); return CallbackReturn::ERROR; } - if (rear_right_axle_joint_name_.empty()) + if (right_rear_axle_joint_name_.empty()) { - RCLCPP_ERROR(logger, "rear_right_axle_joint_name_ is not set"); + RCLCPP_ERROR(logger, "right_rear_axle_joint_name_ is not set"); return CallbackReturn::ERROR; } - wheel_joint_names[0] = front_left_wheel_joint_name_; - wheel_joint_names[1] = front_right_wheel_joint_name_; - wheel_joint_names[2] = rear_left_wheel_joint_name_; - wheel_joint_names[3] = rear_right_wheel_joint_name_; + wheel_joint_names[0] = left_front_wheel_joint_name_; + wheel_joint_names[1] = right_front_wheel_joint_name_; + wheel_joint_names[2] = left_rear_wheel_joint_name_; + wheel_joint_names[3] = right_rear_wheel_joint_name_; - axle_joint_names[0] = front_left_axle_joint_name_; - axle_joint_names[1] = front_right_axle_joint_name_; - axle_joint_names[2] = rear_left_axle_joint_name_; - axle_joint_names[3] = rear_right_axle_joint_name_; + axle_joint_names[0] = left_front_axle_joint_name_; + axle_joint_names[1] = right_front_axle_joint_name_; + axle_joint_names[2] = left_rear_axle_joint_name_; + axle_joint_names[3] = right_rear_axle_joint_name_; cmd_vel_timeout_ = std::chrono::milliseconds( static_cast(get_node()->get_parameter("cmd_vel_timeout").as_double() * 1000.0)); @@ -477,10 +477,10 @@ controller_interface::return_type SwerveController::update( swerveDriveKinematics_.compute_wheel_commands(linear_x_cmd, linear_y_cmd, angular_cmd); std::vector> wheel_data = { - {wheel_command[0], front_left_velocity_threshold_, "front_left_wheel"}, - {wheel_command[1], front_right_velocity_threshold_, "front_right_wheel"}, - {wheel_command[2], rear_left_velocity_threshold_, "rear_left_wheel"}, - {wheel_command[3], rear_right_velocity_threshold_, "rear_right_wheel"}}; + {wheel_command[0], left_front_velocity_threshold_, "left_front_wheel"}, + {wheel_command[1], right_front_velocity_threshold_, "right_front_wheel"}, + {wheel_command[2], left_front_velocity_threshold_, "left_rear_wheel"}, + {wheel_command[3], right_front_velocity_threshold_, "right_rear_wheel"}}; for (const auto & [wheel_command_, threshold, label] : wheel_data) { From ce247f1dd0219a8ed941daa454f93b3d83e20f41 Mon Sep 17 00:00:00 2001 From: nitin Date: Wed, 23 Jul 2025 02:42:49 +0530 Subject: [PATCH 12/19] Modified CMakeLists.txt Signed-off-by: nitin --- doc/controllers_index.rst | 1 + swerve_drive_controller/CMakeLists.txt | 65 ++++++++++++------- swerve_drive_controller/package.xml | 1 + .../src/swerve_drive_controller.cpp | 2 +- 4 files changed, 44 insertions(+), 25 deletions(-) diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index 235800711b..2b77b9b55f 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -31,6 +31,7 @@ Controllers for Wheeled Mobile Robots Mecanum Drive Controllers <../mecanum_drive_controller/doc/userdoc.rst> Steering Controllers Library <../steering_controllers_library/doc/userdoc.rst> Tricycle Controller <../tricycle_controller/doc/userdoc.rst> + Swerve Drive Controller <../swerve_drive_controller/doc/userdoc.rst> Controllers for Manipulators and Other Robots ********************************************* diff --git a/swerve_drive_controller/CMakeLists.txt b/swerve_drive_controller/CMakeLists.txt index c47e5c16e1..421a419fb6 100644 --- a/swerve_drive_controller/CMakeLists.txt +++ b/swerve_drive_controller/CMakeLists.txt @@ -1,7 +1,9 @@ -cmake_minimum_required(VERSION 3.8) +cmake_minimum_required(VERSION 3.16) project(swerve_drive_controller) +find_package(ament_cmake REQUIRED) find_package(ros2_control_cmake REQUIRED) + set_compiler_options() export_windows_symbols() @@ -18,13 +20,14 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS realtime_tools tf2 tf2_msgs + tf2_geometry_msgs ) -find_package(ament_cmake REQUIRED) find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() + add_compile_definitions(RCPPUTILS_VERSION_MAJOR=${rcpputils_VERSION_MAJOR}) add_compile_definitions(RCPPUTILS_VERSION_MINOR=${rcpputils_VERSION_MINOR}) @@ -39,18 +42,30 @@ add_library(swerve_drive_controller SHARED target_compile_features(swerve_drive_controller PUBLIC cxx_std_17) target_include_directories(swerve_drive_controller PUBLIC - $ - $ + "$" + "$" +) + +target_link_libraries(swerve_drive_controller PUBLIC + swerve_drive_controller_parameters + controller_interface::controller_interface + hardware_interface::hardware_interface + pluginlib::pluginlib + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + rcpputils::rcpputils + realtime_tools::realtime_tools + tf2::tf2 + tf2_geometry_msgs::tf2_geometry_msgs + ${tf2_msgs_TARGETS} + ${geometry_msgs_TARGETS} + ${control_msgs_TARGETS} + ${nav_msgs_TARGETS} ) -target_link_libraries(swerve_drive_controller - PUBLIC - swerve_drive_controller_parameters) -ament_target_dependencies(swerve_drive_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) pluginlib_export_plugin_description_file(controller_interface swerve_drive_plugin.xml) if(BUILD_TESTING) - find_package(controller_manager REQUIRED) find_package(ament_cmake_gmock REQUIRED) find_package(ament_cmake_gtest REQUIRED) @@ -62,24 +77,26 @@ if(BUILD_TESTING) target_link_libraries(test_swerve_drive_controller swerve_drive_controller - ) - ament_target_dependencies(test_swerve_drive_controller - geometry_msgs - controller_interface - hardware_interface - nav_msgs - rclcpp - rclcpp_lifecycle - realtime_tools - tf2 - tf2_msgs + controller_interface::controller_interface + hardware_interface::hardware_interface + pluginlib::pluginlib + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + rcpputils::rcpputils + realtime_tools::realtime_tools + tf2::tf2 + ${tf2_msgs_TARGETS} + ${geometry_msgs_TARGETS} + ${control_msgs_TARGETS} + ${nav_msgs_TARGETS} ) add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") + ament_add_gmock(test_load_swerve_drive_controller test/test_load_swerve_drive_controller.cpp) - ament_target_dependencies(test_load_swerve_drive_controller - controller_manager - ros2_control_test_assets + target_link_libraries(test_load_swerve_drive_controller + controller_manager::controller_manager + ros2_control_test_assets::ros2_control_test_assets ) endif() @@ -95,7 +112,7 @@ install(TARGETS swerve_drive_controller swerve_drive_controller_parameters LIBRARY DESTINATION lib ) - ament_export_libraries(swerve_drive_controller) ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_package() + diff --git a/swerve_drive_controller/package.xml b/swerve_drive_controller/package.xml index e64969948b..935f8f39a9 100644 --- a/swerve_drive_controller/package.xml +++ b/swerve_drive_controller/package.xml @@ -25,6 +25,7 @@ realtime_tools tf2 tf2_msgs + tf2_geometry_msgs controller_manager ament_cmake_gtest diff --git a/swerve_drive_controller/src/swerve_drive_controller.cpp b/swerve_drive_controller/src/swerve_drive_controller.cpp index cadc6c3d09..cb7f1b04b6 100644 --- a/swerve_drive_controller/src/swerve_drive_controller.cpp +++ b/swerve_drive_controller/src/swerve_drive_controller.cpp @@ -24,7 +24,7 @@ #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/logging.hpp" -#include "tf2/LinearMath/Quaternion.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" namespace { From aa2899d2622c0f53802034a8ced33fc50866129f Mon Sep 17 00:00:00 2001 From: nitin Date: Sun, 27 Jul 2025 20:04:39 +0530 Subject: [PATCH 13/19] Mentioned swerve_driver_controller in release_notes.rst Signed-off-by: nitin --- doc/release_notes.rst | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 7cb35a7d4e..261d246508 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -12,7 +12,6 @@ joint_trajectory_controller ******************************* * The controller now supports the new anti-windup strategy of the PID class, which allows for more flexible control of the anti-windup behavior. (`#1759 `__). -======= * Scaling support was added in `#1191 `__. With this the controller "stretches the time" with which it progresses in the trajectory. Scaling can either be set @@ -29,3 +28,7 @@ pid_controller * The legacy ``antiwindup`` boolean and integral clamp parameters ``i_clamp_max``/``i_clamp_min`` have been deprecated in favor of the new ``antiwindup_strategy`` parameter. A ``tracking_time_constant`` parameter has also been introduced to configure the back-calculation strategy. * A new ``error_deadband`` parameter stops integration when the error is within a specified range. * PID state publisher can be turned off or on by using ``activate_state_publisher`` parameter. (`#1823 `_). + +swerve_drive_controller +********************************* +* The swerve_drive_controller was added (`#1694 `_). From 67adf94dde2c6f37d358ee6aaeb281fa4f6b5c99 Mon Sep 17 00:00:00 2001 From: nitin Date: Mon, 28 Jul 2025 01:45:44 +0530 Subject: [PATCH 14/19] Made use of generate parameters library Signed-off-by: nitin --- .../swerve_drive_controller.hpp | 48 +--- swerve_drive_controller/package.xml | 3 +- .../src/swerve_drive_controller.cpp | 198 +++++----------- .../swerve_drive_controller_parameter.yaml | 223 ++++++++++++------ .../config/test_swerve_drive_controller.yaml | 24 +- 5 files changed, 233 insertions(+), 263 deletions(-) diff --git a/swerve_drive_controller/include/swerve_drive_controller/swerve_drive_controller.hpp b/swerve_drive_controller/include/swerve_drive_controller/swerve_drive_controller.hpp index f599851ed1..ac4cbceaa3 100644 --- a/swerve_drive_controller/include/swerve_drive_controller/swerve_drive_controller.hpp +++ b/swerve_drive_controller/include/swerve_drive_controller/swerve_drive_controller.hpp @@ -36,6 +36,9 @@ #include "realtime_tools/realtime_publisher.hpp" #include "tf2_msgs/msg/tf_message.hpp" +// auto-generated by generate_parameter_library +#include "swerve_drive_controller/swerve_drive_controller_parameters.hpp" + namespace swerve_drive_controller { @@ -119,44 +122,11 @@ class SwerveController : public controller_interface::ControllerInterface // Handles for four wheels and their axles std::vector> wheel_handles_; std::vector> axle_handles_; - - // Joint names for wheels and axles - std::string left_front_wheel_joint_name_; - std::string right_front_wheel_joint_name_; - std::string left_rear_wheel_joint_name_; - std::string right_rear_wheel_joint_name_; - - std::string left_front_axle_joint_name_; - std::string right_front_axle_joint_name_; - std::string left_rear_axle_joint_name_; - std::string right_rear_axle_joint_name_; - std::array wheel_joint_names{}; std::array axle_joint_names{}; - std::string cmd_vel_topic_; - std::string odometry_topic_; - std::string base_footprint_; - - bool enable_odom_tf_ = true; - bool open_loop_ = false; - bool use_stamped_vel_ = false; - - double left_front_velocity_threshold_; - double right_front_velocity_threshold_; - double left_rear_velocity_threshold_; - double right_rear_velocity_threshold_; - - SwerveDriveKinematics swerveDriveKinematics_; - - std::queue previous_commands_; // last two commands - - double pose_covariance_diagonal_array_[6]; - double twist_covariance_diagonal_array_[6]; - - double publish_rate_ = 50.0; - rclcpp::Duration publish_period_ = rclcpp::Duration::from_nanoseconds(0); - rclcpp::Time previous_publish_timestamp_{0, 0, RCL_CLOCK_UNINITIALIZED}; + std::shared_ptr param_listener_; + Params params_; struct WheelParams { @@ -166,6 +136,11 @@ class SwerveController : public controller_interface::ControllerInterface double center_of_rotation = 0.0; } wheel_params_; + SwerveDriveKinematics swerveDriveKinematics_; + std::queue previous_commands_; // last two commands + rclcpp::Duration publish_period_ = rclcpp::Duration::from_nanoseconds(0); + rclcpp::Time previous_publish_timestamp_{0, 0, RCL_CLOCK_UNINITIALIZED}; + // Timeout to consider cmd_vel commands old std::chrono::milliseconds cmd_vel_timeout_{500}; rclcpp::Time previous_update_timestamp_{0}; @@ -174,9 +149,7 @@ class SwerveController : public controller_interface::ControllerInterface bool subscriber_is_active_ = false; rclcpp::Subscription::SharedPtr velocity_command_subscriber_ = nullptr; rclcpp::Subscription::SharedPtr velocity_command_unstamped_subscriber_ = nullptr; - realtime_tools::RealtimeBuffer> received_velocity_msg_ptr_{nullptr}; - std::shared_ptr> odometry_publisher_ = nullptr; std::shared_ptr> realtime_odometry_publisher_ = nullptr; @@ -186,7 +159,6 @@ class SwerveController : public controller_interface::ControllerInterface realtime_odometry_transform_publisher_ = nullptr; bool is_halted_ = false; - bool reset(); void halt(); }; diff --git a/swerve_drive_controller/package.xml b/swerve_drive_controller/package.xml index 935f8f39a9..e827cea7e2 100644 --- a/swerve_drive_controller/package.xml +++ b/swerve_drive_controller/package.xml @@ -2,9 +2,10 @@ swerve_drive_controller - 5.0.0 + 0.0.0 Controller for a 4 wheel swerve drive robot base. Nitin Maurya + Christoph Fröhlich Apache-2.0 ament_cmake diff --git a/swerve_drive_controller/src/swerve_drive_controller.cpp b/swerve_drive_controller/src/swerve_drive_controller.cpp index cb7f1b04b6..a0c675c7a0 100644 --- a/swerve_drive_controller/src/swerve_drive_controller.cpp +++ b/swerve_drive_controller/src/swerve_drive_controller.cpp @@ -96,36 +96,14 @@ CallbackReturn SwerveController::on_init() } try { - // Declare parameters - auto_declare("joint_steering_left_front", left_front_axle_joint_name_); - auto_declare("joint_steering_right_front", right_front_axle_joint_name_); - auto_declare("joint_steering_left_rear", left_rear_axle_joint_name_); - auto_declare("joint_steering_right_rear", right_rear_axle_joint_name_); - auto_declare("joint_wheel_left_front", left_front_wheel_joint_name_); - auto_declare("joint_wheel_right_front", right_front_wheel_joint_name_); - auto_declare("joint_wheel_left_rear", left_rear_wheel_joint_name_); - auto_declare("joint_wheel_right_rear", right_rear_wheel_joint_name_); - auto_declare("chassis_length", wheel_params_.x_offset); - auto_declare("chassis_width", wheel_params_.y_offset); - auto_declare("wheel_radius", wheel_params_.radius); - auto_declare("center_of_rotation", wheel_params_.center_of_rotation); - // auto_declare("cmd_vel_timeout", cmd_vel_timeout_.count() / 1000.0); - auto_declare("cmd_vel_timeout", static_cast(cmd_vel_timeout_.count()) / 1000.0); - auto_declare("use_stamped_vel", use_stamped_vel_); - auto_declare("cmd_vel_topic", cmd_vel_topic_); - auto_declare("odom", odometry_topic_); - auto_declare("base_footprint", base_footprint_); - auto_declare("publish_rate", publish_rate_); - auto_declare("enable_odom_tf", enable_odom_tf_); - auto_declare("open_loop", open_loop_); - auto_declare("front_left_velocity_threshold", left_front_velocity_threshold_); - auto_declare("front_right_velocity_threshold", right_front_velocity_threshold_); - auto_declare("rear_left_velocity_threshold", left_rear_velocity_threshold_); - auto_declare("rear_right_velocity_threshold", right_rear_velocity_threshold_); + // Create the parameter listener and get the parameters + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); } catch (const std::exception & e) { - return CallbackReturn::ERROR; + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return controller_interface::CallbackReturn::ERROR; } return CallbackReturn::SUCCESS; } @@ -133,142 +111,100 @@ CallbackReturn SwerveController::on_init() InterfaceConfiguration SwerveController::command_interface_configuration() const { std::vector conf_names; - - conf_names.push_back(left_front_wheel_joint_name_ + "/" + HW_IF_VELOCITY); - conf_names.push_back(right_front_wheel_joint_name_ + "/" + HW_IF_VELOCITY); - conf_names.push_back(left_rear_wheel_joint_name_ + "/" + HW_IF_VELOCITY); - conf_names.push_back(right_rear_wheel_joint_name_ + "/" + HW_IF_VELOCITY); - - conf_names.push_back(left_front_axle_joint_name_ + "/" + HW_IF_POSITION); - conf_names.push_back(right_front_axle_joint_name_ + "/" + HW_IF_POSITION); - conf_names.push_back(left_rear_axle_joint_name_ + "/" + HW_IF_POSITION); - conf_names.push_back(right_rear_axle_joint_name_ + "/" + HW_IF_POSITION); - + conf_names.push_back(params_.front_left_wheel_joint + "/" + HW_IF_VELOCITY); + conf_names.push_back(params_.front_right_wheel_joint + "/" + HW_IF_VELOCITY); + conf_names.push_back(params_.rear_left_wheel_joint + "/" + HW_IF_VELOCITY); + conf_names.push_back(params_.rear_right_wheel_joint + "/" + HW_IF_VELOCITY); + conf_names.push_back(params_.front_left_axle_joint + "/" + HW_IF_POSITION); + conf_names.push_back(params_.front_right_axle_joint + "/" + HW_IF_POSITION); + conf_names.push_back(params_.rear_left_axle_joint + "/" + HW_IF_POSITION); + conf_names.push_back(params_.rear_right_axle_joint + "/" + HW_IF_POSITION); return {interface_configuration_type::INDIVIDUAL, conf_names}; } InterfaceConfiguration SwerveController::state_interface_configuration() const { std::vector conf_names; - conf_names.push_back(left_front_wheel_joint_name_ + "/" + HW_IF_VELOCITY); - conf_names.push_back(right_front_wheel_joint_name_ + "/" + HW_IF_VELOCITY); - conf_names.push_back(left_rear_wheel_joint_name_ + "/" + HW_IF_VELOCITY); - conf_names.push_back(right_rear_wheel_joint_name_ + "/" + HW_IF_VELOCITY); - - conf_names.push_back(left_front_axle_joint_name_ + "/" + HW_IF_POSITION); - conf_names.push_back(right_front_axle_joint_name_ + "/" + HW_IF_POSITION); - conf_names.push_back(left_rear_axle_joint_name_ + "/" + HW_IF_POSITION); - conf_names.push_back(right_rear_axle_joint_name_ + "/" + HW_IF_POSITION); - + conf_names.push_back(params_.front_left_wheel_joint + "/" + HW_IF_VELOCITY); + conf_names.push_back(params_.front_right_wheel_joint + "/" + HW_IF_VELOCITY); + conf_names.push_back(params_.rear_left_wheel_joint + "/" + HW_IF_VELOCITY); + conf_names.push_back(params_.rear_right_wheel_joint + "/" + HW_IF_VELOCITY); + conf_names.push_back(params_.front_left_axle_joint + "/" + HW_IF_POSITION); + conf_names.push_back(params_.front_right_axle_joint + "/" + HW_IF_POSITION); + conf_names.push_back(params_.rear_left_axle_joint + "/" + HW_IF_POSITION); + conf_names.push_back(params_.rear_right_axle_joint + "/" + HW_IF_POSITION); return {interface_configuration_type::INDIVIDUAL, conf_names}; } CallbackReturn SwerveController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) { - // auto logger = get_node()->get_logger(); auto logger = rclcpp::get_logger("SwerveController"); try { - left_front_wheel_joint_name_ = get_node()->get_parameter("joint_wheel_left_front").as_string(); - right_front_wheel_joint_name_ = - get_node()->get_parameter("joint_wheel_right_front").as_string(); - left_rear_wheel_joint_name_ = get_node()->get_parameter("joint_wheel_left_rear").as_string(); - right_rear_wheel_joint_name_ = get_node()->get_parameter("joint_wheel_right_rear").as_string(); - left_front_axle_joint_name_ = - get_node()->get_parameter("joint_steering_left_front").as_string(); - right_front_axle_joint_name_ = - get_node()->get_parameter("joint_steering_right_front").as_string(); - left_rear_axle_joint_name_ = get_node()->get_parameter("joint_steering_left_rear").as_string(); - right_rear_axle_joint_name_ = - get_node()->get_parameter("joint_steering_right_rear").as_string(); - cmd_vel_topic_ = get_node()->get_parameter("cmd_vel_topic").as_string(); - odometry_topic_ = get_node()->get_parameter("odom").as_string(); - base_footprint_ = get_node()->get_parameter("base_footprint").as_string(); - publish_rate_ = get_node()->get_parameter("publish_rate").as_double(); - enable_odom_tf_ = get_node()->get_parameter("enable_odom_tf").as_bool(); - open_loop_ = get_node()->get_parameter("open_loop").as_bool(); - - use_stamped_vel_ = get_node()->get_parameter("use_stamped_vel").as_bool(); - - wheel_params_.x_offset = get_node()->get_parameter("chassis_length").as_double(); - wheel_params_.y_offset = get_node()->get_parameter("chassis_width").as_double(); - wheel_params_.radius = get_node()->get_parameter("wheel_radius").as_double(); - wheel_params_.center_of_rotation = get_node()->get_parameter("center_of_rotation").as_double(); - - left_front_velocity_threshold_ = - get_node()->get_parameter("front_left_velocity_threshold").as_double(); - right_front_velocity_threshold_ = - get_node()->get_parameter("front_right_velocity_threshold").as_double(); - left_rear_velocity_threshold_ = - get_node()->get_parameter("rear_left_velocity_threshold").as_double(); - right_rear_velocity_threshold_ = - get_node()->get_parameter("rear_right_velocity_threshold").as_double(); - + wheel_params_.x_offset = params_.chassis_length; + wheel_params_.y_offset = params_.chassis_width; + wheel_params_.radius = params_.wheel_radius; + wheel_params_.center_of_rotation = params_.center_of_rotation; for (std::size_t i = 0; i < 6; ++i) { - pose_covariance_diagonal_array_[i] = 0.01; + params_.pose_covariance_diagonal[i] = 0.01; } for (std::size_t i = 0; i < 6; ++i) { - twist_covariance_diagonal_array_[i] = 0.01; + params_.twist_covariance_diagonal[i] = 0.01; } - if (left_front_wheel_joint_name_.empty()) + if (params_.front_left_wheel_joint.empty()) { - RCLCPP_ERROR(logger, "left_front_wheel_joint_name is not set"); + RCLCPP_ERROR(logger, "front_left_wheel_joint_name is not set"); return CallbackReturn::ERROR; } - if (right_front_wheel_joint_name_.empty()) + if (params_.front_right_wheel_joint.empty()) { - RCLCPP_ERROR(logger, "right_front_wheel_joint_name is not set"); + RCLCPP_ERROR(logger, "front_right_wheel_joint_name is not set"); return CallbackReturn::ERROR; } - if (left_rear_wheel_joint_name_.empty()) + if (params_.rear_left_wheel_joint.empty()) { - RCLCPP_ERROR(logger, "left_rear_wheel_joint_name is not set"); + RCLCPP_ERROR(logger, "rear_left_wheel_joint_name is not set"); return CallbackReturn::ERROR; } - if (right_rear_wheel_joint_name_.empty()) + if (params_.rear_right_wheel_joint.empty()) { - RCLCPP_ERROR(logger, "right_rear_wheel_joint_name is not set"); + RCLCPP_ERROR(logger, "rear_right_wheel_joint_name is not set"); return CallbackReturn::ERROR; } - - if (left_front_axle_joint_name_.empty()) + if (params_.front_left_axle_joint.empty()) { - RCLCPP_ERROR(logger, "left_front_axle_joint_name is not set"); + RCLCPP_ERROR(logger, "front_left_axle_joint_name is not set"); return CallbackReturn::ERROR; } - if (right_front_axle_joint_name_.empty()) + if (params_.front_right_axle_joint.empty()) { - RCLCPP_ERROR(logger, "right_front_axle_joint_name is not set"); + RCLCPP_ERROR(logger, "front_right_axle_joint_name is not set"); return CallbackReturn::ERROR; } - if (left_rear_axle_joint_name_.empty()) + if (params_.rear_left_axle_joint.empty()) { - RCLCPP_ERROR(logger, "left_rear_axle_joint_name_ is not set"); + RCLCPP_ERROR(logger, "rear_left_axle_joint_name_ is not set"); return CallbackReturn::ERROR; } - if (right_rear_axle_joint_name_.empty()) + if (params_.rear_right_axle_joint.empty()) { - RCLCPP_ERROR(logger, "right_rear_axle_joint_name_ is not set"); + RCLCPP_ERROR(logger, "rear_right_axle_joint_name_ is not set"); return CallbackReturn::ERROR; } - - wheel_joint_names[0] = left_front_wheel_joint_name_; - wheel_joint_names[1] = right_front_wheel_joint_name_; - wheel_joint_names[2] = left_rear_wheel_joint_name_; - wheel_joint_names[3] = right_rear_wheel_joint_name_; - - axle_joint_names[0] = left_front_axle_joint_name_; - axle_joint_names[1] = right_front_axle_joint_name_; - axle_joint_names[2] = left_rear_axle_joint_name_; - axle_joint_names[3] = right_rear_axle_joint_name_; - + wheel_joint_names[0] = params_.front_left_wheel_joint; + wheel_joint_names[1] = params_.front_right_wheel_joint; + wheel_joint_names[2] = params_.rear_left_wheel_joint; + wheel_joint_names[3] = params_.rear_right_wheel_joint; + axle_joint_names[0] = params_.front_left_axle_joint; + axle_joint_names[1] = params_.front_right_axle_joint; + axle_joint_names[2] = params_.rear_left_axle_joint; + axle_joint_names[3] = params_.rear_right_axle_joint; cmd_vel_timeout_ = std::chrono::milliseconds( - static_cast(get_node()->get_parameter("cmd_vel_timeout").as_double() * 1000.0)); - + static_cast(params_.cmd_vel_timeout * 1000.0)); if (!reset()) { return CallbackReturn::ERROR; @@ -281,7 +217,7 @@ CallbackReturn SwerveController::on_configure(const rclcpp_lifecycle::State & /* empty_twist.twist.angular.z = 0.0; received_velocity_msg_ptr_.writeFromNonRT(std::make_shared(empty_twist)); - if (use_stamped_vel_) + if (params_.use_stamped_vel) { velocity_command_subscriber_ = get_node()->create_subscription( DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), @@ -340,14 +276,12 @@ CallbackReturn SwerveController::on_configure(const rclcpp_lifecycle::State & /* tf_prefix = tf_prefix + "/"; } - const auto odom_frame_id = tf_prefix + odometry_topic_; - const auto base_frame_id = tf_prefix + base_footprint_; - + const auto odom_frame_id = tf_prefix + params_.odom; + const auto base_frame_id = tf_prefix + params_.base_footprint; auto & odometry_message = realtime_odometry_publisher_->msg_; odometry_message.header.frame_id = odom_frame_id; odometry_message.child_frame_id = base_frame_id; - - publish_period_ = rclcpp::Duration::from_seconds(1.0 / publish_rate_); + publish_period_ = rclcpp::Duration::from_seconds(1.0 / params_.publishe_rate); odometry_message.twist = geometry_msgs::msg::TwistWithCovariance(rosidl_runtime_cpp::MessageInitialization::ALL); @@ -356,10 +290,9 @@ CallbackReturn SwerveController::on_configure(const rclcpp_lifecycle::State & /* for (std::size_t index = 0; index < 6; ++index) { const std::size_t diagonal_index = NUM_DIMENSIONS * index + index; - odometry_message.pose.covariance[diagonal_index] = pose_covariance_diagonal_array_[index]; - odometry_message.twist.covariance[diagonal_index] = twist_covariance_diagonal_array_[index]; + odometry_message.pose.covariance[diagonal_index] = params_.pose_covariance_diagonal[index]; + odometry_message.twist.covariance[diagonal_index] = params_.twist_covariance_diagonal[index]; } - odometry_transform_publisher_ = get_node()->create_publisher( DEFAULT_TRANSFORM_TOPIC, rclcpp::SystemDefaultsQoS()); @@ -452,7 +385,6 @@ controller_interface::return_type SwerveController::update( last_command_msg->twist.linear.x = 0.0; last_command_msg->twist.linear.y = 0.0; last_command_msg->twist.angular.z = 0.0; - received_velocity_msg_ptr_.writeFromNonRT(last_command_msg); } @@ -477,10 +409,10 @@ controller_interface::return_type SwerveController::update( swerveDriveKinematics_.compute_wheel_commands(linear_x_cmd, linear_y_cmd, angular_cmd); std::vector> wheel_data = { - {wheel_command[0], left_front_velocity_threshold_, "left_front_wheel"}, - {wheel_command[1], right_front_velocity_threshold_, "right_front_wheel"}, - {wheel_command[2], left_front_velocity_threshold_, "left_rear_wheel"}, - {wheel_command[3], right_front_velocity_threshold_, "right_rear_wheel"}}; + {wheel_command[0], params_.front_left_velocity_threshold, "front_left_wheel"}, + {wheel_command[1], params_.front_right_velocity_threshold, "front_right_wheel"}, + {wheel_command[2], params_.rear_left_velocity_threshold, "rear_left_wheel"}, + {wheel_command[3], params_.rear_right_velocity_threshold, "rear_right_wheel"}}; for (const auto & [wheel_command_, threshold, label] : wheel_data) { @@ -512,7 +444,7 @@ controller_interface::return_type SwerveController::update( for (std::size_t i = 0; i < 4; ++i) { - if (open_loop_) + if (params_.open_loop) { velocity_array[i] = wheel_command[i].drive_velocity; steering_angle_array[i] = wheel_command[i].steering_angle; @@ -551,7 +483,6 @@ controller_interface::return_type SwerveController::update( odometry_message.pose.pose.orientation.y = orientation.y(); odometry_message.pose.pose.orientation.z = orientation.z(); odometry_message.pose.pose.orientation.w = orientation.w(); - realtime_odometry_publisher_->unlockAndPublish(); } @@ -559,18 +490,15 @@ controller_interface::return_type SwerveController::update( { auto & transform = realtime_odometry_transform_publisher_->msg_.transforms.front(); transform.header.stamp = time; - transform.transform.translation.x = odometry_.x; transform.transform.translation.y = odometry_.y; transform.transform.translation.z = 0.0; - transform.transform.rotation.x = orientation.x(); transform.transform.rotation.y = orientation.y(); transform.transform.rotation.z = orientation.z(); transform.transform.rotation.w = orientation.w(); realtime_odometry_transform_publisher_->unlockAndPublish(); } - return controller_interface::return_type::OK; } diff --git a/swerve_drive_controller/src/swerve_drive_controller_parameter.yaml b/swerve_drive_controller/src/swerve_drive_controller_parameter.yaml index e0ff7259bc..e3ba89bb72 100644 --- a/swerve_drive_controller/src/swerve_drive_controller_parameter.yaml +++ b/swerve_drive_controller/src/swerve_drive_controller_parameter.yaml @@ -1,83 +1,154 @@ swerve_drive_controller: - ros__parameters: - front_wheel_joint: { - type: string, - default_value: "front_wheel_joint", - description: "Name of the front wheel joint." - } - rear_left_wheel_joint: { - type: string, - default_value: "rear_left_wheel_joint", - description: "Name of the rear left wheel joint." - } - rear_right_wheel_joint: { - type: string, - default_value: "rear_right_wheel_joint", - description: "Name of the rear right wheel joint." - } - front_axle_joint: { - type: string, - default_value: "front_axle_joint", - description: "Name of the front axle joint." + front_left_wheel_joint: { + type: string, + default_value: "", + description: "Name of the front left wheel joint." + } + front_right_wheel_joint: { + type: string, + default_value: "", + description: "Name of the rear left wheel joint." + } + rear_left_wheel_joint: { + type: string, + default_value: "", + description: "Name of the rear left wheel joint." + } + rear_right_wheel_joint: { + type: string, + default_value: "", + description: "Name of the rear right wheel joint." + } + front_left_axle_joint: { + type: string, + default_value: "", + description: "Name of the front left axle joint." + } + front_right_axle_joint: { + type: string, + default_value: "", + description: "Name of the front right axle joint." + } + rear_left_axle_joint: { + type: string, + default_value: "", + description: "Name of the rear left axle joint." + } + rear_right_axle_joint: { + type: string, + default_value: "", + description: "Name of the rear right axle joint." + } + chassis_length: { + type: double, + default_value: 0.85, + description: "Length of the robot chassis.", + validation: { + "swerve_drive_controller::gt<>": [0.0] } - rear_left_axle_joint: { - type: string, - default_value: "rear_left_axle_joint", - description: "Name of the rear left axle joint." - } - rear_right_axle_joint: { - type: string, - default_value: "rear_right_axle_joint", - description: "Name of the rear right axle joint." - } - - chassis_length: { - type: double, - default_value: 0.85, - description: "Length of the robot chassis.", - validation: { - "swerve_drive_controller::gt<>": [0.0] - } + } + chassis_width: { + type: double, + default_value: 0.75, + description: "Width of the robot chassis.", + validation: { + "swerve_drive_controller::gt<>": [0.0] } - chassis_width: { - type: double, - default_value: 0.75, - description: "Width of the robot chassis.", - validation: { - "swerve_drive_controller::gt<>": [0.0] - } + } + wheel_radius: { + type: double, + default_value: 0.0775, + description: "Radius of the wheels.", + validation: { + "swerve_drive_controller::gt<>": [0.0] } - wheel_radius: { - type: double, - default_value: 0.0775, - description: "Radius of the wheels.", - validation: { - "swerve_drive_controller::gt<>": [0.0] - } + } + center_of_rotation: { + type: double, + default_value: 0.1, + description: "Center of rotation of robot", + validation: { + "swerve_drive_controller::gt<>": [0.0] } - - cmd_vel_timeout: { - type: double, - default_value: 10.0, - description: "Timeout in seconds before velocity commands are considered stale.", - validation: { - "swerve_drive_controller::gt<>": [0.0] - } - } - use_stamped_vel: { - type: bool, - default_value: false, - description: "If true, velocity commands will be expected to have a timestamp." - } - cmd_vel_topic: { - type: string, - default_value: "/cmd_vel", - description: "Topic name for velocity commands." - } - - open_loop: { - type: bool, - default_value: false, - description: "If set to true the odometry of the robot will be calculated from the commanded values and not from feedback.", + } + cmd_vel_timeout: { + type: double, + default_value: 10.0, + description: "Timeout in seconds before velocity commands are considered stale.", + validation: { + "swerve_drive_controller::gt<>": [0.0] } + } + use_stamped_vel: { + type: bool, + default_value: false, + description: "If true, velocity commands will be expected to have a timestamp." + } + odom: { + type: string, + default_value: "odom", + description: "Name of the frame for odometry." + } + base_footprint: { + type: string, + default_value: "base_footprint", + description: "Name of the frame for base footprint." + } + publishe_rate: { + type: double, + default_value: 50.0, # Hz + description: "Publishing rate (Hz) of the odometry and TF messages." + } + enable_odom_tf: { + type: bool, + default_value: true, + description: "Publish transformation between ``odom_frame_id`` and ``base_frame_id``.", + } + open_loop: { + type: bool, + default_value: false, + description: "If set to true the odometry of the robot will be calculated from the commanded values and not from feedback.", + } + front_left_velocity_threshold: { + type: double, + default_value: 2.0, + description: "Max velocity for front left wheel", + validation: { + "swerve_drive_controller::gt<>": [0.0] + } + } + front_right_velocity_threshold: { + type: double, + default_value: 2.0, + description: "Max velocity for front right wheel", + validation: { + "swerve_drive_controller::gt<>": [0.0] + } + } + rear_left_velocity_threshold: { + type: double, + default_value: 2.0, + description: "Max velocity for rear left wheel", + validation: { + "swerve_drive_controller::gt<>": [0.0] + } + } + rear_right_velocity_threshold: { + type: double, + default_value: 2.0, + description: "Max velocity for rear right wheel", + validation: { + "swerve_drive_controller::gt<>": [0.0] + } + } + pose_covariance_diagonal: { + type: double_array, + default_value: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + description: "Odometry covariance for the encoder output of the robot for the pose. These values should be tuned to your robot's sample odometry data, but these values are a good place to start: ``[0.001, 0.001, 0.001, 0.001, 0.001, 0.01]``.", + } + twist_covariance_diagonal: { + type: double_array, + default_value: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + description: "Odometry covariance for the encoder output of the robot for the speed. These values should be tuned to your robot's sample odometry data, but these values are a good place to start: ``[0.001, 0.001, 0.001, 0.001, 0.001, 0.01]``.", + } diff --git a/swerve_drive_controller/test/config/test_swerve_drive_controller.yaml b/swerve_drive_controller/test/config/test_swerve_drive_controller.yaml index 48f4e2ccab..a5dab3ea11 100644 --- a/swerve_drive_controller/test/config/test_swerve_drive_controller.yaml +++ b/swerve_drive_controller/test/config/test_swerve_drive_controller.yaml @@ -1,6 +1,6 @@ test_swerve_drive_controller: ros__parameters: - update_rate: 10 + update_rate: 10 #Hz joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster @@ -8,31 +8,29 @@ test_swerve_drive_controller: swerve_drive_controller: type: swerve_drive_controller/SwerveController - swerve_drive_controller: ros__parameters: - joint_steering_left_front: "joint_steering_left_front" - joint_steering_right_front: "joint_steering_right_front" - joint_steering_left_rear: "joint_steering_left_rear" - joint_steering_right_rear: "joint_steering_right_rear" - joint_wheel_left_front: "joint_wheel_left_front" - joint_wheel_right_front: "joint_wheel_right_front" - joint_wheel_left_rear: "joint_wheel_left_rear" - joint_wheel_right_rear: "joint_wheel_right_rear" + front_left_wheel_joint: "joint_wheel_left_front" + front_right_wheel_joint: "joint_wheel_right_front" + rear_left_wheel_joint: "joint_wheel_left_rear" + rear_right_wheel_joint: "joint_wheel_right_rear" + + front_left_axle_joint: "joint_steering_left_front" + front_right_axle_joint: "joint_steering_right_front" + rear_left_axle_joint: "joint_steering_left_rear" + rear_right_axle_joint: "joint_steering_right_rear" chassis_length: 0.35 chassis_width: 0.2 wheel_radius: 0.04 center_of_rotation: 0.1 - cmd_vel_timeout: 10.0 use_stamped_vel: false - cmd_vel_topic: "/cmd_vel" odom: odom base_footprint: base_footprint - publishe_rate: 2 + publishe_rate: 50.0 enable_odom_tf: true open_loop: false From 3a0714ed513bde5780f13eef122874fe9e516fac Mon Sep 17 00:00:00 2001 From: nitin Date: Mon, 28 Jul 2025 01:51:45 +0530 Subject: [PATCH 15/19] Removed unwanted merge Signed-off-by: nitin --- doc/migration.rst | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/doc/migration.rst b/doc/migration.rst index a4f00c9472..7713a5b742 100644 --- a/doc/migration.rst +++ b/doc/migration.rst @@ -15,4 +15,6 @@ diff_drive_controller pid_controller ***************************** * Parameters ``enable_feedforward`` and service ``set_feedforward_control`` are removed. Instead, set the feedforward_gain to zero or a non-zero value. (`#1553 `_). -* The legacy ``antiwindup`` boolean and integral clamp parameters ``i_clamp_max``/``i_clamp_min`` have been deprecated in favor of the new ``antiwindup_strategy`` parameter (`#1585 `__). Choose a suitable anti-windup strategy and set the parameters accordingly. +* The legacy ``antiwindup`` boolean and integral clamp parameters ``i_clamp_max``/``i_clamp_min`` have + been deprecated in favor of the new ``antiwindup_strategy`` parameter (`#1585 `__). Choose a suitable anti-windup strategy and set the parameters accordingly. +* PID state publisher topic changed to ```` namespace and is initially turned off. It can be turned on by using ``activate_state_publisher`` parameter. (`#1823 `_). \ No newline at end of file From 1b2a6b1a7d1761871a2bbc73307847841e05e8d1 Mon Sep 17 00:00:00 2001 From: nitin Date: Mon, 28 Jul 2025 01:59:04 +0530 Subject: [PATCH 16/19] Made parameters read only Signed-off-by: nitin --- .../swerve_drive_controller_parameter.yaml | 67 +++++++++++++------ 1 file changed, 46 insertions(+), 21 deletions(-) diff --git a/swerve_drive_controller/src/swerve_drive_controller_parameter.yaml b/swerve_drive_controller/src/swerve_drive_controller_parameter.yaml index e3ba89bb72..b1dd539a59 100644 --- a/swerve_drive_controller/src/swerve_drive_controller_parameter.yaml +++ b/swerve_drive_controller/src/swerve_drive_controller_parameter.yaml @@ -3,42 +3,50 @@ swerve_drive_controller: front_left_wheel_joint: { type: string, default_value: "", - description: "Name of the front left wheel joint." + description: "Name of the front left wheel joint.", + read_only: true } front_right_wheel_joint: { type: string, default_value: "", - description: "Name of the rear left wheel joint." + description: "Name of the rear left wheel joint.", + read_only: true } rear_left_wheel_joint: { type: string, default_value: "", - description: "Name of the rear left wheel joint." + description: "Name of the rear left wheel joint.", + read_only: true } rear_right_wheel_joint: { type: string, default_value: "", - description: "Name of the rear right wheel joint." + description: "Name of the rear right wheel joint.", + read_only: true } front_left_axle_joint: { type: string, default_value: "", - description: "Name of the front left axle joint." + description: "Name of the front left axle joint.", + read_only: true } front_right_axle_joint: { type: string, default_value: "", - description: "Name of the front right axle joint." + description: "Name of the front right axle joint.", + read_only: true } rear_left_axle_joint: { type: string, default_value: "", - description: "Name of the rear left axle joint." + description: "Name of the rear left axle joint.", + read_only: true, } rear_right_axle_joint: { type: string, default_value: "", - description: "Name of the rear right axle joint." + description: "Name of the rear right axle joint.", + read_only: true } chassis_length: { type: double, @@ -46,7 +54,8 @@ swerve_drive_controller: description: "Length of the robot chassis.", validation: { "swerve_drive_controller::gt<>": [0.0] - } + }, + read_only: true } chassis_width: { type: double, @@ -54,7 +63,8 @@ swerve_drive_controller: description: "Width of the robot chassis.", validation: { "swerve_drive_controller::gt<>": [0.0] - } + }, + read_only: true } wheel_radius: { type: double, @@ -62,7 +72,8 @@ swerve_drive_controller: description: "Radius of the wheels.", validation: { "swerve_drive_controller::gt<>": [0.0] - } + }, + read_only: true } center_of_rotation: { type: double, @@ -70,7 +81,8 @@ swerve_drive_controller: description: "Center of rotation of robot", validation: { "swerve_drive_controller::gt<>": [0.0] - } + }, + read_only: true } cmd_vel_timeout: { type: double, @@ -78,37 +90,44 @@ swerve_drive_controller: description: "Timeout in seconds before velocity commands are considered stale.", validation: { "swerve_drive_controller::gt<>": [0.0] - } + }, + read_only: true } use_stamped_vel: { type: bool, default_value: false, - description: "If true, velocity commands will be expected to have a timestamp." + description: "If true, velocity commands will be expected to have a timestamp.", + read_only: true } odom: { type: string, default_value: "odom", - description: "Name of the frame for odometry." + description: "Name of the frame for odometry.", + read_only: true } base_footprint: { type: string, default_value: "base_footprint", - description: "Name of the frame for base footprint." + description: "Name of the frame for base footprint.", + read_only: true } publishe_rate: { type: double, default_value: 50.0, # Hz - description: "Publishing rate (Hz) of the odometry and TF messages." + description: "Publishing rate (Hz) of the odometry and TF messages.", + read_only: true } enable_odom_tf: { type: bool, default_value: true, description: "Publish transformation between ``odom_frame_id`` and ``base_frame_id``.", + read_only: true } open_loop: { type: bool, default_value: false, description: "If set to true the odometry of the robot will be calculated from the commanded values and not from feedback.", + read_only: true } front_left_velocity_threshold: { type: double, @@ -116,7 +135,8 @@ swerve_drive_controller: description: "Max velocity for front left wheel", validation: { "swerve_drive_controller::gt<>": [0.0] - } + }, + read_only: true } front_right_velocity_threshold: { type: double, @@ -124,7 +144,8 @@ swerve_drive_controller: description: "Max velocity for front right wheel", validation: { "swerve_drive_controller::gt<>": [0.0] - } + }, + read_only: true } rear_left_velocity_threshold: { type: double, @@ -132,7 +153,8 @@ swerve_drive_controller: description: "Max velocity for rear left wheel", validation: { "swerve_drive_controller::gt<>": [0.0] - } + }, + read_only: true } rear_right_velocity_threshold: { type: double, @@ -140,15 +162,18 @@ swerve_drive_controller: description: "Max velocity for rear right wheel", validation: { "swerve_drive_controller::gt<>": [0.0] - } + }, + read_only: true } pose_covariance_diagonal: { type: double_array, default_value: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], description: "Odometry covariance for the encoder output of the robot for the pose. These values should be tuned to your robot's sample odometry data, but these values are a good place to start: ``[0.001, 0.001, 0.001, 0.001, 0.001, 0.01]``.", + read_only: true } twist_covariance_diagonal: { type: double_array, default_value: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], description: "Odometry covariance for the encoder output of the robot for the speed. These values should be tuned to your robot's sample odometry data, but these values are a good place to start: ``[0.001, 0.001, 0.001, 0.001, 0.001, 0.01]``.", + read_only: true } From 88ed2baf6d4b23f45f42d5f8f103e080dabf2c64 Mon Sep 17 00:00:00 2001 From: Nitin Maurya <97148529+nitin2606@users.noreply.github.com> Date: Mon, 28 Jul 2025 15:07:12 +0530 Subject: [PATCH 17/19] Update doc/migration.rst Co-authored-by: Aarav Gupta --- doc/migration.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/migration.rst b/doc/migration.rst index 7713a5b742..72de6cabb0 100644 --- a/doc/migration.rst +++ b/doc/migration.rst @@ -17,4 +17,4 @@ pid_controller * Parameters ``enable_feedforward`` and service ``set_feedforward_control`` are removed. Instead, set the feedforward_gain to zero or a non-zero value. (`#1553 `_). * The legacy ``antiwindup`` boolean and integral clamp parameters ``i_clamp_max``/``i_clamp_min`` have been deprecated in favor of the new ``antiwindup_strategy`` parameter (`#1585 `__). Choose a suitable anti-windup strategy and set the parameters accordingly. -* PID state publisher topic changed to ```` namespace and is initially turned off. It can be turned on by using ``activate_state_publisher`` parameter. (`#1823 `_). \ No newline at end of file +* PID state publisher topic changed to ```` namespace and is initially turned off. It can be turned on by using ``activate_state_publisher`` parameter. (`#1823 `_). From e9d72bbeb261b44ade3d322c0ed0c11add7aaaec Mon Sep 17 00:00:00 2001 From: Nitin Maurya <97148529+nitin2606@users.noreply.github.com> Date: Mon, 28 Jul 2025 15:07:26 +0530 Subject: [PATCH 18/19] Update doc/release_notes.rst Co-authored-by: Aarav Gupta --- doc/release_notes.rst | 1 - 1 file changed, 1 deletion(-) diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 261d246508..d8488f7210 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -11,7 +11,6 @@ force_torque_sensor_broadcaster joint_trajectory_controller ******************************* * The controller now supports the new anti-windup strategy of the PID class, which allows for more flexible control of the anti-windup behavior. (`#1759 `__). - * Scaling support was added in `#1191 `__. With this the controller "stretches the time" with which it progresses in the trajectory. Scaling can either be set From d4b5d8e8ad958546cee7d5504159840f3109ae6a Mon Sep 17 00:00:00 2001 From: Nitin Maurya <97148529+nitin2606@users.noreply.github.com> Date: Mon, 28 Jul 2025 15:07:58 +0530 Subject: [PATCH 19/19] Update swerve_drive_controller/package.xml Co-authored-by: Aarav Gupta --- swerve_drive_controller/package.xml | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/swerve_drive_controller/package.xml b/swerve_drive_controller/package.xml index e827cea7e2..465bec6cdb 100644 --- a/swerve_drive_controller/package.xml +++ b/swerve_drive_controller/package.xml @@ -4,8 +4,13 @@ swerve_drive_controller 0.0.0 Controller for a 4 wheel swerve drive robot base. + Bence Magyar + Christoph Froehlich + Denis Štogl + Sai Kishor Kothakota Nitin Maurya - Christoph Fröhlich + + Nitin Maurya Apache-2.0 ament_cmake