diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index 235800711b..01a3e127a9 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. +* ``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/migration.rst b/doc/migration.rst index f264e650ef..a4f00c9472 100644 --- a/doc/migration.rst +++ b/doc/migration.rst @@ -15,5 +15,4 @@ 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. diff --git a/doc/mobile_robot_kinematics.rst b/doc/mobile_robot_kinematics.rst index 4bf285972e..28c077b675 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). + 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 cc4a2bd5d4..9c5729ba16 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -11,6 +11,8 @@ 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 @@ -22,3 +24,4 @@ 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 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 diff --git a/swerve_drive_controller/CMakeLists.txt b/swerve_drive_controller/CMakeLists.txt new file mode 100644 index 0000000000..c47e5c16e1 --- /dev/null +++ b/swerve_drive_controller/CMakeLists.txt @@ -0,0 +1,101 @@ +cmake_minimum_required(VERSION 3.8) +project(swerve_drive_controller) + +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 + tf2 + tf2_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}) + +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) +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) + find_package(ros2_control_test_assets REQUIRED) + + ament_add_gtest(test_swerve_drive_controller + test/test_swerve_drive_controller.cpp + ) + + 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 + ) + + 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 + ) +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..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..2c9b9ac34d --- /dev/null +++ b/swerve_drive_controller/include/swerve_drive_controller/swerve_drive_controller.hpp @@ -0,0 +1,195 @@ +// 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" + +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; + 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::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::vector> wheel_handles_; + 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 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::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 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..38c2559218 --- /dev/null +++ b/swerve_drive_controller/include/swerve_drive_controller/swerve_drive_kinematics.hpp @@ -0,0 +1,99 @@ +// 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 + + +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..e64969948b --- /dev/null +++ b/swerve_drive_controller/package.xml @@ -0,0 +1,38 @@ + + + + swerve_drive_controller + 5.0.0 + Controller for a 4 wheel swerve drive robot base. + Nitin Maurya + Apache-2.0 + + ament_cmake + + 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 + + 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..6a3d4cbc90 --- /dev/null +++ b/swerve_drive_controller/src/swerve_drive_controller.cpp @@ -0,0 +1,648 @@ +// 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/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_ = {{ + {-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_) +{ + 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("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", 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; + } + + 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)); + + 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; + } + + return CallbackReturn::SUCCESS; +} + +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++) { + wheel_handles_[i] = get_wheel(wheel_joint_names[i]); + } + + axle_handles_.resize(4); + 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]) { + 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 = 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 & [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]); + } + 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(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); + + try { + if (previous_publish_timestamp_ + publish_period_ < time) { + previous_publish_timestamp_ += publish_period_; + } + } catch (const std::runtime_error &) { + previous_publish_timestamp_ = time; + } + + 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() +{ + 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) +{ + auto logger = rclcpp::get_logger("SwerveController"); + + if (name.empty()) { + RCLCPP_ERROR(logger, "Joint name not given. Make sure all joints are specified."); + return nullptr; + } + + 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", 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(), + [&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 the state interface for: %s", + name.c_str()); + return nullptr; + } + static_assert(!std::is_const_v>, + "Command handle is const!"); + return std::make_unique(std::ref(*command_handle), std::ref(*state_handle), 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); +} + +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); +} + + +} // 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..d6bd248a1f --- /dev/null +++ b/swerve_drive_controller/src/swerve_drive_kinematics.cpp @@ -0,0 +1,97 @@ +// 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( + 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; + 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 = 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..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..b698b19f1c --- /dev/null +++ b/swerve_drive_controller/test/test_swerve_drive_controller.cpp @@ -0,0 +1,457 @@ +// 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 +#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..2bb21daf8a --- /dev/null +++ b/swerve_drive_controller/test/test_swerve_drive_controller.hpp @@ -0,0 +1,58 @@ +// 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 "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_