diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index fe09839ce5..4e25c3baec 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -29,6 +29,7 @@ Controllers for Wheeled Mobile Robots Differential Drive Controller <../diff_drive_controller/doc/userdoc.rst> Mecanum Drive Controllers <../mecanum_drive_controller/doc/userdoc.rst> + Multi Omni Wheel Drive Controller <../multi_omni_wheel_drive_controller/doc/userdoc.rst> Steering Controllers Library <../steering_controllers_library/doc/userdoc.rst> Tricycle Controller <../tricycle_controller/doc/userdoc.rst> diff --git a/doc/release_notes.rst b/doc/release_notes.rst index eaabcc1e91..3a4fe60c2f 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -59,6 +59,10 @@ mecanum_drive_controller ************************ * 🚀 The mecanum_drive_controller was added 🎉 (`#512 `_). +multi_omni_wheel_drive_controller +********************************* +* 🚀 The multi_omni_wheel_drive_controller was added 🎉 (`#1535 `_). + pid_controller ************************ * 🚀 The PID controller was added 🎉 (`#434 `_). diff --git a/multi_omni_wheel_drive_controller/CMakeLists.txt b/multi_omni_wheel_drive_controller/CMakeLists.txt new file mode 100644 index 0000000000..b3d8ef8a48 --- /dev/null +++ b/multi_omni_wheel_drive_controller/CMakeLists.txt @@ -0,0 +1,97 @@ +cmake_minimum_required(VERSION 3.16) +project(multi_omni_wheel_drive_controller) + +find_package(ros2_control_cmake REQUIRED) +set_compiler_options() +export_windows_symbols() + +# Find dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS + ament_cmake + controller_interface + generate_parameter_library + geometry_msgs + hardware_interface + nav_msgs + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + tf2 + tf2_msgs + Eigen3 +) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +generate_parameter_library( + ${PROJECT_NAME}_parameters + src/${PROJECT_NAME}_parameters.yaml +) + +add_library(${PROJECT_NAME} SHARED + src/${PROJECT_NAME}.cpp + src/odometry.cpp +) +target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_17) +target_include_directories(${PROJECT_NAME} + PUBLIC $ + $ +) +target_link_libraries(${PROJECT_NAME} PRIVATE + ${PROJECT_NAME}_parameters + Eigen3::Eigen +) +ament_target_dependencies(${PROJECT_NAME} PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) +pluginlib_export_plugin_description_file(controller_interface multi_omni_wheel_drive_plugin.xml) + +# Install +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME}/ +) +install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_parameters + EXPORT export_${PROJECT_NAME} + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + ament_add_gmock(test_${PROJECT_NAME} test/test_${PROJECT_NAME}.cpp) + target_link_libraries(test_${PROJECT_NAME} + ${PROJECT_NAME} + ${PROJECT_NAME}_parameters + Eigen3::Eigen + ) + ament_target_dependencies(test_${PROJECT_NAME} + geometry_msgs + hardware_interface + nav_msgs + rclcpp + rclcpp_lifecycle + realtime_tools + tf2 + tf2_msgs + Eigen3 + ) + + add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") + ament_add_gmock(test_load_${PROJECT_NAME} test/test_load_${PROJECT_NAME}.cpp) + ament_target_dependencies(test_load_${PROJECT_NAME} + controller_manager + ros2_control_test_assets + ) + + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/multi_omni_wheel_drive_controller/doc/userdoc.rst b/multi_omni_wheel_drive_controller/doc/userdoc.rst new file mode 100644 index 0000000000..b1f0399fa2 --- /dev/null +++ b/multi_omni_wheel_drive_controller/doc/userdoc.rst @@ -0,0 +1,78 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/multi_omni_wheel_drive_controller/doc/userdoc.rst + +.. _multi_omni_wheel_drive_controller_userdoc: + +multi_omni_wheel_drive_controller +================================= + +Controller for mobile robots with omnidirectional drive. + +Supports using 3 or more omni wheels spaced at an equal angle from each other in a circular formation. +To better understand this, have a look at :ref:`mobile_robot_kinematics`. + +The controller uses velocity input, i.e., stamped Twist messages where linear ``x``, ``y``, and angular ``z`` components are used. +Values in other components are ignored. + +Odometry is computed from hardware feedback and published. + +Other features +-------------- + + + Realtime-safe implementation. + + Odometry publishing + + Automatic stop after command time-out + +Description of controller's interfaces +-------------------------------------- + +References (from a preceding controller) +,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,, + +When controller is in chained mode, it exposes the following references which can be commanded by the preceding controller: + +- ``/linear/x/velocity`` double, in m/s +- ``/linear/y/velocity`` double, in m/s +- ``/angular/z/velocity`` double, in rad/s + +Together, these represent the body twist (which in unchained-mode would be obtained from ~/cmd_vel). + +State interfaces +,,,,,,,,,,,,,,,, + +As feedback interface type the joints' position (``hardware_interface::HW_IF_POSITION``) or velocity (``hardware_interface::HW_IF_VELOCITY``, if parameter ``position_feedback=false``) are used. + +Command interfaces +,,,,,,,,,,,,,,,,,,,,,, + +Joints' velocity (``hardware_interface::HW_IF_VELOCITY``) are used. + + +ROS 2 Interfaces +---------------- + +Subscribers +,,,,,,,,,,, + +~/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. + +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 +,,,,,,,,,, + +This controller uses the `generate_parameter_library `_ to handle its parameters. The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. + +.. generate_parameter_library_details:: ../src/multi_omni_wheel_drive_controller_parameters.yaml + +An example parameter file for this controller can be found in `the test directory `_: + +.. literalinclude:: ../test/config/test_multi_omni_wheel_drive_controller.yaml + :language: yaml diff --git a/multi_omni_wheel_drive_controller/include/multi_omni_wheel_drive_controller/multi_omni_wheel_drive_controller.hpp b/multi_omni_wheel_drive_controller/include/multi_omni_wheel_drive_controller/multi_omni_wheel_drive_controller.hpp new file mode 100644 index 0000000000..74c881c082 --- /dev/null +++ b/multi_omni_wheel_drive_controller/include/multi_omni_wheel_drive_controller/multi_omni_wheel_drive_controller.hpp @@ -0,0 +1,122 @@ +// Copyright 2025 Aarav Gupta +// +// 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 MULTI_OMNI_WHEEL_DRIVE_CONTROLLER__MULTI_OMNI_WHEEL_DRIVE_CONTROLLER_HPP_ +#define MULTI_OMNI_WHEEL_DRIVE_CONTROLLER__MULTI_OMNI_WHEEL_DRIVE_CONTROLLER_HPP_ + +#include +#include +#include + +#include "controller_interface/chainable_controller_interface.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "multi_omni_wheel_drive_controller/odometry.hpp" +#include "nav_msgs/msg/odometry.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 "multi_omni_wheel_drive_controller/multi_omni_wheel_drive_controller_parameters.hpp" + +namespace multi_omni_wheel_drive_controller +{ +class MultiOmniWheelDriveController : public controller_interface::ChainableControllerInterface +{ + using TwistStamped = geometry_msgs::msg::TwistStamped; + +public: + MultiOmniWheelDriveController(); + + controller_interface::CallbackReturn on_init() override; + + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + controller_interface::return_type update_reference_from_subscribers( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + controller_interface::return_type update_and_write_commands( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + controller_interface::CallbackReturn on_cleanup( + const rclcpp_lifecycle::State & previous_state) override; + + controller_interface::CallbackReturn on_error( + const rclcpp_lifecycle::State & previous_state) override; + +protected: + bool on_set_chained_mode(bool chained_mode) override; + + std::vector on_export_reference_interfaces() override; + + // Parameters from ROS for multi_omni_wheel_drive_controller + std::shared_ptr param_listener_; + Params params_; + + const char * feedback_type() const; + + struct WheelHandle + { + std::reference_wrapper feedback; + std::reference_wrapper velocity; + }; + std::vector registered_wheel_handles_; + + controller_interface::CallbackReturn configure_wheel_handles( + const std::vector & wheel_names, std::vector & registered_handles); + + // Timeout to consider cmd_vel commands old + rclcpp::Duration cmd_vel_timeout_ = rclcpp::Duration::from_seconds(0.5); + + bool subscriber_is_active_ = false; + rclcpp::Subscription::SharedPtr velocity_command_subscriber_ = nullptr; + realtime_tools::RealtimeBuffer> received_velocity_msg_ptr_{nullptr}; + + void compute_and_set_wheel_velocities(); + + std::shared_ptr> odometry_publisher_ = nullptr; + std::shared_ptr> + realtime_odometry_publisher_ = nullptr; + + Odometry odometry_; + + std::shared_ptr> odometry_transform_publisher_ = + nullptr; + std::shared_ptr> + realtime_odometry_transform_publisher_ = nullptr; + + // Variables to help limit the publish rate of odometry as described using params_.publish_rate + rclcpp::Duration publish_period_ = rclcpp::Duration::from_seconds(0); + rclcpp::Time previous_publish_timestamp_{0, 0, RCL_CLOCK_UNINITIALIZED}; + + bool reset(); + void halt(); + +private: + void reset_buffers(); +}; +} // namespace multi_omni_wheel_drive_controller + +#endif // MULTI_OMNI_WHEEL_DRIVE_CONTROLLER__MULTI_OMNI_WHEEL_DRIVE_CONTROLLER_HPP_ diff --git a/multi_omni_wheel_drive_controller/include/multi_omni_wheel_drive_controller/odometry.hpp b/multi_omni_wheel_drive_controller/include/multi_omni_wheel_drive_controller/odometry.hpp new file mode 100644 index 0000000000..3456e22b48 --- /dev/null +++ b/multi_omni_wheel_drive_controller/include/multi_omni_wheel_drive_controller/odometry.hpp @@ -0,0 +1,76 @@ +// Copyright 2025 Aarav Gupta +// +// 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 MULTI_OMNI_WHEEL_DRIVE_CONTROLLER__ODOMETRY_HPP_ +#define MULTI_OMNI_WHEEL_DRIVE_CONTROLLER__ODOMETRY_HPP_ + +#include +#include + +#include "rclcpp/time.hpp" + +namespace multi_omni_wheel_drive_controller +{ +class Odometry +{ +public: + Odometry(); + + bool updateFromPos(const std::vector & wheels_pos, const rclcpp::Time & time); + bool updateFromVel(const std::vector & wheels_vel, const rclcpp::Time & time); + bool updateOpenLoop( + const double & linear_x_vel, const double & linear_y_vel, const double & angular_vel, + const rclcpp::Time & time); + void resetOdometry(); + + double getX() const { return x_; } + double getY() const { return y_; } + double getHeading() const { return heading_; } + double getLinearXVel() const { return linear_x_vel_; } + double getLinearYVel() const { return linear_y_vel_; } + double getAngularVel() const { return angular_vel_; } + + void setParams( + const double & robot_radius, const double & wheel_radius, const double & wheel_offset, + const size_t & wheel_count); + +private: + Eigen::Vector3d compute_robot_velocity(const std::vector & wheels_vel) const; + void integrate(const double & dx, const double & dy, const double & dheading); + + // Current timestamp: + rclcpp::Time timestamp_; + + // Current pose: + double x_; // [m] + double y_; // [m] + double heading_; // [rads] + + // Current velocity: + double linear_x_vel_; // [m/s] + double linear_y_vel_; // [m/s] + double angular_vel_; // [rads/s] + + // Robot kinematic parameters: + double robot_radius_; // [m] + double wheel_radius_; // [m] + double wheel_offset_; // [rads] + + // Previous wheel positions/states [rads]: + std::vector wheels_old_pos_; +}; + +} // namespace multi_omni_wheel_drive_controller + +#endif // MULTI_OMNI_WHEEL_DRIVE_CONTROLLER__ODOMETRY_HPP_ diff --git a/multi_omni_wheel_drive_controller/multi_omni_wheel_drive_plugin.xml b/multi_omni_wheel_drive_controller/multi_omni_wheel_drive_plugin.xml new file mode 100644 index 0000000000..9726b00f9e --- /dev/null +++ b/multi_omni_wheel_drive_controller/multi_omni_wheel_drive_plugin.xml @@ -0,0 +1,9 @@ + + + + A controller for mobile robots with omnidirectional drive for 3 or more omni wheels. + + + diff --git a/multi_omni_wheel_drive_controller/package.xml b/multi_omni_wheel_drive_controller/package.xml new file mode 100644 index 0000000000..fc2191579d --- /dev/null +++ b/multi_omni_wheel_drive_controller/package.xml @@ -0,0 +1,35 @@ + + + + multi_omni_wheel_drive_controller + 0.0.0 + TODO: Package description + Aarav Gupta + Apache License 2.0 + + ament_cmake + + generate_parameter_library + ros2_control_cmake + + controller_interface + eigen + geometry_msgs + hardware_interface + nav_msgs + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + tf2 + tf2_msgs + + ament_cmake_gmock + controller_manager + hardware_interface + ros2_control_test_assets + + + ament_cmake + + diff --git a/multi_omni_wheel_drive_controller/src/multi_omni_wheel_drive_controller.cpp b/multi_omni_wheel_drive_controller/src/multi_omni_wheel_drive_controller.cpp new file mode 100644 index 0000000000..fec9408033 --- /dev/null +++ b/multi_omni_wheel_drive_controller/src/multi_omni_wheel_drive_controller.cpp @@ -0,0 +1,569 @@ +// Copyright 2025 Aarav Gupta +// +// 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 "multi_omni_wheel_drive_controller/multi_omni_wheel_drive_controller.hpp" + +#include +#include +#include +#include + +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/logger.hpp" +#include "rclcpp/logging.hpp" +#include "tf2/LinearMath/Quaternion.h" + +namespace +{ +constexpr auto DEFAULT_COMMAND_IN_TOPIC = "~/cmd_vel"; +constexpr auto DEFAULT_ODOMETRY_TOPIC = "~/odom"; +constexpr auto DEFAULT_TRANSFORM_TOPIC = "/tf"; +} // namespace + +namespace multi_omni_wheel_drive_controller +{ +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; + +MultiOmniWheelDriveController::MultiOmniWheelDriveController() +: controller_interface::ChainableControllerInterface() +{ +} + +controller_interface::CallbackReturn MultiOmniWheelDriveController::on_init() +{ + try + { + // Create the parameter listener and get the parameters + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn MultiOmniWheelDriveController::on_configure( + const rclcpp_lifecycle::State &) +{ + auto logger = get_node()->get_logger(); + + // update parameters if they have changed + if (param_listener_->is_old(params_)) + { + params_ = param_listener_->get_params(); + RCLCPP_INFO(logger, "Parameters were updated"); + } + + if (params_.wheel_names.size() < 3) + { + RCLCPP_ERROR( + logger, + "The number of wheels is less than 3. If there are more wheels please pass them all to the " + "controller or use a different one for your wheel configuration."); + return controller_interface::CallbackReturn::ERROR; + } + + odometry_.setParams( + params_.robot_radius, params_.wheel_radius, params_.wheel_offset, params_.wheel_names.size()); + + cmd_vel_timeout_ = rclcpp::Duration::from_seconds(params_.cmd_vel_timeout); + + // Allocate reference interfaces if needed + const int nr_ref_itfs = 3; + reference_interfaces_.resize(nr_ref_itfs, std::numeric_limits::quiet_NaN()); + + // Reset the controller + if (!reset()) + { + return controller_interface::CallbackReturn::ERROR; + } + + // Initialize velocity command subscriber and define its callback function + velocity_command_subscriber_ = get_node()->create_subscription( + DEFAULT_COMMAND_IN_TOPIC, rclcpp::SystemDefaultsQoS(), + [this](const std::shared_ptr msg) -> void + { + if (!subscriber_is_active_) + { + RCLCPP_WARN(get_node()->get_logger(), "Can't accept new commands. subscriber is inactive"); + return; + } + if ((msg->header.stamp.sec == 0) && (msg->header.stamp.nanosec == 0)) + { + RCLCPP_WARN_ONCE( + get_node()->get_logger(), + "Received TwistStamped with zero timestamp, setting it to current " + "time, this message will only be shown once"); + msg->header.stamp = get_node()->now(); + } + + const auto current_time_diff = get_node()->now() - msg->header.stamp; + + if ( + cmd_vel_timeout_ == rclcpp::Duration::from_seconds(0.0) || + current_time_diff < cmd_vel_timeout_) + { + received_velocity_msg_ptr_.writeFromNonRT(msg); + } + else + { + RCLCPP_WARN( + get_node()->get_logger(), + "Ignoring the received message (timestamp %.10f) because it is older than " + "the current time by %.10f seconds, which exceeds the allowed timeout (%.4f)", + rclcpp::Time(msg->header.stamp).seconds(), current_time_diff.seconds(), + cmd_vel_timeout_.seconds()); + } + }); + + // Initialize odometry publisher and message + odometry_publisher_ = get_node()->create_publisher( + DEFAULT_ODOMETRY_TOPIC, rclcpp::SystemDefaultsQoS()); + realtime_odometry_publisher_ = + std::make_shared>( + odometry_publisher_); + + // Append the tf prefix if there is one + std::string tf_prefix = ""; + if (params_.tf_frame_prefix_enable) + { + if (params_.tf_frame_prefix != "") + { + tf_prefix = params_.tf_frame_prefix; + } + else + { + tf_prefix = std::string(get_node()->get_namespace()); + } + + // Make sure prefix does not start with '/' and always ends with '/' + if (tf_prefix.back() != '/') + { + tf_prefix = tf_prefix + "/"; + } + if (tf_prefix.front() == '/') + { + tf_prefix.erase(0, 1); + } + } + const auto odom_frame_id = tf_prefix + params_.odom_frame_id; + const auto base_frame_id = tf_prefix + params_.base_frame_id; + + auto & odometry_message = realtime_odometry_publisher_->msg_; + odometry_message.header.frame_id = odom_frame_id; + odometry_message.child_frame_id = base_frame_id; + + // Limit the publication on the topics /odom and /tf + publish_period_ = rclcpp::Duration::from_seconds(1.0 / params_.publish_rate); + + // Initialize odom values zeros + odometry_message.twist = + geometry_msgs::msg::TwistWithCovariance(rosidl_runtime_cpp::MessageInitialization::ALL); + + constexpr size_t NUM_DIMENSIONS = 6; + for (size_t index = 0; index < 6; ++index) + { + // 0, 7, 14, 21, 28, 35 + const 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]; + } + + // Initialize transform publisher and message + odometry_transform_publisher_ = get_node()->create_publisher( + DEFAULT_TRANSFORM_TOPIC, rclcpp::SystemDefaultsQoS()); + realtime_odometry_transform_publisher_ = + std::make_shared>( + odometry_transform_publisher_); + + // Keeping track of odom and base_link transforms only + 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; + + return controller_interface::CallbackReturn::SUCCESS; +} + +InterfaceConfiguration MultiOmniWheelDriveController::command_interface_configuration() const +{ + std::vector conf_names; + for (const auto & joint_name : params_.wheel_names) + { + conf_names.push_back(joint_name + "/" + HW_IF_VELOCITY); + } + return {interface_configuration_type::INDIVIDUAL, conf_names}; +} + +InterfaceConfiguration MultiOmniWheelDriveController::state_interface_configuration() const +{ + std::vector conf_names; + for (const auto & joint_name : params_.wheel_names) + { + conf_names.push_back(joint_name + "/" + feedback_type()); + } + return {interface_configuration_type::INDIVIDUAL, conf_names}; +} + +controller_interface::CallbackReturn MultiOmniWheelDriveController::on_activate( + const rclcpp_lifecycle::State &) +{ + // Register wheel handles and check if any errors happen + if ( + configure_wheel_handles(params_.wheel_names, registered_wheel_handles_) == + controller_interface::CallbackReturn::ERROR) + { + return controller_interface::CallbackReturn::ERROR; + } + + subscriber_is_active_ = true; + + RCLCPP_DEBUG(get_node()->get_logger(), "Subscriber and publisher are now active."); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn MultiOmniWheelDriveController::on_deactivate( + const rclcpp_lifecycle::State &) +{ + subscriber_is_active_ = false; + halt(); + reset_buffers(); + registered_wheel_handles_.clear(); + return controller_interface::CallbackReturn::SUCCESS; +} + +const char * MultiOmniWheelDriveController::feedback_type() const +{ + return params_.position_feedback ? HW_IF_POSITION : HW_IF_VELOCITY; +} + +controller_interface::return_type MultiOmniWheelDriveController::update_reference_from_subscribers( + const rclcpp::Time & time, const rclcpp::Duration &) +{ + auto logger = get_node()->get_logger(); + + const std::shared_ptr command_msg_ptr = *(received_velocity_msg_ptr_.readFromRT()); + + if (command_msg_ptr == nullptr) + { + RCLCPP_WARN(logger, "Velocity message received was a nullptr."); + return controller_interface::return_type::ERROR; + } + + const auto age_of_last_command = time - command_msg_ptr->header.stamp; + // Brake if cmd_vel has timeout, override the stored command + if (age_of_last_command > cmd_vel_timeout_) + { + reference_interfaces_[0] = 0.0; + reference_interfaces_[1] = 0.0; + reference_interfaces_[2] = 0.0; + } + else if ( + std::isfinite(command_msg_ptr->twist.linear.x) && + std::isfinite(command_msg_ptr->twist.angular.z)) + { + reference_interfaces_[0] = command_msg_ptr->twist.linear.x; + reference_interfaces_[1] = command_msg_ptr->twist.linear.y; + reference_interfaces_[2] = command_msg_ptr->twist.angular.z; + } + else + { + RCLCPP_WARN_SKIPFIRST_THROTTLE( + logger, *get_node()->get_clock(), cmd_vel_timeout_.seconds() * 1000, + "Command message contains NaNs. Not updating reference interfaces."); + } + + return controller_interface::return_type::OK; +} + +controller_interface::return_type MultiOmniWheelDriveController::update_and_write_commands( + const rclcpp::Time & time, const rclcpp::Duration &) +{ + rclcpp::Logger logger = get_node()->get_logger(); + + if ( + !std::isfinite(reference_interfaces_[0]) || !std::isfinite(reference_interfaces_[1]) || + !std::isfinite(reference_interfaces_[2])) + { + // NaNs occur on initialization when the reference interfaces are not yet set + return controller_interface::return_type::OK; + } + + // Update odometry + bool odometry_updated = false; + if (params_.open_loop) + { + odometry_updated = odometry_.updateOpenLoop( + reference_interfaces_[0], reference_interfaces_[1], reference_interfaces_[2], time); + } + else + { + std::vector wheels_feedback(registered_wheel_handles_.size()); // [rads] + for (size_t i = 0; i < static_cast(registered_wheel_handles_.size()); ++i) + { + // Get wheel feedback + const std::optional wheel_feedback_op = + registered_wheel_handles_[i].feedback.get().get_optional(); + if (!wheel_feedback_op.has_value()) + { + RCLCPP_DEBUG(logger, "Unable to retrieve data from [%zu] wheel feedback!", i); + return controller_interface::return_type::OK; + } + const double wheel_feedback = wheel_feedback_op.value(); + + if (std::isnan(wheel_feedback)) + { + RCLCPP_ERROR(logger, "The wheel %s is invalid for index [%zu]", feedback_type(), i); + return controller_interface::return_type::ERROR; + } + + wheels_feedback[i] = wheel_feedback; + } + if (params_.position_feedback) + { + odometry_updated = odometry_.updateFromPos(wheels_feedback, time); + } + else + { + odometry_updated = odometry_.updateFromVel(wheels_feedback, time); + } + } + + if (odometry_updated) + { + tf2::Quaternion orientation; + orientation.setRPY(0.0, 0.0, odometry_.getHeading()); + + bool should_publish = true; + try + { + if (time.seconds() - previous_publish_timestamp_.seconds() < publish_period_.seconds()) + { + should_publish = false; + } + } + catch (const std::runtime_error &) + { + // Handle exceptions when the time source changes and initialize publish timestamp + previous_publish_timestamp_ = time; + } + + if (should_publish) + { + if (realtime_odometry_publisher_->trylock()) + { + auto & odometry_message = realtime_odometry_publisher_->msg_; + odometry_message.header.stamp = time; + odometry_message.pose.pose.position.x = odometry_.getX(); + odometry_message.pose.pose.position.y = odometry_.getY(); + 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(); + odometry_message.twist.twist.linear.x = odometry_.getLinearXVel(); + odometry_message.twist.twist.linear.y = odometry_.getLinearYVel(); + odometry_message.twist.twist.angular.z = odometry_.getAngularVel(); + realtime_odometry_publisher_->unlockAndPublish(); + } + if (params_.enable_odom_tf && realtime_odometry_transform_publisher_->trylock()) + { + auto & transform = realtime_odometry_transform_publisher_->msg_.transforms.front(); + transform.header.stamp = time; + transform.transform.translation.x = odometry_.getX(); + transform.transform.translation.y = odometry_.getY(); + 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(); + } + } + } + + compute_and_set_wheel_velocities(); + + return controller_interface::return_type::OK; +} + +controller_interface::CallbackReturn MultiOmniWheelDriveController::on_cleanup( + const rclcpp_lifecycle::State &) +{ + if (!reset()) + { + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn MultiOmniWheelDriveController::on_error( + const rclcpp_lifecycle::State &) +{ + if (!reset()) + { + return controller_interface::CallbackReturn::ERROR; + } + return controller_interface::CallbackReturn::SUCCESS; +} + +void MultiOmniWheelDriveController::compute_and_set_wheel_velocities() +{ + bool set_command_result = true; + + double angle_bw_wheels = (2 * M_PI) / static_cast(registered_wheel_handles_.size()); + for (size_t i = 0; i < static_cast(registered_wheel_handles_.size()); ++i) + { + set_command_result &= registered_wheel_handles_[i].velocity.get().set_value( + ((std::sin((angle_bw_wheels * static_cast(i)) + params_.wheel_offset) * + reference_interfaces_[0]) - + (std::cos((angle_bw_wheels * static_cast(i)) + params_.wheel_offset) * + reference_interfaces_[1]) - + (reference_interfaces_[2] * params_.robot_radius)) / + params_.wheel_radius); + } + + rclcpp::Logger logger = get_node()->get_logger(); + RCLCPP_DEBUG_EXPRESSION( + logger, !set_command_result, "Unable to set the command to one of the command handles!"); +} + +bool MultiOmniWheelDriveController::reset() +{ + odometry_.resetOdometry(); + + reset_buffers(); + + registered_wheel_handles_.clear(); + + subscriber_is_active_ = false; + velocity_command_subscriber_.reset(); + + return true; +} + +controller_interface::CallbackReturn MultiOmniWheelDriveController::configure_wheel_handles( + const std::vector & wheel_names, std::vector & registered_handles) +{ + rclcpp::Logger logger = get_node()->get_logger(); + + // Register handles + registered_handles.reserve(wheel_names.size()); + for (const auto & wheel_name : wheel_names) + { + const auto interface_name = feedback_type(); + const auto state_handle = std::find_if( + state_interfaces_.cbegin(), state_interfaces_.cend(), + [&wheel_name, &interface_name](const auto & interface) + { + return interface.get_prefix_name() == wheel_name && + interface.get_interface_name() == interface_name; + }); + + if (state_handle == state_interfaces_.cend()) + { + RCLCPP_ERROR(logger, "Unable to obtain joint state handle for %s", wheel_name.c_str()); + return controller_interface::CallbackReturn::ERROR; + } + + const auto command_handle = std::find_if( + command_interfaces_.begin(), command_interfaces_.end(), + [&wheel_name](const auto & interface) + { + return interface.get_prefix_name() == wheel_name && + interface.get_interface_name() == HW_IF_VELOCITY; + }); + + if (command_handle == command_interfaces_.end()) + { + RCLCPP_ERROR(logger, "Unable to obtain joint command handle for %s", wheel_name.c_str()); + return controller_interface::CallbackReturn::ERROR; + } + + registered_handles.emplace_back( + WheelHandle{std::ref(*state_handle), std::ref(*command_handle)}); + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +void MultiOmniWheelDriveController::halt() +{ + bool set_command_result = true; + for (const WheelHandle & wheel_handle : registered_wheel_handles_) + { + set_command_result &= wheel_handle.velocity.get().set_value(0.0); + } + rclcpp::Logger logger = get_node()->get_logger(); + RCLCPP_DEBUG_EXPRESSION( + logger, !set_command_result, "Unable to set the command to one of the command handles!"); +} + +void MultiOmniWheelDriveController::reset_buffers() +{ + std::fill( + reference_interfaces_.begin(), reference_interfaces_.end(), + std::numeric_limits::quiet_NaN()); + + // Fill RealtimeBuffer with NaNs so it will contain a known value + // but still indicate that no command has yet been sent. + received_velocity_msg_ptr_.reset(); + std::shared_ptr empty_msg_ptr = std::make_shared(); + empty_msg_ptr->header.stamp = get_node()->now(); + empty_msg_ptr->twist.linear.x = std::numeric_limits::quiet_NaN(); + empty_msg_ptr->twist.linear.y = std::numeric_limits::quiet_NaN(); + empty_msg_ptr->twist.linear.z = std::numeric_limits::quiet_NaN(); + empty_msg_ptr->twist.angular.x = std::numeric_limits::quiet_NaN(); + empty_msg_ptr->twist.angular.y = std::numeric_limits::quiet_NaN(); + empty_msg_ptr->twist.angular.z = std::numeric_limits::quiet_NaN(); + received_velocity_msg_ptr_.writeFromNonRT(empty_msg_ptr); +} + +bool MultiOmniWheelDriveController::on_set_chained_mode(bool chained_mode) +{ + // Always accept switch to/from chained mode (without linting type-cast error) + return true || chained_mode; +} + +std::vector +MultiOmniWheelDriveController::on_export_reference_interfaces() +{ + std::vector reference_interfaces; + reference_interfaces.reserve(reference_interfaces_.size()); + + std::vector reference_interface_names = {"/linear/x", "/linear/y", "/angular/z"}; + + for (size_t i = 0; i < reference_interfaces_.size(); ++i) + { + reference_interfaces.push_back(hardware_interface::CommandInterface( + get_node()->get_name() + reference_interface_names[i], hardware_interface::HW_IF_VELOCITY, + &reference_interfaces_[i])); + } + + return reference_interfaces; +} +} // namespace multi_omni_wheel_drive_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + multi_omni_wheel_drive_controller::MultiOmniWheelDriveController, + controller_interface::ChainableControllerInterface) diff --git a/multi_omni_wheel_drive_controller/src/multi_omni_wheel_drive_controller_parameters.yaml b/multi_omni_wheel_drive_controller/src/multi_omni_wheel_drive_controller_parameters.yaml new file mode 100644 index 0000000000..ba9cff1a3a --- /dev/null +++ b/multi_omni_wheel_drive_controller/src/multi_omni_wheel_drive_controller_parameters.yaml @@ -0,0 +1,94 @@ +multi_omni_wheel_drive_controller: + wheel_offset: + { + type: double, + default_value: 0.0, # radians + description: "Angular offset (radians) of the first wheel from the positive direction of the x-axis of the robot.", + } + wheel_names: + { + type: string_array, + default_value: [], + description: "Names of the wheels' joints, given in an anti-clockwise order starting from the wheel aligned with the positive direction of the x-axis of the robot / offset from it by the value specified in ``wheel_offset``.", + validation: { not_empty<>: [] }, + } + robot_radius: + { + type: double, + default_value: 0.0, + description: "Radius of the robot, distance between the center of the robot and the wheels. If this parameter is wrong, the robot will not behave correctly in curves.", + validation: { gt<>: [0.0] }, + } + wheel_radius: + { + type: double, + default_value: 0.0, + description: "Radius of a wheel, i.e., wheels size, used for transformation of linear velocity into wheel rotations. If this parameter is wrong the robot will move faster or slower then expected.", + validation: { gt<>: [0.0] }, + } + tf_frame_prefix_enable: + { + type: bool, + default_value: true, + description: "Enables or disables appending tf_prefix to tf frame id's.", + } + tf_frame_prefix: + { + type: string, + default_value: "", + description: "(optional) Prefix to be appended to the tf frames, will be added to odom_id and base_frame_id before publishing. If the parameter is empty, controller's namespace will be used.", + } + odom_frame_id: + { + type: string, + default_value: "odom", + description: "Name of the frame for odometry. This frame is parent of ``base_frame_id`` when controller publishes odometry.", + } + base_frame_id: + { + type: string, + default_value: "base_link", + description: "Name of the robot's base frame that is child of the odometry frame.", + } + pose_covariance_diagonal: + { + type: double_array, + default_value: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + description: "Odometry covariance for the encoder output of the robot for the pose. These values should be tuned to your robot's sample odometry data, but these values are a good place to start: ``[0.001, 0.001, 0.001, 0.001, 0.001, 0.01]``.", + } + twist_covariance_diagonal: + { + type: double_array, + default_value: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + description: "Odometry covariance for the encoder output of the robot for the speed. These values should be tuned to your robot's sample odometry data, but these values are a good place to start: ``[0.001, 0.001, 0.001, 0.001, 0.001, 0.01]``.", + } + 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.", + } + position_feedback: + { + type: bool, + default_value: true, + description: "Only valid if ``open_loop`` is set to false. Set to true if there is position feedback from the hardware, set to false if there is velocity feedback instead.", + } + enable_odom_tf: + { + type: bool, + default_value: true, + description: "Publish transformation between ``odom_frame_id`` and ``base_frame_id``.", + } + cmd_vel_timeout: + { + type: double, + default_value: 0.5, # seconds + description: "Timeout in seconds, after which input command on ``~/cmd_vel`` topic is considered staled.", + } + publish_rate: + { + type: double, + default_value: 50.0, # Hz + description: "Publishing rate (Hz) of the odometry and TF messages.", + } diff --git a/multi_omni_wheel_drive_controller/src/odometry.cpp b/multi_omni_wheel_drive_controller/src/odometry.cpp new file mode 100644 index 0000000000..6377e7e76f --- /dev/null +++ b/multi_omni_wheel_drive_controller/src/odometry.cpp @@ -0,0 +1,160 @@ +// Copyright 2025 Aarav Gupta +// +// 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 "multi_omni_wheel_drive_controller/odometry.hpp" + +#include + +namespace multi_omni_wheel_drive_controller +{ +Odometry::Odometry() +: timestamp_(0.0), + x_(0.0), + y_(0.0), + heading_(0.0), + linear_x_vel_(0.0), + linear_y_vel_(0.0), + angular_vel_(0.0), + robot_radius_(0.0), + wheel_radius_(0.0), + wheels_old_pos_(0.0) +{ +} + +bool Odometry::updateFromPos(const std::vector & wheels_pos, const rclcpp::Time & time) +{ + const double dt = time.seconds() - timestamp_.seconds(); + // We cannot estimate angular velocity with very small time intervals + if (std::fabs(dt) < 1e-6) + { + return false; + } + + // Estimate angular velocity of wheels using old and current position [rads/s]: + std::vector wheels_vel(wheels_pos.size()); + for (size_t i = 0; i < static_cast(wheels_pos.size()); ++i) + { + wheels_vel[i] = (wheels_pos[i] - wheels_old_pos_[i]) / dt; + wheels_old_pos_[i] = wheels_pos[i]; + } + + if (updateFromVel(wheels_vel, time)) + { + return true; + } + return false; +} + +bool Odometry::updateFromVel(const std::vector & wheels_vel, const rclcpp::Time & time) +{ + const double dt = time.seconds() - timestamp_.seconds(); + + // Compute linear and angular velocities of the robot: + const Eigen::Vector3d robot_velocity = compute_robot_velocity(wheels_vel); + + // Integrate odometry: + integrate(robot_velocity(0) * dt, robot_velocity(1) * dt, robot_velocity(2) * dt); + + timestamp_ = time; + + linear_x_vel_ = robot_velocity(0); + linear_y_vel_ = robot_velocity(1); + angular_vel_ = robot_velocity(2); + + return true; +} + +Eigen::Vector3d Odometry::compute_robot_velocity(const std::vector & wheels_vel) const +{ + Eigen::MatrixXd A(wheels_vel.size(), 3); // Transformation Matrix + Eigen::VectorXd omega(wheels_vel.size()); // Wheel angular velocities vector + + const double angle_bw_wheels = (2 * M_PI) / static_cast(wheels_vel.size()); + + for (size_t i = 0; i < wheels_vel.size(); ++i) + { + // Define the transformation matrix + const double theta = (angle_bw_wheels * static_cast(i)) + wheel_offset_; + A(static_cast(i), 0) = std::sin(theta); + A(static_cast(i), 1) = -std::cos(theta); + A(static_cast(i), 2) = -robot_radius_; + + // Define the wheel angular velocities vector + omega(static_cast(i)) = wheels_vel[i]; + } + + // Compute the robot velocities using SVD decomposition + const Eigen::Vector3d V = + A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(omega * wheel_radius_); + + return V; +} + +bool Odometry::updateOpenLoop( + const double & linear_x_vel, const double & linear_y_vel, const double & angular_vel, + const rclcpp::Time & time) +{ + const double dt = time.seconds() - timestamp_.seconds(); + + // Integrate odometry: + integrate(linear_x_vel * dt, linear_y_vel * dt, angular_vel * dt); + + timestamp_ = time; + + // Save last linear and angular velocity: + linear_x_vel_ = linear_x_vel; + linear_y_vel_ = linear_y_vel; + angular_vel_ = angular_vel; + + return true; +} + +void Odometry::resetOdometry() +{ + x_ = 0.0; + y_ = 0.0; + heading_ = 0.0; +} + +void Odometry::setParams( + const double & robot_radius, const double & wheel_radius, const double & wheel_offset, + const size_t & wheel_count) +{ + robot_radius_ = robot_radius; + wheel_radius_ = wheel_radius; + wheel_offset_ = wheel_offset; + wheels_old_pos_.resize(wheel_count, 0.0); +} + +void Odometry::integrate(const double & dx, const double & dy, const double & dheading) +{ + if (std::fabs(dheading) < 1e-6) + { + // For very small dheading, approximate to linear motion + x_ = x_ + ((dx * std::cos(heading_)) - (dy * std::sin(heading_))); + y_ = y_ + ((dx * std::sin(heading_)) + (dy * std::cos(heading_))); + heading_ = heading_ + dheading; + } + else + { + const double heading_old = heading_; + heading_ = heading_ + dheading; + x_ = x_ + ((dx / dheading) * (std::sin(heading_) - std::sin(heading_old))) + + ((dy / dheading) * (std::cos(heading_) - std::cos(heading_old))); + y_ = y_ - (dx / dheading) * (std::cos(heading_) - std::cos(heading_old)) + + (dy / dheading) * (std::sin(heading_) - std::sin(heading_old)); + } +} + +} // namespace multi_omni_wheel_drive_controller diff --git a/multi_omni_wheel_drive_controller/test/config/test_multi_omni_wheel_drive_controller.yaml b/multi_omni_wheel_drive_controller/test/config/test_multi_omni_wheel_drive_controller.yaml new file mode 100644 index 0000000000..adfbf56168 --- /dev/null +++ b/multi_omni_wheel_drive_controller/test/config/test_multi_omni_wheel_drive_controller.yaml @@ -0,0 +1,25 @@ +test_multi_omni_wheel_drive_controller: + ros__parameters: + wheel_offset: 0.0 + wheel_names: + [ + "front_wheel_joint", + "left_wheel_joint", + "back_wheel_joint", + "right_wheel_joint", + ] + + robot_radius: 0.20 + wheel_radius: 0.02 + + odom_frame_id: odom + base_frame_id: base_link + pose_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + twist_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + + open_loop: true + position_feedback: false + enable_odom_tf: true + + cmd_vel_timeout: 0.5 + publish_limited_velocity: true diff --git a/multi_omni_wheel_drive_controller/test/test_load_multi_omni_wheel_drive_controller.cpp b/multi_omni_wheel_drive_controller/test/test_load_multi_omni_wheel_drive_controller.cpp new file mode 100644 index 0000000000..b63f4cd275 --- /dev/null +++ b/multi_omni_wheel_drive_controller/test/test_load_multi_omni_wheel_drive_controller.cpp @@ -0,0 +1,50 @@ +// Copyright 2025 Aarav Gupta +// +// 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 "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadMultiOmniWheelDriveController, 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_multi_omni_wheel_drive_controller.yaml"; + + cm.set_parameter({"test_multi_omni_wheel_drive_controller.params_file", test_file_path}); + cm.set_parameter( + {"test_multi_omni_wheel_drive_controller.type", + "multi_omni_wheel_drive_controller/MultiOmniWheelDriveController"}); + + ASSERT_NE(cm.load_controller("test_multi_omni_wheel_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/multi_omni_wheel_drive_controller/test/test_multi_omni_wheel_drive_controller.cpp b/multi_omni_wheel_drive_controller/test/test_multi_omni_wheel_drive_controller.cpp new file mode 100644 index 0000000000..e65fc3305c --- /dev/null +++ b/multi_omni_wheel_drive_controller/test/test_multi_omni_wheel_drive_controller.cpp @@ -0,0 +1,605 @@ +// Copyright 2025 Aarav Gupta +// +// 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_multi_omni_wheel_drive_controller.hpp" +#include +#include "controller_interface/controller_interface_base.hpp" +#include "lifecycle_msgs/msg/state.hpp" + +using lifecycle_msgs::msg::State; + +class MultiOmniWheelDriveControllerTest +: public MultiOmniWheelDriveControllerFixture +{ +}; + +TEST_F(MultiOmniWheelDriveControllerTest, 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::ERROR); +} + +TEST_F(MultiOmniWheelDriveControllerTest, configure_fails_with_less_than_three_wheels) +{ + ASSERT_EQ( + InitController({"first_wheel_joint", "second_wheel_joint"}), + controller_interface::return_type::OK); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); +} + +TEST_F( + MultiOmniWheelDriveControllerTest, when_controller_configured_expect_properly_exported_interfaces) +{ + ASSERT_EQ(InitController(), controller_interface::return_type::OK); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // Check command interfaces configuration + auto command_interfaces = controller_->command_interface_configuration(); + ASSERT_EQ(command_interfaces.names.size(), wheel_names_.size()); + for (size_t i = 0; i < command_interfaces.names.size(); ++i) + { + EXPECT_EQ(command_interfaces.names[i], wheel_names_[i] + "/" + HW_IF_VELOCITY); + } + EXPECT_EQ( + command_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); + + // Check state interfaces configuration + auto state_interfaces = controller_->state_interface_configuration(); + ASSERT_EQ(state_interfaces.names.size(), wheel_names_.size()); + for (size_t i = 0; i < state_interfaces.names.size(); ++i) + { + EXPECT_EQ(state_interfaces.names[i], wheel_names_[i] + "/" + HW_IF_POSITION); + } + EXPECT_EQ(state_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); + + // Check reference interfaces configuration + auto reference_interfaces = controller_->export_reference_interfaces(); + ASSERT_EQ(reference_interfaces.size(), 3); + + for (size_t i = 0; i < reference_interface_names.size(); ++i) + { + const std::string ref_itf_prefix_name = + std::string(controller_->get_node()->get_name()) + "/" + reference_interface_names[i]; + EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), ref_itf_prefix_name); + EXPECT_EQ( + reference_interfaces[i]->get_name(), + ref_itf_prefix_name + "/" + hardware_interface::HW_IF_VELOCITY); + EXPECT_EQ(reference_interfaces[i]->get_interface_name(), hardware_interface::HW_IF_VELOCITY); + } +} + +TEST_F(MultiOmniWheelDriveControllerTest, configure_succeeds_tf_test_prefix_false_no_namespace) +{ + std::string odom_id = "odom"; + std::string base_link_id = "base_link"; + std::string frame_prefix = "test_prefix"; + + ASSERT_EQ( + InitController( + wheel_names_, {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(false)), + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}), + controller_interface::return_type::OK); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto odometry_message = controller_->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; + /* tf_frame_prefix_enable is false so no modifications to the frame id's */ + ASSERT_EQ(test_odom_frame_id, odom_id); + ASSERT_EQ(test_base_frame_id, base_link_id); +} + +TEST_F(MultiOmniWheelDriveControllerTest, configure_succeeds_tf_test_prefix_true_no_namespace) +{ + std::string odom_id = "odom"; + std::string base_link_id = "base_link"; + std::string frame_prefix = "test_prefix"; + + ASSERT_EQ( + InitController( + wheel_names_, {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true)), + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}), + controller_interface::return_type::OK); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto odometry_message = controller_->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; + + /* tf_frame_prefix_enable is true and frame_prefix is not blank so should be appended to the frame + * id's */ + ASSERT_EQ(test_odom_frame_id, frame_prefix + "/" + odom_id); + ASSERT_EQ(test_base_frame_id, frame_prefix + "/" + base_link_id); +} + +TEST_F(MultiOmniWheelDriveControllerTest, configure_succeeds_tf_blank_prefix_true_no_namespace) +{ + std::string odom_id = "odom"; + std::string base_link_id = "base_link"; + std::string frame_prefix = ""; + + ASSERT_EQ( + InitController( + wheel_names_, {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true)), + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}), + controller_interface::return_type::OK); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto odometry_message = controller_->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; + /* tf_frame_prefix_enable is true but frame_prefix is blank so should not be appended to the frame + * id's */ + ASSERT_EQ(test_odom_frame_id, odom_id); + ASSERT_EQ(test_base_frame_id, base_link_id); +} + +TEST_F(MultiOmniWheelDriveControllerTest, configure_succeeds_tf_test_prefix_false_set_namespace) +{ + std::string test_namespace = "/test_namespace"; + + std::string odom_id = "odom"; + std::string base_link_id = "base_link"; + std::string frame_prefix = "test_prefix"; + + ASSERT_EQ( + InitController( + wheel_names_, + {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(false)), + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}, + test_namespace), + controller_interface::return_type::OK); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto odometry_message = controller_->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; + /* tf_frame_prefix_enable is false so no modifications to the frame id's */ + ASSERT_EQ(test_odom_frame_id, odom_id); + ASSERT_EQ(test_base_frame_id, base_link_id); +} + +TEST_F(MultiOmniWheelDriveControllerTest, configure_succeeds_tf_test_prefix_true_set_namespace) +{ + std::string test_namespace = "/test_namespace"; + + std::string odom_id = "odom"; + std::string base_link_id = "base_link"; + std::string frame_prefix = "test_prefix"; + + ASSERT_EQ( + InitController( + wheel_names_, + {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true)), + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}, + test_namespace), + controller_interface::return_type::OK); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto odometry_message = controller_->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; + + /* tf_frame_prefix_enable is true and frame_prefix is not blank so should be appended to the frame + * id's instead of the namespace*/ + ASSERT_EQ(test_odom_frame_id, frame_prefix + "/" + odom_id); + ASSERT_EQ(test_base_frame_id, frame_prefix + "/" + base_link_id); +} + +TEST_F(MultiOmniWheelDriveControllerTest, configure_succeeds_tf_blank_prefix_true_set_namespace) +{ + std::string test_namespace = "/test_namespace"; + std::string odom_id = "odom"; + std::string base_link_id = "base_link"; + std::string frame_prefix = ""; + + ASSERT_EQ( + InitController( + wheel_names_, + {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true)), + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}, + test_namespace), + controller_interface::return_type::OK); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto odometry_message = controller_->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; + std::string ns_prefix = test_namespace.erase(0, 1) + "/"; + /* tf_frame_prefix_enable is true but frame_prefix is blank so namespace should be appended to the + * frame id's */ + ASSERT_EQ(test_odom_frame_id, ns_prefix + odom_id); + ASSERT_EQ(test_base_frame_id, ns_prefix + base_link_id); +} + +TEST_F(MultiOmniWheelDriveControllerTest, 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(MultiOmniWheelDriveControllerTest, activate_succeeds_with_pos_resources_assigned) +{ + ASSERT_EQ(InitController(), controller_interface::return_type::OK); + // We implicitly test that by default position feedback is required + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + assignResourcesPosFeedback(); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); +} + +TEST_F(MultiOmniWheelDriveControllerTest, activate_succeeds_with_vel_resources_assigned) +{ + ASSERT_EQ( + InitController( + wheel_names_, {rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(false))}), + controller_interface::return_type::OK); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + assignResourcesVelFeedback(); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); +} + +TEST_F(MultiOmniWheelDriveControllerTest, activate_fails_with_wrong_resources_assigned_1) +{ + ASSERT_EQ( + InitController( + wheel_names_, {rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(false))}), + controller_interface::return_type::OK); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + assignResourcesPosFeedback(); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_ERROR); +} + +TEST_F(MultiOmniWheelDriveControllerTest, activate_fails_with_wrong_resources_assigned_2) +{ + ASSERT_EQ( + InitController( + wheel_names_, {rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(true))}), + controller_interface::return_type::OK); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + assignResourcesVelFeedback(); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_ERROR); +} + +// When not in chained mode, we want to test that +// 1. The controller is configurable and all lifecycle functions work properly +// 2. command_interfaces are set to 0.0 when cmd_vel_timeout_ is exceeded and on deactivation +// 3. command_interfaces are set to correct command values the command messages are not timed-out. +// In particular, make sure that the command_interface is not set to NaN right when it starts up. +TEST_F(MultiOmniWheelDriveControllerTest, chainable_controller_unchained_mode) +{ + ASSERT_EQ(InitController(), controller_interface::return_type::OK); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_TRUE(controller_->is_chainable()); + ASSERT_TRUE(controller_->set_chained_mode(false)); + ASSERT_FALSE(controller_->is_in_chained_mode()); + + auto state = controller_->get_node()->configure(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + assignResourcesPosFeedback(); + + state = controller_->get_node()->activate(); + ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + + waitForSetup(); + + // Reference interfaces should be NaN on initialization + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + // But NaNs should not propagate to command interfaces + for (size_t i = 0; i < command_itfs_.size(); i++) + { + ASSERT_FALSE(std::isnan(command_itfs_[i].get_optional().value())); + } + + // Check that a late command message causes the command interfaces to be set to 0.0 + publish_twist(); + + // delay enough time to trigger the timeout (cmd_vel_timeout_ = 0.5s) + controller_->wait_for_twist(executor); + std::this_thread::sleep_for(std::chrono::milliseconds(501)); + + ASSERT_EQ( + controller_->update( + cmd_vel_publisher_node_->get_clock()->now(), 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); + } + + // Now check that a timely published command message sets the command interfaces to the correct + // values + publish_twist(); + // wait for msg to be published to the system + 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_wheels_vel_cmds = {-15.0, 5.0, 5.0, -15.0}; + for (size_t i = 0; i < command_itfs_.size(); i++) + { + EXPECT_DOUBLE_EQ(command_itfs_[i].get_optional().value(), expected_wheels_vel_cmds[i]); + } + + // Now check that the command interfaces are set to 0.0 on deactivation + std::this_thread::sleep_for(std::chrono::milliseconds(300)); + state = controller_->get_node()->deactivate(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); + for (size_t i = 0; i < command_itfs_.size(); i++) + { + EXPECT_EQ(command_itfs_[i].get_optional().value(), 0.0); + } + + // cleanup + state = controller_->get_node()->cleanup(); + ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); + for (size_t i = 0; i < command_itfs_.size(); i++) + { + EXPECT_EQ(command_itfs_[i].get_optional().value(), 0.0); + } + + state = controller_->get_node()->configure(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + executor.cancel(); +} + +// When in chained mode, we want to test that +// 1. The controller is configurable and all lifecycle functions work properly +// 2. command_interfaces are set to 0.0 on deactivation +// 3. command_interfaces are set to correct command values (not set to NaN right when it starts up) +TEST_F(MultiOmniWheelDriveControllerTest, chainable_controller_chained_mode) +{ + ASSERT_EQ(InitController(), controller_interface::return_type::OK); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_TRUE(controller_->is_chainable()); + ASSERT_TRUE(controller_->set_chained_mode(true)); + ASSERT_TRUE(controller_->is_in_chained_mode()); + + auto state = controller_->get_node()->configure(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + assignResourcesPosFeedback(); + + state = controller_->get_node()->activate(); + ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + + waitForSetup(); + + // Reference interfaces should be NaN on initialization + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + // But NaNs should not propagate to command interfaces + for (size_t i = 0; i < command_itfs_.size(); i++) + { + ASSERT_FALSE(std::isnan(command_itfs_[i].get_optional().value())); + } + + // Imitate preceding controllers by setting reference_interfaces_ + controller_->reference_interfaces_[0] = 1.0; + controller_->reference_interfaces_[1] = 1.0; + controller_->reference_interfaces_[2] = 1.0; + + 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_wheels_vel_cmds = {-15.0, 5.0, 5.0, -15.0}; + for (size_t i = 0; i < command_itfs_.size(); i++) + { + EXPECT_DOUBLE_EQ(command_itfs_[i].get_optional().value(), expected_wheels_vel_cmds[i]); + } + + // Now check that the command interfaces are set to 0.0 on deactivation + std::this_thread::sleep_for(std::chrono::milliseconds(300)); + state = controller_->get_node()->deactivate(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); + for (size_t i = 0; i < command_itfs_.size(); i++) + { + EXPECT_EQ(command_itfs_[i].get_optional().value(), 0.0); + } + + // cleanup + state = controller_->get_node()->cleanup(); + ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); + for (size_t i = 0; i < command_itfs_.size(); i++) + { + EXPECT_EQ(command_itfs_[i].get_optional().value(), 0.0); + } + + state = controller_->get_node()->configure(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + executor.cancel(); +} + +// Make sure that the controller is properly reset when deactivated +// and accepts new commands as expected when it is activated again. +TEST_F(MultiOmniWheelDriveControllerTest, deactivate_then_activate) +{ + ASSERT_EQ(InitController(), controller_interface::return_type::OK); + // choose radius = 1 so that the command values (rev/s) are the same as the linear velocity (m/s) + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_TRUE(controller_->set_chained_mode(false)); + + auto state = controller_->get_node()->configure(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + assignResourcesPosFeedback(); + + state = controller_->get_node()->activate(); + ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + + waitForSetup(); + + // Reference interfaces should be NaN on initialization + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + // But NaNs should not propagate to command interfaces + for (size_t i = 0; i < command_itfs_.size(); i++) + { + ASSERT_FALSE(std::isnan(command_itfs_[i].get_optional().value())); + } + + // Now check that a timely published command message sets the command interfaces to the correct + // values + publish_twist(); + // wait for msg is be published to the system + 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_wheels_vel_cmds = {-15.0, 5.0, 5.0, -15.0}; + for (size_t i = 0; i < command_itfs_.size(); i++) + { + EXPECT_DOUBLE_EQ(command_itfs_[i].get_optional().value(), expected_wheels_vel_cmds[i]); + } + + // Now check that the command interfaces are set to 0.0 on deactivation + // (despite calls to update()) + 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); + } + + // Activate again + state = controller_->get_node()->activate(); + ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + + waitForSetup(); + + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)) + << "Reference interfaces should initially be NaN on activation"; + } + for (size_t i = 0; i < command_itfs_.size(); i++) + { + EXPECT_EQ(command_itfs_[i].get_optional().value(), 0.0); + } + + // A new command should work as expected + publish_twist(); + 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 < command_itfs_.size(); i++) + { + EXPECT_DOUBLE_EQ(command_itfs_[i].get_optional().value(), expected_wheels_vel_cmds[i]); + } + + // Deactivate again 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.id(), State::PRIMARY_STATE_UNCONFIGURED); + executor.cancel(); +} + +TEST_F(MultiOmniWheelDriveControllerTest, 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()); + + ASSERT_TRUE(controller_->set_chained_mode(false)); + + auto state = controller_->get_node()->configure(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + assignResourcesPosFeedback(); + + state = controller_->get_node()->activate(); + ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + + waitForSetup(); + + // published command message with zero timestamp sets the command interfaces to the correct values + publish_twist_timestamped(rclcpp::Time(0, 0, RCL_ROS_TIME)); + // wait for msg is be published to the system + 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_wheels_vel_cmds = {-15.0, 5.0, 5.0, -15.0}; + for (size_t i = 0; i < command_itfs_.size(); i++) + { + EXPECT_DOUBLE_EQ(command_itfs_[i].get_optional().value(), expected_wheels_vel_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.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; +} diff --git a/multi_omni_wheel_drive_controller/test/test_multi_omni_wheel_drive_controller.hpp b/multi_omni_wheel_drive_controller/test/test_multi_omni_wheel_drive_controller.hpp new file mode 100644 index 0000000000..ec22828806 --- /dev/null +++ b/multi_omni_wheel_drive_controller/test/test_multi_omni_wheel_drive_controller.hpp @@ -0,0 +1,245 @@ +// Copyright 2025 Aarav Gupta +// +// 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_MULTI_OMNI_WHEEL_DRIVE_CONTROLLER_HPP_ +#define TEST_MULTI_OMNI_WHEEL_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 "multi_omni_wheel_drive_controller/multi_omni_wheel_drive_controller.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors.hpp" +#include "rclcpp/parameter_value.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/utilities.hpp" + +using hardware_interface::HW_IF_POSITION; +using hardware_interface::HW_IF_VELOCITY; + +namespace +{ +constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; +constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; +std::vector wheel_names_ = { + "front_wheel_joint", "left_wheel_joint", "back_wheel_joint", "right_wheel_joint"}; +} // namespace + +// subclassing and friending so we can access member variables +class TestableMultiOmniWheelDriveController +: public multi_omni_wheel_drive_controller::MultiOmniWheelDriveController +{ + FRIEND_TEST( + MultiOmniWheelDriveControllerTest, configure_succeeds_tf_test_prefix_false_no_namespace); + FRIEND_TEST( + MultiOmniWheelDriveControllerTest, configure_succeeds_tf_test_prefix_true_no_namespace); + FRIEND_TEST( + MultiOmniWheelDriveControllerTest, configure_succeeds_tf_blank_prefix_true_no_namespace); + FRIEND_TEST( + MultiOmniWheelDriveControllerTest, configure_succeeds_tf_test_prefix_false_set_namespace); + FRIEND_TEST( + MultiOmniWheelDriveControllerTest, configure_succeeds_tf_test_prefix_true_set_namespace); + FRIEND_TEST( + MultiOmniWheelDriveControllerTest, configure_succeeds_tf_blank_prefix_true_set_namespace); + FRIEND_TEST(MultiOmniWheelDriveControllerTest, cleanup); + FRIEND_TEST(MultiOmniWheelDriveControllerTest, chainable_controller_unchained_mode); + FRIEND_TEST(MultiOmniWheelDriveControllerTest, chainable_controller_chained_mode); + FRIEND_TEST(MultiOmniWheelDriveControllerTest, deactivate_then_activate); + FRIEND_TEST( + MultiOmniWheelDriveControllerTest, command_with_zero_timestamp_is_accepted_with_warning); + + /** + * @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)); + } + } +}; + +// We are using template class here for easier reuse of Fixture in specializations of controllers +template +class MultiOmniWheelDriveControllerFixture : public ::testing::Test +{ +public: + void SetUp() + { + // Initialize controller + controller_ = std::make_unique(); + + cmd_vel_publisher_node_ = std::make_shared("cmd_vel_publisher"); + cmd_vel_publisher_ = + cmd_vel_publisher_node_->create_publisher( + "/test_multi_omni_wheel_drive_controller/cmd_vel", rclcpp::SystemDefaultsQoS()); + } + + 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 assignResourcesPosFeedback() + { + std::vector state_ifs; + state_ifs.reserve(wheels_pos_states_.size()); + state_itfs_.reserve(wheels_pos_states_.size()); + for (size_t i = 0; i < wheels_pos_states_.size(); ++i) + { + state_itfs_.emplace_back(hardware_interface::StateInterface( + wheel_names_[i], HW_IF_POSITION, &wheels_pos_states_[i])); + state_ifs.emplace_back(state_itfs_.back()); + } + + std::vector command_ifs; + command_ifs.reserve(wheels_vel_cmds_.size()); + command_itfs_.reserve(wheels_vel_cmds_.size()); + for (size_t i = 0; i < wheels_vel_cmds_.size(); ++i) + { + command_itfs_.emplace_back(hardware_interface::CommandInterface( + wheel_names_[i], HW_IF_VELOCITY, &wheels_vel_cmds_[i])); + command_ifs.emplace_back(command_itfs_.back()); + } + + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + } + + void assignResourcesVelFeedback() + { + std::vector state_ifs; + state_ifs.reserve(wheels_vel_states_.size()); + state_itfs_.reserve(wheels_vel_states_.size()); + for (size_t i = 0; i < wheels_vel_states_.size(); ++i) + { + state_itfs_.emplace_back(hardware_interface::StateInterface( + wheel_names_[i], HW_IF_VELOCITY, &wheels_vel_states_[i])); + state_ifs.emplace_back(state_itfs_.back()); + } + + std::vector command_ifs; + command_ifs.reserve(wheels_vel_cmds_.size()); + command_itfs_.reserve(wheels_vel_cmds_.size()); + for (size_t i = 0; i < wheels_vel_cmds_.size(); ++i) + { + command_itfs_.emplace_back(hardware_interface::CommandInterface( + wheel_names_[i], HW_IF_VELOCITY, &wheels_vel_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_init = wheel_names_, + const std::vector & parameters = {}, const std::string ns = "") + { + auto node_options = rclcpp::NodeOptions(); + std::vector parameter_overrides; + + parameter_overrides.push_back( + rclcpp::Parameter("wheel_names", rclcpp::ParameterValue(wheel_joints_init))); + parameter_overrides.push_back(rclcpp::Parameter("robot_radius", rclcpp::ParameterValue(0.5))); + parameter_overrides.push_back(rclcpp::Parameter("wheel_radius", rclcpp::ParameterValue(0.1))); + + parameter_overrides.insert(parameter_overrides.end(), parameters.begin(), parameters.end()); + node_options.parameter_overrides(parameter_overrides); + + return controller_->init(controller_name_, urdf_, 0, ns, node_options); + } + + std::vector wheels_pos_states_ = {1, 1, 1, 1}; + std::vector wheels_vel_states_ = {1, 1, 1, 1}; + std::vector wheels_vel_cmds_ = {0.1, 0.2, 0.3, 0.4}; + + std::vector state_itfs_; + std::vector command_itfs_; + + std::vector reference_interface_names = {"linear/x", "linear/y", "angular/z"}; + + std::unique_ptr controller_; + rclcpp::Node::SharedPtr cmd_vel_publisher_node_; + rclcpp::Publisher::SharedPtr cmd_vel_publisher_; + + const std::string urdf_ = ""; + std::string controller_name_ = "test_multi_omni_wheel_drive_controller"; +}; + +#endif // TEST_MULTI_OMNI_WHEEL_DRIVE_CONTROLLER_HPP_ diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 5d7f9ba011..5136f54b6a 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -30,6 +30,7 @@ joint_state_broadcaster joint_trajectory_controller mecanum_drive_controller + multi_omni_wheel_drive_controller parallel_gripper_controller pid_controller pose_broadcaster