diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index 4bbe6eb673..648cef04cf 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -32,6 +32,7 @@ Controllers for Wheeled Mobile Robots Omni Wheel Drive Controller <../omni_wheel_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/doc/release_notes.rst b/doc/release_notes.rst index 2f9ae8ebab..fa2029108a 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -37,6 +37,6 @@ pid_controller * 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 `_). -motion_primitives_forward_controller -******************************************* -* 🚀 The motion_primitives_forward_controller was added 🎉 (`#1636 `_). +swerve_drive_controller +********************************* +* The swerve_drive_controller was added (`#1694 `_). diff --git a/swerve_drive_controller/CMakeLists.txt b/swerve_drive_controller/CMakeLists.txt new file mode 100644 index 0000000000..198e673a31 --- /dev/null +++ b/swerve_drive_controller/CMakeLists.txt @@ -0,0 +1,118 @@ +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() + +set(THIS_PACKAGE_INCLUDE_DEPENDS + controller_interface + generate_parameter_library + geometry_msgs + hardware_interface + nav_msgs + pluginlib + rclcpp + rclcpp_lifecycle + rcpputils + realtime_tools + angles + tf2 + tf2_msgs + tf2_geometry_msgs +) + +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 +) + +add_library(swerve_drive_controller SHARED + src/swerve_drive_controller.cpp + src/swerve_drive_kinematics.cpp +) + +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 + angles::angles + tf2_geometry_msgs::tf2_geometry_msgs + ${tf2_msgs_TARGETS} + ${geometry_msgs_TARGETS} + ${control_msgs_TARGETS} + ${nav_msgs_TARGETS} +) + +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(ros2_control_test_assets REQUIRED) + + ament_add_gmock(test_swerve_drive_controller + test/test_swerve_drive_controller.cpp + ) + + target_link_libraries(test_swerve_drive_controller + swerve_drive_controller + 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) + target_link_libraries(test_load_swerve_drive_controller + controller_manager::controller_manager + ros2_control_test_assets::ros2_control_test_assets + ) +endif() + +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/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..46a9ebec5e --- /dev/null +++ b/swerve_drive_controller/doc/userdoc.rst @@ -0,0 +1,45 @@ +.. _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 +,,,,,,,,,,,, + +~/cmd_vel [geometry_msgs/msg/TwistStamped] (If ``use_stamped_vel=true``) + 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. + +~/cmd_vel [geometry_msgs/msg/Twist] (If ``use_stamped_vel=false``) + 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..fc1739a72c --- /dev/null +++ b/swerve_drive_controller/include/swerve_drive_controller/swerve_drive_controller.hpp @@ -0,0 +1,223 @@ +// 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 +// +// 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" + +// auto-generated by generate_parameter_library +#include "swerve_drive_controller/swerve_drive_controller_parameters.hpp" + +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; + +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; + +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; + +private: + template + std::optional 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 = rclcpp::get_logger("SwerveController"); + + if (name.empty()) + { + RCLCPP_ERROR(logger, "Joint name not given. Make sure all joints are specified."); + return std::nullopt; + } + + 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() == expected_interface_name && + interface.get_interface_name() == hw_if_type; + }); + + if (command_handle == command_interfaces.end()) + { + RCLCPP_ERROR( + logger, "Unable to find command interface for: %s (expected: %s, type: %s)", name.c_str(), + expected_interface_name.c_str(), hw_if_type.c_str()); + return std::nullopt; + } + auto state_handle = std::find_if( + state_interfaces.begin(), state_interfaces.end(), + [&expected_interface_name, &hw_if_type](const auto & interface) + { + 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 find state interface for: %s (expected: %s, type: %s)", name.c_str(), + expected_interface_name.c_str(), hw_if_type.c_str()); + return std::nullopt; + } + return T(std::ref(*command_handle), std::ref(*state_handle), name); + } + + inline std::optional get_wheel( + std::vector & command_interfaces, + const std::vector & state_interfaces, + const std::string & name) + { + return get_interface_object( + command_interfaces, state_interfaces, name, "/velocity", "velocity"); + } + + inline std::optional get_axle( + std::vector & command_interfaces, + const std::vector & state_interfaces, + const std::string & name) + { + return get_interface_object( + command_interfaces, state_interfaces, name, "/position", "position"); + } + +protected: + // Handles for four wheels and their axles + std::vector> wheel_handles_; + std::vector> axle_handles_; + std::array wheel_joint_names{}; + std::array axle_joint_names{}; + + std::shared_ptr param_listener_; + Params 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}; + + // Topic Subscription + bool subscriber_is_active_ = false; + rclcpp::Subscription::SharedPtr velocity_command_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; + tf2_msgs::msg::TFMessage odometry_transform_message_; + + 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..7141d11d08 --- /dev/null +++ b/swerve_drive_controller/include/swerve_drive_controller/swerve_drive_kinematics.hpp @@ -0,0 +1,98 @@ +// 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 +// +// 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 +#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 Default Constructor + SwerveDriveKinematics(); + + /** + * @brief Sets necessary params required for kinematics calculation. + * @param wheel_base Distance between front and rear axles (meters). + * @param track_width Distance between left and right wheels (meters). + * @param x_offset Optional global x offset of wheel positions. + * @param y_offset Optional global y offset of wheel positions. + * @attention order enforced as: front_left, front_right, rear_left, rear_right + */ + void calculate_wheel_position( + double wheel_base, double track_width, double x_offset = 0.0, double y_offset = 0.0); + + /** + * @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 +}; +} // 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..9c92e2b46c --- /dev/null +++ b/swerve_drive_controller/package.xml @@ -0,0 +1,46 @@ + + + + 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 + + Nitin Maurya + Apache-2.0 + + ament_cmake + + generate_parameter_library + ros2_control_cmake + + angles + backward_ros + geometry_msgs + hardware_interface + control_toolbox + controller_interface + nav_msgs + pluginlib + rclcpp + rclcpp_lifecycle + rcpputils + realtime_tools + tf2 + tf2_msgs + tf2_geometry_msgs + + controller_manager + ament_cmake_gtest + ament_cmake_gmock + hardware_interface_testing + ros2_control_test_assets + + + 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..e758ade2d1 --- /dev/null +++ b/swerve_drive_controller/src/swerve_drive_controller.cpp @@ -0,0 +1,523 @@ +// 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 +// +// 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_geometry_msgs/tf2_geometry_msgs.hpp" + +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(); } + +SwerveController::SwerveController() + +: controller_interface::ControllerInterface() +{ + 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(node->get_logger(), "Node is null in on_init"); + return controller_interface::CallbackReturn::ERROR; + } + try + { + // Create the parameter listener and get the parameters + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); + + swerveDriveKinematics_.calculate_wheel_position( + params_.wheelbase, params_.trackwidth, params_.offset[0], params_.offset[1]); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + return CallbackReturn::SUCCESS; +} + +InterfaceConfiguration SwerveController::command_interface_configuration() const +{ + std::vector conf_names; + 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(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(); + try + { + for (std::size_t i = 0; i < 6; ++i) + { + params_.pose_covariance_diagonal[i] = 0.01; + } + for (std::size_t i = 0; i < 6; ++i) + { + params_.twist_covariance_diagonal[i] = 0.01; + } + if (params_.front_left_wheel_joint.empty()) + { + RCLCPP_ERROR(logger, "front_left_wheel_joint_name is not set"); + return CallbackReturn::ERROR; + } + if (params_.front_right_wheel_joint.empty()) + { + RCLCPP_ERROR(logger, "front_right_wheel_joint_name is not set"); + return CallbackReturn::ERROR; + } + if (params_.rear_left_wheel_joint.empty()) + { + RCLCPP_ERROR(logger, "rear_left_wheel_joint_name is not set"); + return CallbackReturn::ERROR; + } + if (params_.rear_right_wheel_joint.empty()) + { + RCLCPP_ERROR(logger, "rear_right_wheel_joint_name is not set"); + return CallbackReturn::ERROR; + } + if (params_.front_left_axle_joint.empty()) + { + RCLCPP_ERROR(logger, "front_left_axle_joint_name is not set"); + return CallbackReturn::ERROR; + } + if (params_.front_right_axle_joint.empty()) + { + RCLCPP_ERROR(logger, "front_right_axle_joint_name is not set"); + return CallbackReturn::ERROR; + } + if (params_.rear_left_axle_joint.empty()) + { + RCLCPP_ERROR(logger, "rear_left_axle_joint_name_ is not set"); + return CallbackReturn::ERROR; + } + if (params_.rear_right_axle_joint.empty()) + { + RCLCPP_ERROR(logger, "rear_right_axle_joint_name_ is not set"); + return CallbackReturn::ERROR; + } + 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(params_.cmd_vel_timeout * 1000.0)); + 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)); + + 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)); + }); + + 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 + 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; + + 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] = 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()); + + realtime_odometry_transform_publisher_ = + std::make_shared>( + odometry_transform_publisher_); + + 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; + } + + return CallbackReturn::SUCCESS; +} + +CallbackReturn SwerveController::on_activate(const rclcpp_lifecycle::State &) +{ + auto logger = get_node()->get_logger(); + + wheel_handles_.resize(4); + for (std::size_t i = 0; i < 4; i++) + { + wheel_handles_[i] = get_wheel(command_interfaces_, state_interfaces_, wheel_joint_names[i]); + } + + axle_handles_.resize(4); + for (std::size_t i = 0; i < 4; i++) + { + axle_handles_[i] = get_axle(command_interfaces_, state_interfaces_, axle_joint_names[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]) + { + 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); + } + + 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(); + + RCLCPP_INFO(logger, "Updated Kinematics"); + 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; + + auto wheel_command = + swerveDriveKinematics_.compute_wheel_commands(linear_x_cmd, linear_y_cmd, angular_cmd); + + std::vector> wheel_data = { + {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) + { + if (wheel_command_.drive_velocity > threshold) + { + wheel_command_.drive_velocity = threshold; + } + } + + for (std::size_t i = 0; i < 4; i++) + { + if (!axle_handles_[i].has_value() || !wheel_handles_[i].has_value()) + { + 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; + + swerve_drive_controller::OdometryState odometry_; + + std::array velocity_array{}; + std::array steering_angle_array{}; + + for (std::size_t i = 0; i < 4; ++i) + { + if (params_.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); + + if (realtime_odometry_publisher_) + { + 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_->tryPublish(odometry_message); + } + + if (realtime_odometry_transform_publisher_) + { + auto & transform = odometry_transform_message_.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_->tryPublish(odometry_transform_message_); + } + previous_publish_timestamp_ = time; + 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(); + + 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() +{ + 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); + } +} + +} // 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..0b02780899 --- /dev/null +++ b/swerve_drive_controller/src/swerve_drive_controller_parameter.yaml @@ -0,0 +1,170 @@ +swerve_drive_controller: + + front_left_wheel_joint: { + type: string, + default_value: "", + 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.", + read_only: true + } + rear_left_wheel_joint: { + type: string, + default_value: "", + 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.", + read_only: true + } + front_left_axle_joint: { + type: string, + default_value: "", + 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.", + read_only: true + } + rear_left_axle_joint: { + type: string, + default_value: "", + 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.", + read_only: true + } + wheelbase: { + type: double, + default_value: 0.85, + description: "Wheelbase is the distance between front and rear wheels.", + validation: { + "swerve_drive_controller::gt<>": [0.0] + }, + read_only: true + } + trackwidth: { + type: double, + default_value: 0.75, + description: "Trackwidth is the distance between left and right wheels.", + validation: { + "swerve_drive_controller::gt<>": [0.0] + }, + read_only: true + } + offset: { + type: double_array, + default_value: [0.0, 0.0], + description: "Offset of center of gravity from geometric center", + read_only: true + } + wheel_radius: { + type: double, + default_value: 0.0775, + description: "Radius of the wheels.", + validation: { + "swerve_drive_controller::gt<>": [0.0] + }, + read_only: true + } + center_of_rotation: { + type: double_array, + default_value: [0.0, 0.0], + description: "Center of rotation (x,y) of robot", + read_only: true + } + 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] + }, + read_only: true + } + odom: { + type: string, + default_value: "odom", + 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.", + 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, + default_value: 2.0, + description: "Max velocity for front left wheel", + validation: { + "swerve_drive_controller::gt<>": [0.0] + }, + read_only: true + } + front_right_velocity_threshold: { + type: double, + default_value: 2.0, + description: "Max velocity for front right wheel", + validation: { + "swerve_drive_controller::gt<>": [0.0] + }, + read_only: true + } + rear_left_velocity_threshold: { + type: double, + default_value: 2.0, + description: "Max velocity for rear left wheel", + validation: { + "swerve_drive_controller::gt<>": [0.0] + }, + read_only: true + } + rear_right_velocity_threshold: { + type: double, + default_value: 2.0, + 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 + } 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..0e1b657d8e --- /dev/null +++ b/swerve_drive_controller/src/swerve_drive_kinematics.cpp @@ -0,0 +1,95 @@ +// 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 +// +// 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() : odometry_{0.0, 0.0, 0.0} {} + +void SwerveDriveKinematics::calculate_wheel_position( + double wheel_base, double track_width, double x_offset, double y_offset) +{ + double half_length = wheel_base / 2.0; + double half_width = track_width / 2.0; + + wheel_positions_[0] = {half_length - x_offset, half_width - y_offset}; // Front Left (+x, +y) + wheel_positions_[1] = {half_length - x_offset, -half_width - y_offset}; // Front Right (+x, -y) + wheel_positions_[2] = {-half_length - x_offset, half_width - y_offset}; // Rear Left (-x, +y) + wheel_positions_[3] = {-half_length - x_offset, -half_width - y_offset}; // Rear Right (-x, -y) +} + +std::array SwerveDriveKinematics::compute_wheel_commands( + double linear_velocity_x, double linear_velocity_y, double angular_velocity_z) +{ + std::array 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; + 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; + + // 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 = angles::normalize_angle(odometry_.theta + wz_robot * dt); + return odometry_; +} +} // 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..bd9b7721ac --- /dev/null +++ b/swerve_drive_controller/test/config/test_swerve_drive_controller.yaml @@ -0,0 +1,38 @@ +test_swerve_drive_controller: + ros__parameters: + update_rate: 10 #Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + swerve_drive_controller: + type: swerve_drive_controller/SwerveController + +swerve_drive_controller: + ros__parameters: + + 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 + odom: odom + base_footprint: base_footprint + 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..cd73a06815 --- /dev/null +++ b/swerve_drive_controller/test/test_load_swerve_drive_controller.cpp @@ -0,0 +1,50 @@ +// 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 +// +// 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..6b7d967658 --- /dev/null +++ b/swerve_drive_controller/test/test_swerve_drive_controller.cpp @@ -0,0 +1,396 @@ +// 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 +// +// 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 "controller_interface/controller_interface_base.hpp" +#include "lifecycle_msgs/msg/state.hpp" + +using lifecycle_msgs::msg::State; + +namespace swerve_drive_controller +{ + +class SwerveDriveControllerTest : public SwerveDriveControllerFixture +{ +}; + +TEST_F(SwerveDriveControllerTest, init_fails_without_parameters) +{ + const auto ret = + controller_->init(controller_name_, urdf_, 0, "", controller_->define_custom_node_options()); + ASSERT_EQ(ret, controller_interface::return_type::OK); +} + +TEST_F(SwerveDriveControllerTest, configure_fails_with_missing_wheels) +{ + std::vector wheel_joints = {"front_left_wheel_joint", "front_right_wheel_joint"}; + std::vector steering_joints = {"front_left_axle_joint", "front_right_axle_joint"}; + ASSERT_EQ(InitController(wheel_joints, steering_joints), controller_interface::return_type::OK); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); +} + +TEST_F(SwerveDriveControllerTest, configure_succeeds_no_namespace) +{ + ASSERT_EQ(InitController(), controller_interface::return_type::OK); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto command_interfaces = controller_->command_interface_configuration(); + ASSERT_EQ( + command_interfaces.names.size(), wheel_joint_names_.size() + steering_joint_names_.size()); + for (size_t i = 0; i < wheel_joint_names_.size(); ++i) + { + EXPECT_EQ(command_interfaces.names[i], wheel_joint_names_[i] + "/" + HW_IF_VELOCITY); + } + for (size_t i = 0; i < steering_joint_names_.size(); ++i) + { + EXPECT_EQ( + command_interfaces.names[i + wheel_joint_names_.size()], + steering_joint_names_[i] + "/" + HW_IF_POSITION); + } + EXPECT_EQ( + command_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); + + auto state_interfaces = controller_->state_interface_configuration(); + ASSERT_EQ( + state_interfaces.names.size(), wheel_joint_names_.size() + steering_joint_names_.size()); + for (size_t i = 0; i < wheel_joint_names_.size(); ++i) + { + EXPECT_EQ(state_interfaces.names[i], wheel_joint_names_[i] + "/" + HW_IF_VELOCITY); + } + for (size_t i = 0; i < steering_joint_names_.size(); ++i) + { + EXPECT_EQ( + state_interfaces.names[i + wheel_joint_names_.size()], + steering_joint_names_[i] + "/" + HW_IF_POSITION); + } + EXPECT_EQ(state_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); +} + +TEST_F(SwerveDriveControllerTest, configure_succeeds_with_namespace) +{ + std::string test_namespace = "/test_namespace"; + ASSERT_EQ( + InitController(wheel_joint_names_, steering_joint_names_, {}, test_namespace), + controller_interface::return_type::OK); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); +} + +TEST_F(SwerveDriveControllerTest, configure_succeeds_tf_prefix_false_no_namespace) +{ + std::string odom_id = "odom"; + std::string base_frame_id = "base_footprint"; + std::string frame_prefix = "test_prefix"; + + ASSERT_EQ( + InitController( + wheel_joint_names_, steering_joint_names_, + {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(false)), + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), + rclcpp::Parameter("odom", rclcpp::ParameterValue(odom_id)), + rclcpp::Parameter("base_footprint", rclcpp::ParameterValue(base_frame_id))}), + controller_interface::return_type::OK); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto odometry_message = controller_->get_realtime_odometry_publisher()->msg_; + std::string test_odom_frame_id = odometry_message.header.frame_id; + std::string test_base_frame_id = odometry_message.child_frame_id; + // Namespace is "/", so no prefix + ASSERT_EQ(test_odom_frame_id, odom_id); + ASSERT_EQ(test_base_frame_id, base_frame_id); +} + +TEST_F(SwerveDriveControllerTest, configure_succeeds_tf_prefix_true_no_namespace) +{ + std::string odom_id = "odom"; + std::string base_frame_id = "base_footprint"; + std::string frame_prefix = "test_prefix"; + + ASSERT_EQ( + InitController( + wheel_joint_names_, steering_joint_names_, + {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true)), + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), + rclcpp::Parameter("odom", rclcpp::ParameterValue(odom_id)), + rclcpp::Parameter("base_footprint", rclcpp::ParameterValue(base_frame_id))}), + controller_interface::return_type::OK); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto odometry_message = controller_->get_realtime_odometry_publisher()->msg_; + std::string test_odom_frame_id = odometry_message.header.frame_id; + std::string test_base_frame_id = odometry_message.child_frame_id; + ASSERT_EQ(test_odom_frame_id, odom_id); + ASSERT_EQ(test_base_frame_id, base_frame_id); +} + +TEST_F(SwerveDriveControllerTest, configure_succeeds_tf_blank_prefix_true_no_namespace) +{ + std::string odom_id = "odom"; + std::string base_frame_id = "base_footprint"; + std::string frame_prefix = ""; + + ASSERT_EQ( + InitController( + wheel_joint_names_, steering_joint_names_, + {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true)), + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), + rclcpp::Parameter("odom", rclcpp::ParameterValue(odom_id)), + rclcpp::Parameter("base_footprint", rclcpp::ParameterValue(base_frame_id))}), + controller_interface::return_type::OK); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto odometry_message = controller_->get_realtime_odometry_publisher()->msg_; + std::string test_odom_frame_id = odometry_message.header.frame_id; + std::string test_base_frame_id = odometry_message.child_frame_id; + ASSERT_EQ(test_odom_frame_id, odom_id); + ASSERT_EQ(test_base_frame_id, base_frame_id); +} + +TEST_F(SwerveDriveControllerTest, configure_succeeds_tf_prefix_false_set_namespace) +{ + std::string test_namespace = "/test_namespace"; + std::string odom_id = "odom"; + std::string base_frame_id = "base_footprint"; + std::string frame_prefix = "test_prefix"; + + ASSERT_EQ( + InitController( + wheel_joint_names_, steering_joint_names_, + {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(false)), + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), + rclcpp::Parameter("odom", rclcpp::ParameterValue(odom_id)), + rclcpp::Parameter("base_footprint", rclcpp::ParameterValue(base_frame_id))}, + test_namespace), + controller_interface::return_type::OK); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto odometry_message = controller_->get_realtime_odometry_publisher()->msg_; + std::string test_odom_frame_id = odometry_message.header.frame_id; + std::string test_base_frame_id = odometry_message.child_frame_id; + ASSERT_EQ(test_odom_frame_id, "/test_namespace/odom"); + ASSERT_EQ(test_base_frame_id, "/test_namespace/base_footprint"); +} + +TEST_F(SwerveDriveControllerTest, configure_succeeds_tf_prefix_true_set_namespace) +{ + std::string test_namespace = "/test_namespace"; + std::string odom_id = "odom"; + std::string base_frame_id = "base_footprint"; + std::string frame_prefix = "test_prefix"; + + ASSERT_EQ( + InitController( + wheel_joint_names_, steering_joint_names_, + {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true)), + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), + rclcpp::Parameter("odom", rclcpp::ParameterValue(odom_id)), + rclcpp::Parameter("base_footprint", rclcpp::ParameterValue(base_frame_id))}, + test_namespace), + controller_interface::return_type::OK); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto odometry_message = controller_->get_realtime_odometry_publisher()->msg_; + std::string test_odom_frame_id = odometry_message.header.frame_id; + std::string test_base_frame_id = odometry_message.child_frame_id; + ASSERT_EQ(test_odom_frame_id, "/test_namespace/odom"); + ASSERT_EQ(test_base_frame_id, "/test_namespace/base_footprint"); +} + +TEST_F(SwerveDriveControllerTest, configure_succeeds_tf_blank_prefix_true_set_namespace) +{ + std::string test_namespace = "/test_namespace"; + std::string odom_id = "odom"; + std::string base_frame_id = "base_footprint"; + std::string frame_prefix = ""; + + ASSERT_EQ( + InitController( + wheel_joint_names_, steering_joint_names_, + {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true)), + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), + rclcpp::Parameter("odom", rclcpp::ParameterValue(odom_id)), + rclcpp::Parameter("base_footprint", rclcpp::ParameterValue(base_frame_id))}, + test_namespace), + controller_interface::return_type::OK); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto odometry_message = controller_->get_realtime_odometry_publisher()->msg_; + std::string test_odom_frame_id = odometry_message.header.frame_id; + std::string test_base_frame_id = odometry_message.child_frame_id; + ASSERT_EQ(test_odom_frame_id, "/test_namespace/odom"); + ASSERT_EQ(test_base_frame_id, "/test_namespace/base_footprint"); +} + +TEST_F(SwerveDriveControllerTest, activate_fails_without_resources_assigned) +{ + ASSERT_EQ(InitController(), controller_interface::return_type::OK); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_ERROR); +} + +TEST_F(SwerveDriveControllerTest, activate_succeeds_with_resources_assigned) +{ + ASSERT_EQ(InitController(), controller_interface::return_type::OK); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + assignResources(); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); +} + +TEST_F(SwerveDriveControllerTest, deactivate_then_activate) +{ + ASSERT_EQ(InitController(), controller_interface::return_type::OK); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + auto state = controller_->get_node()->configure(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + assignResources(); + + state = controller_->get_node()->activate(); + ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + + waitForSetup(); + + publish_twist(1.0, 0.0, 0.0); + controller_->wait_for_twist(executor); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + std::vector expected_wheel_vel_cmds = {1.0, 1.0, 1.0, 1.0}; + std::vector expected_steering_pos_cmds = {0.0, 0.0, 0.0, 0.0}; + for (size_t i = 0; i < wheel_vel_cmds_.size(); i++) + { + EXPECT_DOUBLE_EQ(command_itfs_[i].get_optional().value(), expected_wheel_vel_cmds[i]); + } + for (size_t i = 0; i < steering_pos_cmds_.size(); i++) + { + EXPECT_DOUBLE_EQ( + command_itfs_[i + wheel_vel_cmds_.size()].get_optional().value(), + expected_steering_pos_cmds[i]); + } + + std::this_thread::sleep_for(std::chrono::milliseconds(300)); + state = controller_->get_node()->deactivate(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + for (size_t i = 0; i < command_itfs_.size(); i++) + { + EXPECT_EQ(command_itfs_[i].get_optional().value(), 0.0); + } + + state = controller_->get_node()->activate(); + ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + + waitForSetup(); + + for (size_t i = 0; i < command_itfs_.size(); i++) + { + EXPECT_EQ(command_itfs_[i].get_optional().value(), 0.0); + } + + publish_twist(1.0, 0.0, 0.0); // Forward motion + controller_->wait_for_twist(executor); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + for (size_t i = 0; i < wheel_vel_cmds_.size(); i++) + { + EXPECT_DOUBLE_EQ(command_itfs_[i].get_optional().value(), expected_wheel_vel_cmds[i]); + } + for (size_t i = 0; i < steering_pos_cmds_.size(); i++) + { + EXPECT_DOUBLE_EQ( + command_itfs_[i + wheel_vel_cmds_.size()].get_optional().value(), + expected_steering_pos_cmds[i]); + } + + // Deactivate and cleanup + std::this_thread::sleep_for(std::chrono::milliseconds(300)); + state = controller_->get_node()->deactivate(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); + state = controller_->get_node()->cleanup(); + ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); + executor.cancel(); +} + +TEST_F(SwerveDriveControllerTest, command_with_zero_timestamp_is_accepted_with_warning) +{ + ASSERT_EQ(InitController(), controller_interface::return_type::OK); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + auto state = controller_->get_node()->configure(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + assignResources(); + + state = controller_->get_node()->activate(); + ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + + waitForSetup(); + + publish_twist_timestamped(rclcpp::Time(0, 0, RCL_ROS_TIME), 1.0, 0.0, 0.0); + controller_->wait_for_twist(executor); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + std::vector expected_wheel_vel_cmds = {1.0, 1.0, 1.0, 1.0}; + std::vector expected_steering_pos_cmds = {0.0, 0.0, 0.0, 0.0}; + for (size_t i = 0; i < wheel_vel_cmds_.size(); i++) + { + EXPECT_DOUBLE_EQ(command_itfs_[i].get_optional().value(), expected_wheel_vel_cmds[i]); + } + for (size_t i = 0; i < steering_pos_cmds_.size(); i++) + { + EXPECT_DOUBLE_EQ( + command_itfs_[i + wheel_vel_cmds_.size()].get_optional().value(), + expected_steering_pos_cmds[i]); + } + + std::this_thread::sleep_for(std::chrono::milliseconds(300)); + state = controller_->get_node()->deactivate(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); + state = controller_->get_node()->cleanup(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_UNCONFIGURED); + executor.cancel(); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} +} // namespace swerve_drive_controller 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..e7e9e8572f --- /dev/null +++ b/swerve_drive_controller/test/test_swerve_drive_controller.hpp @@ -0,0 +1,286 @@ +// 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 +// +// 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 +#include +#include +#include +#include "hardware_interface/handle.hpp" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors.hpp" +#include "rclcpp/parameter_value.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/utilities.hpp" +#include "swerve_drive_controller/swerve_drive_controller.hpp" + +using hardware_interface::HW_IF_POSITION; +using hardware_interface::HW_IF_VELOCITY; + +namespace swerve_drive_controller +{ +constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; +constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; + +std::vector wheel_joint_names_ = { + "front_left_wheel_joint", "front_right_wheel_joint", "rear_left_wheel_joint", + "rear_right_wheel_joint"}; +std::vector steering_joint_names_ = { + "front_left_axle_joint", "front_right_axle_joint", "rear_left_axle_joint", + "rear_right_axle_joint"}; + +class TestableSwerveDriveController : public SwerveController +{ + FRIEND_TEST(SwerveDriveControllerTest, init_fails_without_parameters); + FRIEND_TEST(SwerveDriveControllerTest, configure_fails_with_missing_wheels); + FRIEND_TEST(SwerveDriveControllerTest, configure_succeeds_no_namespace); + FRIEND_TEST(SwerveDriveControllerTest, configure_succeeds_with_namespace); + FRIEND_TEST(SwerveDriveControllerTest, configure_succeeds_tf_prefix_false_no_namespace); + FRIEND_TEST(SwerveDriveControllerTest, configure_succeeds_tf_prefix_true_no_namespace); + FRIEND_TEST(SwerveDriveControllerTest, configure_succeeds_tf_blank_prefix_true_no_namespace); + FRIEND_TEST(SwerveDriveControllerTest, configure_succeeds_tf_prefix_false_set_namespace); + FRIEND_TEST(SwerveDriveControllerTest, configure_succeeds_tf_prefix_true_set_namespace); + FRIEND_TEST(SwerveDriveControllerTest, configure_succeeds_tf_blank_prefix_true_set_namespace); + FRIEND_TEST(SwerveDriveControllerTest, activate_fails_without_resources_assigned); + FRIEND_TEST(SwerveDriveControllerTest, activate_succeeds_with_resources_assigned); + FRIEND_TEST(SwerveDriveControllerTest, deactivate_then_activate); + FRIEND_TEST(SwerveDriveControllerTest, command_with_zero_timestamp_is_accepted_with_warning); + +public: + std::shared_ptr> + get_realtime_odometry_publisher() const + { + return realtime_odometry_publisher_; + } + + /** + * @brief wait_for_twist block until a new twist is received. + * Requires that the executor is not spinned elsewhere between the + * message publication and the call to this function + */ + void wait_for_twist( + rclcpp::Executor & executor, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds(500)) + { + auto until = get_node()->get_clock()->now() + timeout; + while (get_node()->get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + } +}; + +template +class SwerveDriveControllerFixture : public ::testing::Test +{ +public: + void SetUp() override + { + controller_ = std::make_unique(); + + cmd_vel_publisher_node_ = std::make_shared("cmd_vel_publisher"); + cmd_vel_publisher_ = + cmd_vel_publisher_node_->create_publisher( + "/test_swerve_drive_controller/cmd_vel", rclcpp::SystemDefaultsQoS()); + + odom_subscriber_node_ = std::make_shared("odom_subscriber"); + odom_sub_ = odom_subscriber_node_->create_subscription( + "/test_swerve_drive_controller/odom", 10, + [this](const nav_msgs::msg::Odometry::SharedPtr msg) { last_odom_msg_ = msg; }); + } + + static void SetUpTestCase() { rclcpp::init(0, nullptr); } + + static void TearDownTestCase() { rclcpp::shutdown(); } + +protected: + void publish_twist(double linear_x = 1.0, double linear_y = 1.0, double angular = 1.0) + { + publish_twist_timestamped( + cmd_vel_publisher_node_->get_clock()->now(), linear_x, linear_y, angular); + } + + void publish_twist_timestamped( + const rclcpp::Time & stamp, const double & twist_linear_x = 1.0, + const double & twist_linear_y = 1.0, const double & twist_angular_z = 1.0) + { + const char * topic_name = cmd_vel_publisher_->get_topic_name(); + size_t wait_count = 0; + while (cmd_vel_publisher_node_->count_subscribers(topic_name) == 0) + { + if (wait_count >= 5) + { + auto error_msg = + std::string("publishing to ") + topic_name + " but no node subscribes to it"; + throw std::runtime_error(error_msg); + } + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + ++wait_count; + } + + geometry_msgs::msg::TwistStamped msg; + msg.header.stamp = stamp; + msg.twist.linear.x = twist_linear_x; + msg.twist.linear.y = twist_linear_y; + msg.twist.linear.z = std::numeric_limits::quiet_NaN(); + msg.twist.angular.x = std::numeric_limits::quiet_NaN(); + msg.twist.angular.y = std::numeric_limits::quiet_NaN(); + msg.twist.angular.z = twist_angular_z; + + cmd_vel_publisher_->publish(msg); + } + + /// \brief wait for the subscriber and publisher to completely setup + void waitForSetup() + { + constexpr std::chrono::seconds TIMEOUT{2}; + auto clock = cmd_vel_publisher_node_->get_clock(); + auto start = clock->now(); + while (cmd_vel_publisher_->get_subscription_count() <= 0) + { + if ((clock->now() - start) > TIMEOUT) + { + FAIL(); + } + rclcpp::spin_some(cmd_vel_publisher_node_); + } + } + + void assignResources() + { + std::vector state_ifs; + state_ifs.reserve(wheel_vel_states_.size() + steering_pos_states_.size()); + state_itfs_.reserve(wheel_vel_states_.size() + steering_pos_states_.size()); + std::vector command_ifs; + command_ifs.reserve(wheel_vel_cmds_.size() + steering_pos_cmds_.size()); + command_itfs_.reserve(wheel_vel_cmds_.size() + steering_pos_cmds_.size()); + + // Wheel velocity interfaces + for (size_t i = 0; i < wheel_vel_states_.size(); ++i) + { + state_itfs_.emplace_back( + hardware_interface::StateInterface( + wheel_joint_names_[i], HW_IF_VELOCITY, &wheel_vel_states_[i])); + state_ifs.emplace_back(state_itfs_.back()); + command_itfs_.emplace_back( + hardware_interface::CommandInterface( + wheel_joint_names_[i], HW_IF_VELOCITY, &wheel_vel_cmds_[i])); + command_ifs.emplace_back(command_itfs_.back()); + } + + // Steering position interfaces + for (size_t i = 0; i < steering_pos_states_.size(); ++i) + { + state_itfs_.emplace_back( + hardware_interface::StateInterface( + steering_joint_names_[i], HW_IF_POSITION, &steering_pos_states_[i])); + state_ifs.emplace_back(state_itfs_.back()); + command_itfs_.emplace_back( + hardware_interface::CommandInterface( + steering_joint_names_[i], HW_IF_POSITION, &steering_pos_cmds_[i])); + command_ifs.emplace_back(command_itfs_.back()); + } + + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + } + + controller_interface::return_type InitController( + const std::vector & wheel_joints = wheel_joint_names_, + const std::vector & steering_joints = steering_joint_names_, + const std::vector & parameters = {}, const std::string & ns = "") + { + auto node_options = rclcpp::NodeOptions(); + std::vector parameter_overrides; + + if (wheel_joints.size() > 0) + { + parameter_overrides.push_back(rclcpp::Parameter("front_left_wheel_joint", wheel_joints[0])); + } + if (wheel_joints.size() > 1) + { + parameter_overrides.push_back(rclcpp::Parameter("front_right_wheel_joint", wheel_joints[1])); + } + if (wheel_joints.size() > 2) + { + parameter_overrides.push_back(rclcpp::Parameter("rear_left_wheel_joint", wheel_joints[2])); + } + if (wheel_joints.size() > 3) + { + parameter_overrides.push_back(rclcpp::Parameter("rear_right_wheel_joint", wheel_joints[3])); + } + if (steering_joints.size() > 0) + { + parameter_overrides.push_back(rclcpp::Parameter("front_left_axle_joint", steering_joints[0])); + } + if (steering_joints.size() > 1) + { + parameter_overrides.push_back( + rclcpp::Parameter("front_right_axle_joint", steering_joints[1])); + } + if (steering_joints.size() > 2) + { + parameter_overrides.push_back(rclcpp::Parameter("rear_left_axle_joint", steering_joints[2])); + } + if (steering_joints.size() > 3) + { + parameter_overrides.push_back(rclcpp::Parameter("rear_right_axle_joint", steering_joints[3])); + } + + if (wheel_joints.size() >= 4 && steering_joints.size() >= 4) + { + parameter_overrides.push_back(rclcpp::Parameter("wheelbase", rclcpp::ParameterValue(0.85))); + parameter_overrides.push_back(rclcpp::Parameter("trackwidth", rclcpp::ParameterValue(0.75))); + parameter_overrides.push_back(rclcpp::Parameter("wheel_radius", rclcpp::ParameterValue(0.1))); + parameter_overrides.push_back( + rclcpp::Parameter("cmd_vel_timeout", rclcpp::ParameterValue(0.5))); + parameter_overrides.push_back(rclcpp::Parameter("odom", rclcpp::ParameterValue("odom"))); + parameter_overrides.push_back( + rclcpp::Parameter("base_footprint", rclcpp::ParameterValue("base_footprint"))); + parameter_overrides.push_back(rclcpp::Parameter("open_loop", rclcpp::ParameterValue(false))); + } + + parameter_overrides.insert(parameter_overrides.end(), parameters.begin(), parameters.end()); + node_options.parameter_overrides(parameter_overrides); + + return controller_->init("test_swerve_drive_controller", "", 0, ns, node_options); + } + + std::vector wheel_vel_states_ = {1.0, 1.0, 1.0, 1.0}; + std::vector wheel_vel_cmds_ = {0.0, 0.0, 0.0, 0.0}; + std::vector steering_pos_states_ = {0.0, 0.0, 0.0, 0.0}; + std::vector steering_pos_cmds_ = {0.0, 0.0, 0.0, 0.0}; + + std::vector state_itfs_; + std::vector command_itfs_; + + std::unique_ptr controller_; + rclcpp::Node::SharedPtr cmd_vel_publisher_node_; + rclcpp::Publisher::SharedPtr cmd_vel_publisher_; + rclcpp::Node::SharedPtr odom_subscriber_node_; + rclcpp::Subscription::SharedPtr odom_sub_; + nav_msgs::msg::Odometry::SharedPtr last_odom_msg_; + const std::string urdf_ = ""; + std::string controller_name_ = "test_swerve_drive_controller"; +}; +} // namespace swerve_drive_controller +#endif // TEST_SWERVE_DRIVE_CONTROLLER_HPP_