From de0725f9587ae4a9d7d6851ff2c51a81d5035a6f Mon Sep 17 00:00:00 2001 From: Aarav Gupta Date: Wed, 12 Feb 2025 13:16:48 +0530 Subject: [PATCH 01/18] Add multi_omni_wheel_drive_controller --- doc/controllers_index.rst | 1 + doc/release_notes.rst | 4 + .../CMakeLists.txt | 99 +++ .../doc/userdoc.rst | 77 ++ .../multi_omni_wheel_drive_controller.hpp | 128 ++++ .../odometry.hpp | 75 ++ .../multi_omni_wheel_drive_plugin.xml | 9 + multi_omni_wheel_drive_controller/package.xml | 35 + .../src/multi_omni_wheel_drive_controller.cpp | 578 ++++++++++++++ ...mni_wheel_drive_controller_parameters.yaml | 92 +++ .../src/odometry.cpp | 160 ++++ ...est_multi_omni_wheel_drive_controller.yaml | 25 + ...load_multi_omni_wheel_drive_controller.cpp | 50 ++ ...test_multi_omni_wheel_drive_controller.cpp | 706 ++++++++++++++++++ ...test_multi_omni_wheel_drive_controller.hpp | 248 ++++++ ros2_controllers/package.xml | 1 + 16 files changed, 2288 insertions(+) create mode 100644 multi_omni_wheel_drive_controller/CMakeLists.txt create mode 100644 multi_omni_wheel_drive_controller/doc/userdoc.rst create mode 100644 multi_omni_wheel_drive_controller/include/multi_omni_wheel_drive_controller/multi_omni_wheel_drive_controller.hpp create mode 100644 multi_omni_wheel_drive_controller/include/multi_omni_wheel_drive_controller/odometry.hpp create mode 100644 multi_omni_wheel_drive_controller/multi_omni_wheel_drive_plugin.xml create mode 100644 multi_omni_wheel_drive_controller/package.xml create mode 100644 multi_omni_wheel_drive_controller/src/multi_omni_wheel_drive_controller.cpp create mode 100644 multi_omni_wheel_drive_controller/src/multi_omni_wheel_drive_controller_parameters.yaml create mode 100644 multi_omni_wheel_drive_controller/src/odometry.cpp create mode 100644 multi_omni_wheel_drive_controller/test/config/test_multi_omni_wheel_drive_controller.yaml create mode 100644 multi_omni_wheel_drive_controller/test/test_load_multi_omni_wheel_drive_controller.cpp create mode 100644 multi_omni_wheel_drive_controller/test/test_multi_omni_wheel_drive_controller.cpp create mode 100644 multi_omni_wheel_drive_controller/test/test_multi_omni_wheel_drive_controller.hpp 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 74cd0402f1..dfbb888006 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -57,6 +57,10 @@ mecanum_drive_controller ************************ * 🚀 The mecanum_drive_controller was added 🎉 (`#512 `_). +multi_omni_wheel_drive_controller +********************************* +* 🚀 The multi_omni_wheel_drive_controller was added 🎉 (`#512 `_). + 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..1f5006ea47 --- /dev/null +++ b/multi_omni_wheel_drive_controller/CMakeLists.txt @@ -0,0 +1,99 @@ +cmake_minimum_required(VERSION 3.16) +project(multi_omni_wheel_drive_controller) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) +endif() + +# 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..1cc359856b --- /dev/null +++ b/multi_omni_wheel_drive_controller/doc/userdoc.rst @@ -0,0 +1,77 @@ +: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. +The positive direction of the axis of rotation for each wheel should be facing outwards from the center of the robot. + +As input it takes velocity commands for the robot body, which are translated to wheel commands. + +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). + +Feedback +,,,,,,,, + +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. + +Output +,,,,,, + +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_parameter.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..abed70aa31 --- /dev/null +++ b/multi_omni_wheel_drive_controller/include/multi_omni_wheel_drive_controller/multi_omni_wheel_drive_controller.hpp @@ -0,0 +1,128 @@ +// 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 + +#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 is_halted_ = false; + + 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; + + rclcpp::Time previous_update_timestamp_{0}; + + // publish rate limiter + double publish_rate_ = 50.0; + rclcpp::Duration publish_period_ = rclcpp::Duration::from_nanoseconds(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..1c0a72ba0b --- /dev/null +++ b/multi_omni_wheel_drive_controller/include/multi_omni_wheel_drive_controller/odometry.hpp @@ -0,0 +1,75 @@ +// 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(); + + void init(const rclcpp::Time & time); + bool update(const std::vector & wheels_pos, const rclcpp::Time & time); + bool updateFromVelocity(const std::vector & wheels_vel, const rclcpp::Time & time); + void 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_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_; // [rad] + + // Current velocity: + double linear_x_vel_; // [m/s] + double linear_y_vel_; // [m/s] + double angular_vel_; // [rad/s] + + // Robot kinematic parameters: + double robot_radius_; // [m] + double wheel_offset_; // [rad] + + // Previous wheel positions/states [m]: + 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..130870ea2c --- /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 + + controller_interface + eigen + generate_parameter_library + geometry_msgs + hardware_interface + nav_msgs + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + tf2 + tf2_msgs + + ament_lint_auto + ament_lint_common + ament_add_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..cb07921262 --- /dev/null +++ b/multi_omni_wheel_drive_controller/src/multi_omni_wheel_drive_controller.cpp @@ -0,0 +1,578 @@ +// 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/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_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_rate_ = params_.publish_rate; + publish_period_ = rclcpp::Duration::from_seconds(1.0 / 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; + + previous_update_timestamp_ = get_node()->get_clock()->now(); + 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; + } + + is_halted_ = false; + 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; + if (!is_halted_) + { + halt(); + is_halted_ = true; + } + 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(); + if (get_lifecycle_state().id() == State::PRIMARY_STATE_INACTIVE) + { + if (!is_halted_) + { + halt(); + is_halted_ = true; + } + return controller_interface::return_type::OK; + } + + 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."); + } + + previous_update_timestamp_ = time; + + return controller_interface::return_type::OK; +} + +controller_interface::return_type MultiOmniWheelDriveController::update_and_write_commands( + const rclcpp::Time & time, const rclcpp::Duration &) +{ + auto logger = get_node()->get_logger(); + if (get_lifecycle_state().id() == State::PRIMARY_STATE_INACTIVE) + { + if (!is_halted_) + { + halt(); + is_halted_ = true; + } + return controller_interface::return_type::OK; + } + + 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; + } + + if (params_.open_loop) + { + odometry_.updateOpenLoop( + reference_interfaces_[0], reference_interfaces_[1], reference_interfaces_[2], time); + } + else + { + // [m] + std::vector wheels_feedback(registered_wheel_handles_.size()); + for (size_t i = 0; i < static_cast(registered_wheel_handles_.size()); ++i) + { + const double wheel_feedback = registered_wheel_handles_[i].feedback.get().get_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 * params_.wheel_radius; + } + if (params_.position_feedback) + { + odometry_.update(wheels_feedback, time); + } + else + { + odometry_.updateFromVelocity(wheels_feedback, time); + } + } + + tf2::Quaternion orientation; + orientation.setRPY(0.0, 0.0, odometry_.getHeading()); + + bool should_publish = false; + try + { + if (previous_publish_timestamp_ + publish_period_ < time) + { + previous_publish_timestamp_ += publish_period_; + should_publish = true; + } + } + catch (const std::runtime_error &) + { + // Handle exceptions when the time source changes and initialize publish timestamp + previous_publish_timestamp_ = time; + should_publish = true; + } + + 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() +{ + 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) + { + 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); + } +} + +bool MultiOmniWheelDriveController::reset() +{ + odometry_.resetOdometry(); + + reset_buffers(); + + registered_wheel_handles_.clear(); + + subscriber_is_active_ = false; + velocity_command_subscriber_.reset(); + + is_halted_ = false; + return true; +} + +controller_interface::CallbackReturn MultiOmniWheelDriveController::configure_wheel_handles( + const std::vector & wheel_names, std::vector & registered_handles) +{ + auto 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() +{ + for (const WheelHandle & wheel_handle : registered_wheel_handles_) + { + wheel_handle.velocity.get().set_value(0.0); + } +} + +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()); + + reference_interfaces.push_back(hardware_interface::CommandInterface( + get_node()->get_name(), std::string("linear/x/") + hardware_interface::HW_IF_VELOCITY, + &reference_interfaces_[0])); + + reference_interfaces.push_back(hardware_interface::CommandInterface( + get_node()->get_name(), std::string("linear/y/") + hardware_interface::HW_IF_VELOCITY, + &reference_interfaces_[1])); + + reference_interfaces.push_back(hardware_interface::CommandInterface( + get_node()->get_name(), std::string("angular/z/") + hardware_interface::HW_IF_VELOCITY, + &reference_interfaces_[2])); + + 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..6dfc46778b --- /dev/null +++ b/multi_omni_wheel_drive_controller/src/multi_omni_wheel_drive_controller_parameters.yaml @@ -0,0 +1,92 @@ +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..76ea03458d --- /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 +#include "tf2/LinearMath/Matrix3x3.hpp" +#include "tf2/LinearMath/Quaternion.hpp" + +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), + wheels_old_pos_(0.0) +{ +} + +void Odometry::init(const rclcpp::Time & time) +{ + // Reset timestamp: + timestamp_ = time; +} + +bool Odometry::update(const std::vector & wheels_pos, const rclcpp::Time & time) +{ + // We cannot estimate the speed with very small time intervals: + const double dt = time.seconds() - timestamp_.seconds(); + if (dt < 0.0001) + { + return false; // Interval too small to integrate with + } + + // Estimate velocity of wheels using old and current position [m/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]; + } + + updateFromVelocity(wheels_vel, time); + return true; +} + +bool Odometry::updateFromVelocity(const std::vector & wheels_vel, const rclcpp::Time & time) +{ + const double dt = time.seconds() - timestamp_.seconds(); + if (dt < 0.0001) + { + return false; // Interval too small to integrate with + } + + // 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 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 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); + + return V; +} + +void Odometry::updateOpenLoop( + const double & linear_x_vel, const double & linear_y_vel, const double & angular_vel, + const rclcpp::Time & time) +{ + // Save last linear and angular velocity: + linear_x_vel_ = linear_x_vel; + linear_y_vel_ = linear_y_vel; + angular_vel_ = angular_vel; + + // Integrate odometry: + const double dt = time.seconds() - timestamp_.seconds(); + timestamp_ = time; + integrate(linear_x_vel * dt, linear_y_vel * dt, angular_vel * dt); +} + +void Odometry::resetOdometry() +{ + x_ = 0.0; + y_ = 0.0; + heading_ = 0.0; +} + +void Odometry::setParams( + const double & robot_radius, const double & wheel_offset, const size_t & wheel_count) +{ + robot_radius_ = robot_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) +{ + heading_ = heading_ + dheading; + + // Create a quaternion representing the rotation from the base frame to the odom frame + tf2::Quaternion q_base_to_odom; + q_base_to_odom.setRPY(0.0, 0.0, heading_); + // Create a rotation matrix from the quaternion + const tf2::Matrix3x3 rot_base_to_odom = tf2::Matrix3x3(q_base_to_odom); + + // Define the displacement vector in the base frame + const tf2::Vector3 displacement_base(dx, dy, 0.0); + // Rotate the displacement in the base frame into the odom frame + const tf2::Vector3 displacement_odom = rot_base_to_odom * displacement_base; + + x_ = x_ + displacement_odom.x(); + y_ = y_ + displacement_odom.y(); +} + +} // 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..ce820c1a81 --- /dev/null +++ b/multi_omni_wheel_drive_controller/test/test_multi_omni_wheel_drive_controller.cpp @@ -0,0 +1,706 @@ +// 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_name = std::string(controller_->get_node()->get_name()) + + std::string("/") + reference_interface_names[i]; + EXPECT_EQ(reference_interfaces[i]->get_name(), ref_itf_name); + EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(reference_interfaces[i]->get_interface_name(), reference_interface_names[i]); + } +} + +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); +} + +TEST_F(MultiOmniWheelDriveControllerTest, cleanup) +{ + ASSERT_EQ(InitController(), controller_interface::return_type::OK); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + auto state = controller_->get_node()->configure(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + assignResourcesPosFeedback(); + + state = controller_->get_node()->activate(); + ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + + waitForSetup(); + + 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); + + state = controller_->get_node()->deactivate(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + state = controller_->get_node()->cleanup(); + ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); + + // Wheels should be stopped + double value; + for (size_t i = 0; i < command_itfs_.size(); i++) + { + if (command_itfs_[i].get_value(value)) + { + EXPECT_EQ(value, 0.0); + } + } + + executor.cancel(); +} + +// 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 + double value; + for (size_t i = 0; i < command_itfs_.size(); i++) + { + if (command_itfs_[i].get_value(value)) + { + ASSERT_FALSE(std::isnan(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++) + { + if (command_itfs_[i].get_value(value)) + { + EXPECT_EQ(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++) + { + if (command_itfs_[i].get_value(value)) + { + EXPECT_DOUBLE_EQ(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++) + { + if (command_itfs_[i].get_value(value)) + { + EXPECT_EQ(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++) + { + if (command_itfs_[i].get_value(value)) + { + EXPECT_EQ(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 + double value; + for (size_t i = 0; i < command_itfs_.size(); i++) + { + if (command_itfs_[i].get_value(value)) + { + ASSERT_FALSE(std::isnan(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++) + { + if (command_itfs_[i].get_value(value)) + { + EXPECT_DOUBLE_EQ(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++) + { + if (command_itfs_[i].get_value(value)) + { + EXPECT_EQ(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++) + { + if (command_itfs_[i].get_value(value)) + { + EXPECT_EQ(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 + double value; + for (size_t i = 0; i < command_itfs_.size(); i++) + { + if (command_itfs_[i].get_value(value)) + { + ASSERT_FALSE(std::isnan(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++) + { + if (command_itfs_[i].get_value(value)) + { + EXPECT_DOUBLE_EQ(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++) + { + if (command_itfs_[i].get_value(value)) + { + EXPECT_EQ(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++) + { + if (command_itfs_[i].get_value(value)) + { + EXPECT_EQ(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++) + { + if (command_itfs_[i].get_value(value)) + { + EXPECT_DOUBLE_EQ(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); + double value; + std::vector expected_wheels_vel_cmds = {-15.0, 5.0, 5.0, -15.0}; + for (size_t i = 0; i < command_itfs_.size(); i++) + { + if (command_itfs_[i].get_value(value)) + { + EXPECT_DOUBLE_EQ(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..c32d6cb30c --- /dev/null +++ b/multi_omni_wheel_drive_controller/test/test_multi_omni_wheel_drive_controller.hpp @@ -0,0 +1,248 @@ +// 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 +#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 = { + std::string("linear/x/") + HW_IF_VELOCITY, std::string("linear/y/") + HW_IF_VELOCITY, + std::string("angular/z/") + HW_IF_VELOCITY}; + + 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 4495828bff..094fc1510a 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 From cddfb9c1df8d875cbb9146266510234ad540b798 Mon Sep 17 00:00:00 2001 From: Aarav Gupta Date: Wed, 12 Feb 2025 13:59:30 +0530 Subject: [PATCH 02/18] Fix typo --- doc/release_notes.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/release_notes.rst b/doc/release_notes.rst index dfbb888006..67e52baa7d 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -59,7 +59,7 @@ mecanum_drive_controller multi_omni_wheel_drive_controller ********************************* -* 🚀 The multi_omni_wheel_drive_controller was added 🎉 (`#512 `_). +* 🚀 The multi_omni_wheel_drive_controller was added 🎉 (`#1535 `_). pid_controller ************************ From 82b6d87751007493a1aaf67b1b2490056af5dcbf Mon Sep 17 00:00:00 2001 From: Aarav Gupta Date: Wed, 12 Feb 2025 23:21:17 +0530 Subject: [PATCH 03/18] Update package.xml --- multi_omni_wheel_drive_controller/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/multi_omni_wheel_drive_controller/package.xml b/multi_omni_wheel_drive_controller/package.xml index 130870ea2c..0ffc8041df 100644 --- a/multi_omni_wheel_drive_controller/package.xml +++ b/multi_omni_wheel_drive_controller/package.xml @@ -22,9 +22,9 @@ tf2 tf2_msgs + ament_cmake_gmock ament_lint_auto ament_lint_common - ament_add_gmock controller_manager hardware_interface ros2_control_test_assets From ff32b2dc44a521f4166182126f9a52eb66460a28 Mon Sep 17 00:00:00 2001 From: Aarav Gupta Date: Wed, 12 Feb 2025 23:27:38 +0530 Subject: [PATCH 04/18] Remove ament_lint test dependencies in package.xml --- multi_omni_wheel_drive_controller/package.xml | 2 -- 1 file changed, 2 deletions(-) diff --git a/multi_omni_wheel_drive_controller/package.xml b/multi_omni_wheel_drive_controller/package.xml index 0ffc8041df..f81d6bb9b5 100644 --- a/multi_omni_wheel_drive_controller/package.xml +++ b/multi_omni_wheel_drive_controller/package.xml @@ -23,8 +23,6 @@ tf2_msgs ament_cmake_gmock - ament_lint_auto - ament_lint_common controller_manager hardware_interface ros2_control_test_assets From a5d581a9c3bdaaf1e2a3c5c813fb03e06bd51c7c Mon Sep 17 00:00:00 2001 From: Aarav Gupta Date: Thu, 13 Feb 2025 01:42:12 +0530 Subject: [PATCH 05/18] Update mobile_robot_kinematics.rst --- .../omni_wheel_omnidirectional_drive.drawio | 148 ++++++++++++++++++ .../omni_wheel_omnidirectional_drive.svg | 4 + doc/mobile_robot_kinematics.rst | 87 +++++++++- .../doc/userdoc.rst | 6 +- 4 files changed, 241 insertions(+), 4 deletions(-) create mode 100644 doc/images/omni_wheel_omnidirectional_drive.drawio create mode 100644 doc/images/omni_wheel_omnidirectional_drive.svg diff --git a/doc/images/omni_wheel_omnidirectional_drive.drawio b/doc/images/omni_wheel_omnidirectional_drive.drawio new file mode 100644 index 0000000000..ec8061e393 --- /dev/null +++ b/doc/images/omni_wheel_omnidirectional_drive.drawio @@ -0,0 +1,148 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/doc/images/omni_wheel_omnidirectional_drive.svg b/doc/images/omni_wheel_omnidirectional_drive.svg new file mode 100644 index 0000000000..f49dca8cfd --- /dev/null +++ b/doc/images/omni_wheel_omnidirectional_drive.svg @@ -0,0 +1,4 @@ + + + +
 \ No newline at end of file diff --git a/doc/mobile_robot_kinematics.rst b/doc/mobile_robot_kinematics.rst index 2ab4f1c0ee..57864654b3 100644 --- a/doc/mobile_robot_kinematics.rst +++ b/doc/mobile_robot_kinematics.rst @@ -23,7 +23,92 @@ The forward integration of the kinematic model using the encoders of the wheel a Omnidirectional Wheeled Mobile Robots ..................................... -Robots with omniwheels or mecanum wheels. Section will be updated if controllers for these robots are implemented. +Omnidirectional Drive Robots using Omni Wheels +,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,, + +The below explains the kinematics of omnidirectional drive robots using 3 or more omni wheels. +It follows the coordinate conventions defined in `ROS REP 103 `__. + +.. image:: images/omni_wheel_omnidirectional_drive.svg + :width: 550 + :align: center + :alt: Omnidirectional Drive Robot using Omni Wheels + +* :math:`x_b,y_b` is the robot's body-frame coordinate system, located at the contact point of the wheel on the ground. +* :math:`x_w,y_w` is the world coordinate system. +* :math:`v_{b,x},` is the robot's linear velocity on the x-axis. +* :math:`v_{b,y}` is the robot's linear velocity on the y-axis. +* :math:`w_{b,z}` is the robot's angular velocity on the z-axis. +* :math:`R` is the robot's radius / the distance between the robot's center and the wheels. +* Red arrows on the wheels signify the positive direction of their rotation. +* :math:`\gamma` is the angular offset of the first wheel from :math:`x_b`. +* :math:`\theta` is the angle between each wheel which can be calculated using the below equation where :math:`n` is the number of wheels. + +.. math:: + + θ = \frac{2\pi}{n} + +**Inverse Kinematics** + +The necessary angular velocity of the wheels to achieve a desired body twist can be calculated using the below matrix: + +.. math:: + + A = + \begin{bmatrix} + sin(\gamma) & -cos(\gamma) & -R \\ + sin(\theta + \gamma) & -cos(\theta + \gamma) & -R\\ + sin(2\theta + \gamma) & -cos(2\theta + \gamma) & -R\\ + sin(3\theta + \gamma) & -cos(3\theta + \gamma) & -R\\ + \vdots & \vdots & \vdots\\ + sin((n-1)\theta + \gamma) & -cos((n-1)\theta + \gamma) & -R\\ + \end{bmatrix} + +.. math:: + + \begin{bmatrix} + w_1\\ + w_2\\ + w_3\\ + w_4\\ + \vdots\\ + w_n + \end{bmatrix} = + \frac{1}{r} + A + \begin{bmatrix} + v_{b,x}\\ + v_{b,y}\\ + w_{b,z}\\ + \end{bmatrix} + +Here :math:`w_1..w_n` are the angular velocities of the wheels and :math:`r` is the radius of the wheels. +These equations can be written in algebraic form for any wheel :math:`i` like this: + +.. math:: + \omega_i = \frac{\sin((i-1)\theta + \gamma) v_{b,x} - \cos((i-1)\theta + \gamma) v_{b,y} - R w_{b,z}}{r} + +**Forward Kinematics** + +The body twist of the robot can be obtained from the wheel velocities by using the pseudoinverse of matrix :math:`A`. + +.. math:: + + \begin{bmatrix} + v_{b,x}\\ + v_{b,y}\\ + w_{b,z}\\ + \end{bmatrix} = + A^+ + (r + \begin{bmatrix} + w_1\\ + w_2\\ + w_3\\ + w_4\\ + \vdots\\ + w_n + \end{bmatrix}) Nonholonomic Wheeled Mobile Robots ..................................... diff --git a/multi_omni_wheel_drive_controller/doc/userdoc.rst b/multi_omni_wheel_drive_controller/doc/userdoc.rst index 1cc359856b..5413e7d91a 100644 --- a/multi_omni_wheel_drive_controller/doc/userdoc.rst +++ b/multi_omni_wheel_drive_controller/doc/userdoc.rst @@ -8,7 +8,7 @@ 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. -The positive direction of the axis of rotation for each wheel should be facing outwards from the center of the robot. +To better understand this, have a look at :ref:`mobile_robot_kinematics`. As input it takes velocity commands for the robot body, which are translated to wheel commands. @@ -67,9 +67,9 @@ Publishers 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. +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_parameter.yaml +.. 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 `_: From 4b26153d1685876ad0c1740d31d900a3a6a33b28 Mon Sep 17 00:00:00 2001 From: Aarav Gupta Date: Thu, 13 Feb 2025 17:04:23 +0530 Subject: [PATCH 06/18] Update in accordance to #1534 Also removed a redundant test. --- .../multi_omni_wheel_drive_controller.hpp | 2 - .../src/multi_omni_wheel_drive_controller.cpp | 26 +-------- ...test_multi_omni_wheel_drive_controller.cpp | 54 ------------------- 3 files changed, 1 insertion(+), 81 deletions(-) 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 index abed70aa31..f6d5bd0144 100644 --- 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 @@ -91,8 +91,6 @@ class MultiOmniWheelDriveController : public controller_interface::ChainableCont // Timeout to consider cmd_vel commands old rclcpp::Duration cmd_vel_timeout_ = rclcpp::Duration::from_seconds(0.5); - bool is_halted_ = false; - bool subscriber_is_active_ = false; rclcpp::Subscription::SharedPtr velocity_command_subscriber_ = nullptr; realtime_tools::RealtimeBuffer> received_velocity_msg_ptr_{nullptr}; 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 index cb07921262..18b3fb2ac3 100644 --- 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 @@ -235,7 +235,6 @@ controller_interface::CallbackReturn MultiOmniWheelDriveController::on_activate( return controller_interface::CallbackReturn::ERROR; } - is_halted_ = false; subscriber_is_active_ = true; RCLCPP_DEBUG(get_node()->get_logger(), "Subscriber and publisher are now active."); @@ -246,11 +245,7 @@ controller_interface::CallbackReturn MultiOmniWheelDriveController::on_deactivat const rclcpp_lifecycle::State &) { subscriber_is_active_ = false; - if (!is_halted_) - { - halt(); - is_halted_ = true; - } + halt(); reset_buffers(); registered_wheel_handles_.clear(); return controller_interface::CallbackReturn::SUCCESS; @@ -265,15 +260,6 @@ controller_interface::return_type MultiOmniWheelDriveController::update_referenc const rclcpp::Time & time, const rclcpp::Duration &) { auto logger = get_node()->get_logger(); - if (get_lifecycle_state().id() == State::PRIMARY_STATE_INACTIVE) - { - if (!is_halted_) - { - halt(); - is_halted_ = true; - } - return controller_interface::return_type::OK; - } const std::shared_ptr command_msg_ptr = *(received_velocity_msg_ptr_.readFromRT()); @@ -315,15 +301,6 @@ controller_interface::return_type MultiOmniWheelDriveController::update_and_writ const rclcpp::Time & time, const rclcpp::Duration &) { auto logger = get_node()->get_logger(); - if (get_lifecycle_state().id() == State::PRIMARY_STATE_INACTIVE) - { - if (!is_halted_) - { - halt(); - is_halted_ = true; - } - return controller_interface::return_type::OK; - } if ( !std::isfinite(reference_interfaces_[0]) || !std::isfinite(reference_interfaces_[1]) || @@ -466,7 +443,6 @@ bool MultiOmniWheelDriveController::reset() subscriber_is_active_ = false; velocity_command_subscriber_.reset(); - is_halted_ = false; return true; } 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 index ce820c1a81..5252a068e3 100644 --- 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 @@ -295,50 +295,6 @@ TEST_F(MultiOmniWheelDriveControllerTest, activate_fails_with_wrong_resources_as ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_ERROR); } -TEST_F(MultiOmniWheelDriveControllerTest, cleanup) -{ - ASSERT_EQ(InitController(), controller_interface::return_type::OK); - - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(controller_->get_node()->get_node_base_interface()); - auto state = controller_->get_node()->configure(); - ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); - assignResourcesPosFeedback(); - - state = controller_->get_node()->activate(); - ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); - - waitForSetup(); - - 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); - - state = controller_->get_node()->deactivate(); - ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); - ASSERT_EQ( - controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - state = controller_->get_node()->cleanup(); - ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); - - // Wheels should be stopped - double value; - for (size_t i = 0; i < command_itfs_.size(); i++) - { - if (command_itfs_[i].get_value(value)) - { - EXPECT_EQ(value, 0.0); - } - } - - executor.cancel(); -} - // 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 @@ -417,14 +373,9 @@ TEST_F(MultiOmniWheelDriveControllerTest, chainable_controller_unchained_mode) } // 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++) { if (command_itfs_[i].get_value(value)) @@ -506,14 +457,9 @@ TEST_F(MultiOmniWheelDriveControllerTest, chainable_controller_chained_mode) } // 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++) { if (command_itfs_[i].get_value(value)) From 249c6f99be75ec438ffc6b08f53b17a0e5886e59 Mon Sep 17 00:00:00 2001 From: Aarav Gupta Date: Thu, 13 Feb 2025 17:16:43 +0530 Subject: [PATCH 07/18] Fix pre-commit for omni_wheel_omnidirectional_drive.svg --- doc/images/omni_wheel_omnidirectional_drive.svg | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/images/omni_wheel_omnidirectional_drive.svg b/doc/images/omni_wheel_omnidirectional_drive.svg index f49dca8cfd..8d1ee5c1f9 100644 --- a/doc/images/omni_wheel_omnidirectional_drive.svg +++ b/doc/images/omni_wheel_omnidirectional_drive.svg @@ -1,4 +1,4 @@ -
 \ No newline at end of file +
 From 363ebe4ed726bbbccfc0479138358203d41f50e0 Mon Sep 17 00:00:00 2001 From: Aarav Gupta Date: Sat, 15 Feb 2025 09:37:06 +0530 Subject: [PATCH 08/18] Apply suggestions from code review MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Christoph Fröhlich --- doc/mobile_robot_kinematics.rst | 47 ++++++++++++++++----------------- 1 file changed, 23 insertions(+), 24 deletions(-) diff --git a/doc/mobile_robot_kinematics.rst b/doc/mobile_robot_kinematics.rst index 57864654b3..cb9fff8e71 100644 --- a/doc/mobile_robot_kinematics.rst +++ b/doc/mobile_robot_kinematics.rst @@ -38,9 +38,9 @@ It follows the coordinate conventions defined in `ROS REP 103 Date: Sat, 15 Feb 2025 09:38:38 +0530 Subject: [PATCH 09/18] Apply suggestions from code review MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Christoph Fröhlich --- multi_omni_wheel_drive_controller/doc/userdoc.rst | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/multi_omni_wheel_drive_controller/doc/userdoc.rst b/multi_omni_wheel_drive_controller/doc/userdoc.rst index 5413e7d91a..b1f0399fa2 100644 --- a/multi_omni_wheel_drive_controller/doc/userdoc.rst +++ b/multi_omni_wheel_drive_controller/doc/userdoc.rst @@ -10,7 +10,8 @@ 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`. -As input it takes velocity commands for the robot body, which are translated to wheel commands. +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. @@ -35,13 +36,13 @@ When controller is in chained mode, it exposes the following references which ca Together, these represent the body twist (which in unchained-mode would be obtained from ~/cmd_vel). -Feedback -,,,,,,,, +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. +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. -Output -,,,,,, +Command interfaces +,,,,,,,,,,,,,,,,,,,,,, Joints' velocity (``hardware_interface::HW_IF_VELOCITY``) are used. From 57059bfba5f09f2bd2b35e9feff1f6dd37b9616f Mon Sep 17 00:00:00 2001 From: Aarav Gupta Date: Sat, 15 Feb 2025 10:23:52 +0530 Subject: [PATCH 10/18] Update diagram according to review --- .../omni_wheel_omnidirectional_drive.drawio | 96 ++++++++++++------- .../omni_wheel_omnidirectional_drive.svg | 2 +- 2 files changed, 64 insertions(+), 34 deletions(-) diff --git a/doc/images/omni_wheel_omnidirectional_drive.drawio b/doc/images/omni_wheel_omnidirectional_drive.drawio index ec8061e393..d825a5acb2 100644 --- a/doc/images/omni_wheel_omnidirectional_drive.drawio +++ b/doc/images/omni_wheel_omnidirectional_drive.drawio @@ -1,12 +1,21 @@ - + + + + + + + + + + @@ -46,9 +55,6 @@ - - - @@ -79,33 +85,21 @@ - - + + - + - + - - - - - - - + - - - - - - @@ -118,8 +112,32 @@ - - + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -127,21 +145,33 @@ - - - - - + + - - + + + + + - + - - + + + + + + + + + + + + + + diff --git a/doc/images/omni_wheel_omnidirectional_drive.svg b/doc/images/omni_wheel_omnidirectional_drive.svg index 8d1ee5c1f9..c4ec883256 100644 --- a/doc/images/omni_wheel_omnidirectional_drive.svg +++ b/doc/images/omni_wheel_omnidirectional_drive.svg @@ -1,4 +1,4 @@ -
 +
 \ No newline at end of file From 8c0c3eccd6f8dbb4f600835307df3aae78ac747c Mon Sep 17 00:00:00 2001 From: Aarav Gupta Date: Sat, 15 Feb 2025 11:09:34 +0530 Subject: [PATCH 11/18] Update mobile_robot_kinematics.rst --- doc/mobile_robot_kinematics.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/mobile_robot_kinematics.rst b/doc/mobile_robot_kinematics.rst index cb9fff8e71..b861733ee7 100644 --- a/doc/mobile_robot_kinematics.rst +++ b/doc/mobile_robot_kinematics.rst @@ -82,7 +82,7 @@ The necessary angular velocity of the wheels to achieve a desired body twist can \omega_{b,z}\\ \end{bmatrix} -Here :math:`w_1,\ldots,w_n` are the angular velocities of the wheels and :math:`r` is the radius of the wheels. +Here :math:`\omega_1,\ldots,\omega_n` are the angular velocities of the wheels and :math:`r` is the radius of the wheels. These equations can be written in algebraic form for any wheel :math:`i` like this: .. math:: From 077eba57eedd17015e654cd3f9c1207b2a785346 Mon Sep 17 00:00:00 2001 From: Aarav Gupta Date: Sun, 16 Feb 2025 17:32:55 +0530 Subject: [PATCH 12/18] Do some code cleanup and fixes - Pass angular positions and velocity to Odometry class - Check if dt is too small in update_open_lopp() in Odometry class - Improve the checks that determine if an odometry message should be published --- .../multi_omni_wheel_drive_controller.hpp | 3 +- .../odometry.hpp | 6 +- .../src/multi_omni_wheel_drive_controller.cpp | 102 +++++++++--------- ...mni_wheel_drive_controller_parameters.yaml | 8 +- .../src/odometry.cpp | 32 ++++-- 5 files changed, 84 insertions(+), 67 deletions(-) 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 index f6d5bd0144..5ec5518ee1 100644 --- 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 @@ -110,8 +110,7 @@ class MultiOmniWheelDriveController : public controller_interface::ChainableCont rclcpp::Time previous_update_timestamp_{0}; - // publish rate limiter - double publish_rate_ = 50.0; + // Variables to help limit the publish rate of odometry as described using params_.publish_rate rclcpp::Duration publish_period_ = rclcpp::Duration::from_nanoseconds(0); rclcpp::Time previous_publish_timestamp_{0, 0, RCL_CLOCK_UNINITIALIZED}; 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 index 1c0a72ba0b..e708e53328 100644 --- 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 @@ -30,7 +30,7 @@ class Odometry void init(const rclcpp::Time & time); bool update(const std::vector & wheels_pos, const rclcpp::Time & time); bool updateFromVelocity(const std::vector & wheels_vel, const rclcpp::Time & time); - void updateOpenLoop( + bool updateOpenLoop( const double & linear_x_vel, const double & linear_y_vel, const double & angular_vel, const rclcpp::Time & time); void resetOdometry(); @@ -43,7 +43,8 @@ class Odometry double getAngularVel() const { return angular_vel_; } void setParams( - const double & robot_radius, const double & wheel_offset, const size_t & wheel_count); + 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; @@ -64,6 +65,7 @@ class Odometry // Robot kinematic parameters: double robot_radius_; // [m] + double wheel_radius_; // [m] double wheel_offset_; // [rad] // Previous wheel positions/states [m]: 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 index 18b3fb2ac3..1ddedf0185 100644 --- 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 @@ -81,7 +81,8 @@ controller_interface::CallbackReturn MultiOmniWheelDriveController::on_configure return controller_interface::CallbackReturn::ERROR; } - odometry_.setParams(params_.robot_radius, params_.wheel_offset, params_.wheel_names.size()); + 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); @@ -171,8 +172,7 @@ controller_interface::CallbackReturn MultiOmniWheelDriveController::on_configure odometry_message.child_frame_id = base_frame_id; // Limit the publication on the topics /odom and /tf - publish_rate_ = params_.publish_rate; - publish_period_ = rclcpp::Duration::from_seconds(1.0 / publish_rate_); + publish_period_ = rclcpp::Duration::from_seconds(1.0 / params_.publish_rate); // Initialize odom values zeros odometry_message.twist = @@ -310,15 +310,16 @@ controller_interface::return_type MultiOmniWheelDriveController::update_and_writ return controller_interface::return_type::OK; } + // Update odometry + bool odometry_updated = false; if (params_.open_loop) { - odometry_.updateOpenLoop( + odometry_updated = odometry_.updateOpenLoop( reference_interfaces_[0], reference_interfaces_[1], reference_interfaces_[2], time); } else { - // [m] - std::vector wheels_feedback(registered_wheel_handles_.size()); + std::vector wheels_feedback(registered_wheel_handles_.size()); // [rads] for (size_t i = 0; i < static_cast(registered_wheel_handles_.size()); ++i) { const double wheel_feedback = registered_wheel_handles_[i].feedback.get().get_value(); @@ -329,65 +330,66 @@ controller_interface::return_type MultiOmniWheelDriveController::update_and_writ return controller_interface::return_type::ERROR; } - wheels_feedback[i] = wheel_feedback * params_.wheel_radius; + wheels_feedback[i] = wheel_feedback; } if (params_.position_feedback) { - odometry_.update(wheels_feedback, time); + odometry_updated = odometry_.update(wheels_feedback, time); } else { - odometry_.updateFromVelocity(wheels_feedback, time); + odometry_updated = odometry_.updateFromVelocity(wheels_feedback, time); } } - tf2::Quaternion orientation; - orientation.setRPY(0.0, 0.0, odometry_.getHeading()); - - bool should_publish = false; - try + if (odometry_updated) { - if (previous_publish_timestamp_ + publish_period_ < time) + tf2::Quaternion orientation; + orientation.setRPY(0.0, 0.0, odometry_.getHeading()); + + bool should_publish = true; + try { - previous_publish_timestamp_ += publish_period_; - should_publish = true; + if (time - previous_publish_timestamp_ < publish_period_) + { + should_publish = false; + } } - } - catch (const std::runtime_error &) - { - // Handle exceptions when the time source changes and initialize publish timestamp - previous_publish_timestamp_ = time; - should_publish = true; - } - - if (should_publish) - { - if (realtime_odometry_publisher_->trylock()) + catch (const std::runtime_error &) { - 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(); + // Handle exceptions when the time source changes and initialize publish timestamp + previous_publish_timestamp_ = time; } - if (params_.enable_odom_tf && realtime_odometry_transform_publisher_->trylock()) + + if (should_publish) { - 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(); + 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(); + } } } 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 index 6dfc46778b..ba9cff1a3a 100644 --- 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 @@ -80,12 +80,14 @@ multi_omni_wheel_drive_controller: default_value: true, description: "Publish transformation between ``odom_frame_id`` and ``base_frame_id``.", } - cmd_vel_timeout: { + 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.", + description: "Timeout in seconds, after which input command on ``~/cmd_vel`` topic is considered staled.", } - publish_rate: { + 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 index 76ea03458d..ab42ce3930 100644 --- a/multi_omni_wheel_drive_controller/src/odometry.cpp +++ b/multi_omni_wheel_drive_controller/src/odometry.cpp @@ -29,6 +29,7 @@ Odometry::Odometry() linear_y_vel_(0.0), angular_vel_(0.0), robot_radius_(0.0), + wheel_radius_(0.0), wheels_old_pos_(0.0) { } @@ -48,7 +49,7 @@ bool Odometry::update(const std::vector & wheels_pos, const rclcpp::Time return false; // Interval too small to integrate with } - // Estimate velocity of wheels using old and current position [m/s]: + // 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) { @@ -86,7 +87,7 @@ bool Odometry::updateFromVelocity(const std::vector & wheels_vel, const 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 velocities vector + Eigen::VectorXd omega(wheels_vel.size()); // Wheel angular velocities vector const double angle_bw_wheels = (2 * M_PI) / static_cast(wheels_vel.size()); @@ -98,29 +99,38 @@ Eigen::Vector3d Odometry::compute_robot_velocity(const std::vector & whe A(static_cast(i), 1) = -std::cos(theta); A(static_cast(i), 2) = -robot_radius_; - // Define the wheel velocities vector + // 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); + const Eigen::Vector3d V = + A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(omega * wheel_radius_); return V; } -void Odometry::updateOpenLoop( +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(); + if (dt < 0.0001) + { + return false; // Interval too small to integrate with + } + + // 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; - // Integrate odometry: - const double dt = time.seconds() - timestamp_.seconds(); - timestamp_ = time; - integrate(linear_x_vel * dt, linear_y_vel * dt, angular_vel * dt); + return true; } void Odometry::resetOdometry() @@ -131,9 +141,11 @@ void Odometry::resetOdometry() } void Odometry::setParams( - const double & robot_radius, const double & wheel_offset, const size_t & wheel_count) + 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); } From c487fa8bfbaaa721eab1f9f40c07e4a7555c6dca Mon Sep 17 00:00:00 2001 From: Aarav Gupta Date: Sun, 16 Feb 2025 17:47:23 +0530 Subject: [PATCH 13/18] Fix a comment --- .../multi_omni_wheel_drive_controller/odometry.hpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) 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 index e708e53328..a1818b3484 100644 --- 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 @@ -56,19 +56,19 @@ class Odometry // Current pose: double x_; // [m] double y_; // [m] - double heading_; // [rad] + double heading_; // [rads] // Current velocity: double linear_x_vel_; // [m/s] double linear_y_vel_; // [m/s] - double angular_vel_; // [rad/s] + double angular_vel_; // [rads/s] // Robot kinematic parameters: double robot_radius_; // [m] double wheel_radius_; // [m] - double wheel_offset_; // [rad] + double wheel_offset_; // [rads] - // Previous wheel positions/states [m]: + // Previous wheel positions/states [rads]: std::vector wheels_old_pos_; }; From a67ac1f9ce0313e19bc3dbfb51111fbd21529b30 Mon Sep 17 00:00:00 2001 From: Aarav Gupta Date: Sun, 16 Feb 2025 22:55:50 +0530 Subject: [PATCH 14/18] Update integration in odometry --- .../odometry.hpp | 5 +- .../src/multi_omni_wheel_drive_controller.cpp | 4 +- .../src/odometry.cpp | 58 ++++++++----------- 3 files changed, 27 insertions(+), 40 deletions(-) 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 index a1818b3484..3456e22b48 100644 --- 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 @@ -27,9 +27,8 @@ class Odometry public: Odometry(); - void init(const rclcpp::Time & time); - bool update(const std::vector & wheels_pos, const rclcpp::Time & time); - bool updateFromVelocity(const std::vector & wheels_vel, const rclcpp::Time & time); + 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); 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 index 1ddedf0185..ae046fa2aa 100644 --- 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 @@ -334,11 +334,11 @@ controller_interface::return_type MultiOmniWheelDriveController::update_and_writ } if (params_.position_feedback) { - odometry_updated = odometry_.update(wheels_feedback, time); + odometry_updated = odometry_.updateFromPos(wheels_feedback, time); } else { - odometry_updated = odometry_.updateFromVelocity(wheels_feedback, time); + odometry_updated = odometry_.updateFromVel(wheels_feedback, time); } } diff --git a/multi_omni_wheel_drive_controller/src/odometry.cpp b/multi_omni_wheel_drive_controller/src/odometry.cpp index ab42ce3930..5a5ec3e5fc 100644 --- a/multi_omni_wheel_drive_controller/src/odometry.cpp +++ b/multi_omni_wheel_drive_controller/src/odometry.cpp @@ -15,8 +15,6 @@ #include "multi_omni_wheel_drive_controller/odometry.hpp" #include -#include "tf2/LinearMath/Matrix3x3.hpp" -#include "tf2/LinearMath/Quaternion.hpp" namespace multi_omni_wheel_drive_controller { @@ -34,13 +32,7 @@ Odometry::Odometry() { } -void Odometry::init(const rclcpp::Time & time) -{ - // Reset timestamp: - timestamp_ = time; -} - -bool Odometry::update(const std::vector & wheels_pos, const rclcpp::Time & time) +bool Odometry::updateFromPos(const std::vector & wheels_pos, const rclcpp::Time & time) { // We cannot estimate the speed with very small time intervals: const double dt = time.seconds() - timestamp_.seconds(); @@ -57,17 +49,16 @@ bool Odometry::update(const std::vector & wheels_pos, const rclcpp::Time wheels_old_pos_[i] = wheels_pos[i]; } - updateFromVelocity(wheels_vel, time); - return true; + if (updateFromVel(wheels_vel, time)) + { + return true; + } + return false; } -bool Odometry::updateFromVelocity(const std::vector & wheels_vel, const rclcpp::Time & time) +bool Odometry::updateFromVel(const std::vector & wheels_vel, const rclcpp::Time & time) { const double dt = time.seconds() - timestamp_.seconds(); - if (dt < 0.0001) - { - return false; // Interval too small to integrate with - } // Compute linear and angular velocities of the robot: const Eigen::Vector3d robot_velocity = compute_robot_velocity(wheels_vel); @@ -115,10 +106,6 @@ bool Odometry::updateOpenLoop( const rclcpp::Time & time) { const double dt = time.seconds() - timestamp_.seconds(); - if (dt < 0.0001) - { - return false; // Interval too small to integrate with - } // Integrate odometry: integrate(linear_x_vel * dt, linear_y_vel * dt, angular_vel * dt); @@ -152,21 +139,22 @@ void Odometry::setParams( void Odometry::integrate(const double & dx, const double & dy, const double & dheading) { - heading_ = heading_ + dheading; - - // Create a quaternion representing the rotation from the base frame to the odom frame - tf2::Quaternion q_base_to_odom; - q_base_to_odom.setRPY(0.0, 0.0, heading_); - // Create a rotation matrix from the quaternion - const tf2::Matrix3x3 rot_base_to_odom = tf2::Matrix3x3(q_base_to_odom); - - // Define the displacement vector in the base frame - const tf2::Vector3 displacement_base(dx, dy, 0.0); - // Rotate the displacement in the base frame into the odom frame - const tf2::Vector3 displacement_odom = rot_base_to_odom * displacement_base; - - x_ = x_ + displacement_odom.x(); - y_ = y_ + displacement_odom.y(); + 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 From ad3eda79065cb9730e919d2e872769d0f2f25e27 Mon Sep 17 00:00:00 2001 From: Aarav Gupta Date: Tue, 25 Feb 2025 12:17:14 +0530 Subject: [PATCH 15/18] Fix incorrect merge --- .../omni_wheel_omnidirectional_drive.drawio | 205 ++++++++++-------- .../omni_wheel_omnidirectional_drive.svg | 2 +- 2 files changed, 119 insertions(+), 88 deletions(-) diff --git a/doc/images/omni_wheel_omnidirectional_drive.drawio b/doc/images/omni_wheel_omnidirectional_drive.drawio index d825a5acb2..71a628ba0f 100644 --- a/doc/images/omni_wheel_omnidirectional_drive.drawio +++ b/doc/images/omni_wheel_omnidirectional_drive.drawio @@ -1,21 +1,27 @@ - + - - - - - + + + + + - + - - + + + + + + + + @@ -34,7 +40,7 @@ - + @@ -50,127 +56,152 @@ - + - - - - - - - + - + - - - - - + + - - - - - + + - - + + - - - - - + + - - + + - + - - + + - - + + - - + + - - + + - - + + + + + - + - - + + - - + + - - + + - - + + - + + + + - - + + + + + + - + - - + + + + + - + + + + + + + + + + - - + + - + + + + + + + + + + - - + + - - - - + - - + + - + + + + + + + + + + - - + + - - + + - - + + - - + + - - + + + + + diff --git a/doc/images/omni_wheel_omnidirectional_drive.svg b/doc/images/omni_wheel_omnidirectional_drive.svg index 411e40cd63..215bb702b8 100644 --- a/doc/images/omni_wheel_omnidirectional_drive.svg +++ b/doc/images/omni_wheel_omnidirectional_drive.svg @@ -1,4 +1,4 @@ -
 +
 From fb75835b6131fa8518a1fe42fa0a7d2307622eea Mon Sep 17 00:00:00 2001 From: Aarav Gupta Date: Sat, 15 Mar 2025 13:04:09 +0530 Subject: [PATCH 16/18] Cleanup code and fix deprecation warnings --- .../multi_omni_wheel_drive_controller.hpp | 4 +- .../src/multi_omni_wheel_drive_controller.cpp | 34 +++++--- .../src/odometry.cpp | 6 +- ...test_multi_omni_wheel_drive_controller.cpp | 79 ++++--------------- ...test_multi_omni_wheel_drive_controller.hpp | 1 - 5 files changed, 44 insertions(+), 80 deletions(-) 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 index 5ec5518ee1..f9c77f60a0 100644 --- 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 @@ -108,10 +108,8 @@ class MultiOmniWheelDriveController : public controller_interface::ChainableCont std::shared_ptr> realtime_odometry_transform_publisher_ = nullptr; - rclcpp::Time previous_update_timestamp_{0}; - // Variables to help limit the publish rate of odometry as described using params_.publish_rate - rclcpp::Duration publish_period_ = rclcpp::Duration::from_nanoseconds(0); + rclcpp::Duration publish_period_ = rclcpp::Duration::from_seconds(0); rclcpp::Time previous_publish_timestamp_{0, 0, RCL_CLOCK_UNINITIALIZED}; bool reset(); 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 index ae046fa2aa..1c53056809 100644 --- 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 @@ -16,6 +16,7 @@ #include #include +#include #include #include @@ -200,7 +201,6 @@ controller_interface::CallbackReturn MultiOmniWheelDriveController::on_configure 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(); return controller_interface::CallbackReturn::SUCCESS; } @@ -292,15 +292,13 @@ controller_interface::return_type MultiOmniWheelDriveController::update_referenc "Command message contains NaNs. Not updating reference interfaces."); } - previous_update_timestamp_ = time; - return controller_interface::return_type::OK; } controller_interface::return_type MultiOmniWheelDriveController::update_and_write_commands( const rclcpp::Time & time, const rclcpp::Duration &) { - auto logger = get_node()->get_logger(); + rclcpp::Logger logger = get_node()->get_logger(); if ( !std::isfinite(reference_interfaces_[0]) || !std::isfinite(reference_interfaces_[1]) || @@ -322,7 +320,15 @@ controller_interface::return_type MultiOmniWheelDriveController::update_and_writ std::vector wheels_feedback(registered_wheel_handles_.size()); // [rads] for (size_t i = 0; i < static_cast(registered_wheel_handles_.size()); ++i) { - const double wheel_feedback = registered_wheel_handles_[i].feedback.get().get_value(); + // 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)) { @@ -350,7 +356,7 @@ controller_interface::return_type MultiOmniWheelDriveController::update_and_writ bool should_publish = true; try { - if (time - previous_publish_timestamp_ < publish_period_) + if (time.seconds() - previous_publish_timestamp_.seconds() < publish_period_.seconds()) { should_publish = false; } @@ -421,10 +427,12 @@ controller_interface::CallbackReturn MultiOmniWheelDriveController::on_error( 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) { - registered_wheel_handles_[i].velocity.get().set_value( + 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) * @@ -432,6 +440,10 @@ void MultiOmniWheelDriveController::compute_and_set_wheel_velocities() (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() @@ -451,7 +463,7 @@ bool MultiOmniWheelDriveController::reset() controller_interface::CallbackReturn MultiOmniWheelDriveController::configure_wheel_handles( const std::vector & wheel_names, std::vector & registered_handles) { - auto logger = get_node()->get_logger(); + rclcpp::Logger logger = get_node()->get_logger(); // Register handles registered_handles.reserve(wheel_names.size()); @@ -495,10 +507,14 @@ controller_interface::CallbackReturn MultiOmniWheelDriveController::configure_wh void MultiOmniWheelDriveController::halt() { + bool set_command_result = true; for (const WheelHandle & wheel_handle : registered_wheel_handles_) { - wheel_handle.velocity.get().set_value(0.0); + 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() diff --git a/multi_omni_wheel_drive_controller/src/odometry.cpp b/multi_omni_wheel_drive_controller/src/odometry.cpp index 5a5ec3e5fc..6377e7e76f 100644 --- a/multi_omni_wheel_drive_controller/src/odometry.cpp +++ b/multi_omni_wheel_drive_controller/src/odometry.cpp @@ -34,11 +34,11 @@ Odometry::Odometry() bool Odometry::updateFromPos(const std::vector & wheels_pos, const rclcpp::Time & time) { - // We cannot estimate the speed with very small time intervals: const double dt = time.seconds() - timestamp_.seconds(); - if (dt < 0.0001) + // We cannot estimate angular velocity with very small time intervals + if (std::fabs(dt) < 1e-6) { - return false; // Interval too small to integrate with + return false; } // Estimate angular velocity of wheels using old and current position [rads/s]: 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 index 5252a068e3..2bf165ca2d 100644 --- 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 @@ -326,13 +326,9 @@ TEST_F(MultiOmniWheelDriveControllerTest, chainable_controller_unchained_mode) EXPECT_TRUE(std::isnan(interface)); } // But NaNs should not propagate to command interfaces - double value; for (size_t i = 0; i < command_itfs_.size(); i++) { - if (command_itfs_[i].get_value(value)) - { - ASSERT_FALSE(std::isnan(value)); - } + 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 @@ -348,10 +344,7 @@ TEST_F(MultiOmniWheelDriveControllerTest, chainable_controller_unchained_mode) controller_interface::return_type::OK); for (size_t i = 0; i < command_itfs_.size(); i++) { - if (command_itfs_[i].get_value(value)) - { - EXPECT_EQ(value, 0.0); - } + 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 @@ -366,10 +359,7 @@ TEST_F(MultiOmniWheelDriveControllerTest, chainable_controller_unchained_mode) std::vector expected_wheels_vel_cmds = {-15.0, 5.0, 5.0, -15.0}; for (size_t i = 0; i < command_itfs_.size(); i++) { - if (command_itfs_[i].get_value(value)) - { - EXPECT_DOUBLE_EQ(value, expected_wheels_vel_cmds[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 @@ -378,10 +368,7 @@ TEST_F(MultiOmniWheelDriveControllerTest, chainable_controller_unchained_mode) ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); for (size_t i = 0; i < command_itfs_.size(); i++) { - if (command_itfs_[i].get_value(value)) - { - EXPECT_EQ(value, 0.0); - } + EXPECT_EQ(command_itfs_[i].get_optional().value(), 0.0); } // cleanup @@ -389,10 +376,7 @@ TEST_F(MultiOmniWheelDriveControllerTest, chainable_controller_unchained_mode) ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); for (size_t i = 0; i < command_itfs_.size(); i++) { - if (command_itfs_[i].get_value(value)) - { - EXPECT_EQ(value, 0.0); - } + EXPECT_EQ(command_itfs_[i].get_optional().value(), 0.0); } state = controller_->get_node()->configure(); @@ -430,13 +414,9 @@ TEST_F(MultiOmniWheelDriveControllerTest, chainable_controller_chained_mode) EXPECT_TRUE(std::isnan(interface)); } // But NaNs should not propagate to command interfaces - double value; for (size_t i = 0; i < command_itfs_.size(); i++) { - if (command_itfs_[i].get_value(value)) - { - ASSERT_FALSE(std::isnan(value)); - } + ASSERT_FALSE(std::isnan(command_itfs_[i].get_optional().value())); } // Imitate preceding controllers by setting reference_interfaces_ @@ -450,10 +430,7 @@ TEST_F(MultiOmniWheelDriveControllerTest, chainable_controller_chained_mode) std::vector expected_wheels_vel_cmds = {-15.0, 5.0, 5.0, -15.0}; for (size_t i = 0; i < command_itfs_.size(); i++) { - if (command_itfs_[i].get_value(value)) - { - EXPECT_DOUBLE_EQ(value, expected_wheels_vel_cmds[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 @@ -462,10 +439,7 @@ TEST_F(MultiOmniWheelDriveControllerTest, chainable_controller_chained_mode) ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); for (size_t i = 0; i < command_itfs_.size(); i++) { - if (command_itfs_[i].get_value(value)) - { - EXPECT_EQ(value, 0.0); - } + EXPECT_EQ(command_itfs_[i].get_optional().value(), 0.0); } // cleanup @@ -473,10 +447,7 @@ TEST_F(MultiOmniWheelDriveControllerTest, chainable_controller_chained_mode) ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); for (size_t i = 0; i < command_itfs_.size(); i++) { - if (command_itfs_[i].get_value(value)) - { - EXPECT_EQ(value, 0.0); - } + EXPECT_EQ(command_itfs_[i].get_optional().value(), 0.0); } state = controller_->get_node()->configure(); @@ -511,13 +482,9 @@ TEST_F(MultiOmniWheelDriveControllerTest, deactivate_then_activate) EXPECT_TRUE(std::isnan(interface)); } // But NaNs should not propagate to command interfaces - double value; for (size_t i = 0; i < command_itfs_.size(); i++) { - if (command_itfs_[i].get_value(value)) - { - ASSERT_FALSE(std::isnan(value)); - } + 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 @@ -532,10 +499,7 @@ TEST_F(MultiOmniWheelDriveControllerTest, deactivate_then_activate) std::vector expected_wheels_vel_cmds = {-15.0, 5.0, 5.0, -15.0}; for (size_t i = 0; i < command_itfs_.size(); i++) { - if (command_itfs_[i].get_value(value)) - { - EXPECT_DOUBLE_EQ(value, expected_wheels_vel_cmds[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 @@ -548,10 +512,7 @@ TEST_F(MultiOmniWheelDriveControllerTest, deactivate_then_activate) controller_interface::return_type::OK); for (size_t i = 0; i < command_itfs_.size(); i++) { - if (command_itfs_[i].get_value(value)) - { - EXPECT_EQ(value, 0.0); - } + EXPECT_EQ(command_itfs_[i].get_optional().value(), 0.0); } // Activate again @@ -567,10 +528,7 @@ TEST_F(MultiOmniWheelDriveControllerTest, deactivate_then_activate) } for (size_t i = 0; i < command_itfs_.size(); i++) { - if (command_itfs_[i].get_value(value)) - { - EXPECT_EQ(value, 0.0); - } + EXPECT_EQ(command_itfs_[i].get_optional().value(), 0.0); } // A new command should work as expected @@ -582,10 +540,7 @@ TEST_F(MultiOmniWheelDriveControllerTest, deactivate_then_activate) controller_interface::return_type::OK); for (size_t i = 0; i < command_itfs_.size(); i++) { - if (command_itfs_[i].get_value(value)) - { - EXPECT_DOUBLE_EQ(value, expected_wheels_vel_cmds[i]); - } + EXPECT_DOUBLE_EQ(command_itfs_[i].get_optional().value(), expected_wheels_vel_cmds[i]); } // Deactivate again and cleanup @@ -623,14 +578,10 @@ TEST_F(MultiOmniWheelDriveControllerTest, command_with_zero_timestamp_is_accepte ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - double value; std::vector expected_wheels_vel_cmds = {-15.0, 5.0, 5.0, -15.0}; for (size_t i = 0; i < command_itfs_.size(); i++) { - if (command_itfs_[i].get_value(value)) - { - EXPECT_DOUBLE_EQ(value, expected_wheels_vel_cmds[i]); - } + EXPECT_DOUBLE_EQ(command_itfs_[i].get_optional().value(), expected_wheels_vel_cmds[i]); } // Deactivate and cleanup 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 index c32d6cb30c..a5bc9e971e 100644 --- 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 @@ -18,7 +18,6 @@ #include #include #include -#include #include #include #include From d4f76304595ee7ebac3254c26d961b1aa9ef6aa4 Mon Sep 17 00:00:00 2001 From: Aarav Gupta Date: Sat, 15 Mar 2025 13:09:58 +0530 Subject: [PATCH 17/18] Cleanup includes --- .../multi_omni_wheel_drive_controller.hpp | 1 - .../src/multi_omni_wheel_drive_controller.cpp | 2 +- 2 files changed, 1 insertion(+), 2 deletions(-) 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 index f9c77f60a0..74c881c082 100644 --- 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 @@ -16,7 +16,6 @@ #define MULTI_OMNI_WHEEL_DRIVE_CONTROLLER__MULTI_OMNI_WHEEL_DRIVE_CONTROLLER_HPP_ #include -#include #include #include 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 index 1c53056809..82d7903462 100644 --- 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 @@ -16,12 +16,12 @@ #include #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" From a70960e0a9ea801598d091bb93803438a35b5e6c Mon Sep 17 00:00:00 2001 From: Aarav Gupta Date: Sat, 29 Mar 2025 18:55:52 +0530 Subject: [PATCH 18/18] Fix exported interface naming and use global cmake macros --- .../CMakeLists.txt | 8 +++----- multi_omni_wheel_drive_controller/package.xml | 4 +++- .../src/multi_omni_wheel_drive_controller.cpp | 17 +++++++---------- .../test_multi_omni_wheel_drive_controller.cpp | 12 +++++++----- .../test_multi_omni_wheel_drive_controller.hpp | 4 +--- 5 files changed, 21 insertions(+), 24 deletions(-) diff --git a/multi_omni_wheel_drive_controller/CMakeLists.txt b/multi_omni_wheel_drive_controller/CMakeLists.txt index 1f5006ea47..b3d8ef8a48 100644 --- a/multi_omni_wheel_drive_controller/CMakeLists.txt +++ b/multi_omni_wheel_drive_controller/CMakeLists.txt @@ -1,11 +1,9 @@ cmake_minimum_required(VERSION 3.16) project(multi_omni_wheel_drive_controller) -if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable - -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct - -Werror=missing-braces) -endif() +find_package(ros2_control_cmake REQUIRED) +set_compiler_options() +export_windows_symbols() # Find dependencies set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/multi_omni_wheel_drive_controller/package.xml b/multi_omni_wheel_drive_controller/package.xml index f81d6bb9b5..fc2191579d 100644 --- a/multi_omni_wheel_drive_controller/package.xml +++ b/multi_omni_wheel_drive_controller/package.xml @@ -9,9 +9,11 @@ ament_cmake + generate_parameter_library + ros2_control_cmake + controller_interface eigen - generate_parameter_library geometry_msgs hardware_interface nav_msgs 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 index 82d7903462..fec9408033 100644 --- 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 @@ -549,17 +549,14 @@ MultiOmniWheelDriveController::on_export_reference_interfaces() std::vector reference_interfaces; reference_interfaces.reserve(reference_interfaces_.size()); - reference_interfaces.push_back(hardware_interface::CommandInterface( - get_node()->get_name(), std::string("linear/x/") + hardware_interface::HW_IF_VELOCITY, - &reference_interfaces_[0])); + std::vector reference_interface_names = {"/linear/x", "/linear/y", "/angular/z"}; - reference_interfaces.push_back(hardware_interface::CommandInterface( - get_node()->get_name(), std::string("linear/y/") + hardware_interface::HW_IF_VELOCITY, - &reference_interfaces_[1])); - - reference_interfaces.push_back(hardware_interface::CommandInterface( - get_node()->get_name(), std::string("angular/z/") + hardware_interface::HW_IF_VELOCITY, - &reference_interfaces_[2])); + 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; } 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 index 2bf165ca2d..e65fc3305c 100644 --- 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 @@ -72,11 +72,13 @@ TEST_F( for (size_t i = 0; i < reference_interface_names.size(); ++i) { - const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + - std::string("/") + reference_interface_names[i]; - EXPECT_EQ(reference_interfaces[i]->get_name(), ref_itf_name); - EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(reference_interfaces[i]->get_interface_name(), reference_interface_names[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); } } 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 index a5bc9e971e..ec22828806 100644 --- 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 @@ -232,9 +232,7 @@ class MultiOmniWheelDriveControllerFixture : public ::testing::Test std::vector state_itfs_; std::vector command_itfs_; - std::vector reference_interface_names = { - std::string("linear/x/") + HW_IF_VELOCITY, std::string("linear/y/") + HW_IF_VELOCITY, - std::string("angular/z/") + HW_IF_VELOCITY}; + std::vector reference_interface_names = {"linear/x", "linear/y", "angular/z"}; std::unique_ptr controller_; rclcpp::Node::SharedPtr cmd_vel_publisher_node_;