diff --git a/doc/images/six_wheeler_steering.drawio b/doc/images/six_wheeler_steering.drawio new file mode 100644 index 0000000000..7e125b56b8 --- /dev/null +++ b/doc/images/six_wheeler_steering.drawio @@ -0,0 +1,164 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/doc/images/six_wheeler_steering.svg b/doc/images/six_wheeler_steering.svg new file mode 100644 index 0000000000..9201e493ee --- /dev/null +++ b/doc/images/six_wheeler_steering.svg @@ -0,0 +1,4 @@ + + + +

$$\sqrt{d_2^2+(r-d_1)^2}$$
$$\sqrt{d_2^2+(r+d_1)^2}$$
D4
D4
D1
D1
D2
D2
D3
D3
R
R \ No newline at end of file diff --git a/doc/six_wheel_rover_controller.rst b/doc/six_wheel_rover_controller.rst new file mode 100644 index 0000000000..074c41db08 --- /dev/null +++ b/doc/six_wheel_rover_controller.rst @@ -0,0 +1,133 @@ +.. _doc_six_wheel_rover_controller: + +six_wheel_rover_controller +============================= + +This page introduces the kinematic model for a six-wheeled rover featuring four-wheel steering, commonly used in rocker-bogie style vehicles. + +Kinematics of the system +------------------------ + +The controller uses a standard Ackermann-style kinematic model adapted for a six-wheel configuration. The core principle is that to avoid wheel slip during a turn, the axes of all wheels must intersect at a single point known as the **Instantaneous Center of Rotation (ICR)**. + +The parameters shown in the diagram are essential for the kinematic calculations. + +Diagram +------------------ +.. image:: images/six_wheeler_steering.svg + :width: 600px + :align: center + :alt: Six-wheel steering controller kinematic diagram + +Inverse Kinematics +------------------ + +Inverse kinematics determine the required joint commands from a desired vehicle body velocity (a ``Twist`` message). + +**Steering Joint Angles** + +Given a desired turning radius, :math:`R`, the required steering angles for the inner and outer front wheels are calculated as follows: + +.. math:: + + \theta_{inner} = \arctan\left(\frac{d_3}{|R| - d_1}\right) + +.. math:: + + \theta_{outer} = \arctan\left(\frac{d_3}{|R| + d_1}\right) + +The rear wheels are commanded to the same angle magnitudes but in the opposite direction to articulate the turn. + +**Drive Wheel Velocities** + +The velocity of each drive wheel is proportional to its distance from the ICR. The vehicle's angular velocity, :math:`\omega_{center}`, is first determined from the desired linear velocity, :math:`v`: + +.. math:: + + \omega_{center} = \frac{v}{R} + +The linear velocity for each wheel (:math:`v_{wheel}`) is then found based on its unique turning radius. This is finally converted to the motor's angular velocity command, :math:`\omega_{wheel}`: + +.. math:: + + \omega_{wheel} = \frac{v_{wheel}}{r_{wheel}} + +Forward Kinematics +------------------ + +Forward kinematics estimate the vehicle's current ``Twist`` from its joint states. The rover's linear velocity, :math:`v_x`, is estimated from the average velocity of the two non-steerable middle wheels. + +.. math:: + + v_x = \frac{(\omega_{left\_middle} + \omega_{right\_middle})}{2} \cdot r_{wheel} + +The vehicle's angular velocity, :math:`\omega_z`, is then derived from this linear velocity and an estimation of the current turning radius, :math:`R_{approx}`. + +.. math:: + + \omega_z = \frac{v_x}{R_{approx}} + +Controller Parameters +--------------------- + +.. list-table:: Controller Parameters + :widths: 20 80 + :header-rows: 1 + + * - Parameter + - Description + * - ``traction_joints`` + - A list of the six drive wheel joint names from the robot's URDF model, in the following order: front-left, front-right, middle-left, middle-right, rear-left, rear-right. + * - ``steering_joints`` + - A list of the four corner steering joint names from the robot's URDF model, in the following order: front-left, front-right, rear-left, rear-right. + * - ``d1`` + - Half the front/rear track width (meters). Lateral distance from the longitudinal center to the steering pivot. + * - ``d2`` + - Longitudinal distance from rover center to the rear axle (meters). + * - ``d3`` + - Longitudinal distance from rover center to the front axle (meters). + * - ``d4`` + - Half the middle track width (meters). Lateral distance from the center to the middle wheel's center. + * - ``wheel_radius`` + - Radius of the drive wheels (meters). + * - ``odom_frame_id`` + - The name of the odometry frame. Default: ``odom``. + * - ``base_frame_id`` + - The name of the robot's base frame. Default: ``base_link``. + +Example Configuration +--------------------- + +.. code-block:: yaml + + six_wheel_steering_controller: + ros__parameters: + + # ---- JOINT CONFIGURATION ---- + # [ACTION REQUIRED] Replace with the exact joint names from your robot's URDF. + traction_joints: [ + "front_left_wheel_joint", + "middle_left_wheel_joint", + "rear_left_wheel_joint", + "front_right_wheel_joint", + "middle_right_wheel_joint", + "rear_right_wheel_joint" + ] + steering_joints: [ + "front_left_steer_joint", + "rear_left_steer_joint", + "front_right_steer_joint", + "rear_right_steer_joint" + ] + + # ---- KINEMATIC PARAMETERS (in meters) ---- + # [ACTION REQUIRED] Measure these values from your specific rover. + d1: 0.4 + d2: 0.5 + d3: 0.5 + d4: 0.45 + wheel_radius: 0.15 + + # ---- ODOMETRY CONFIGURATION ---- + odom_frame_id: "odom" + base_frame_id: "base_link" diff --git a/generic_steering_controller/CMakeLists.txt b/generic_steering_controller/CMakeLists.txt new file mode 100644 index 0000000000..f8a1eef2a5 --- /dev/null +++ b/generic_steering_controller/CMakeLists.txt @@ -0,0 +1,139 @@ +cmake_minimum_required(VERSION 3.16) +project(generic_steering_controller) + +find_package(ros2_control_cmake REQUIRED) +set_compiler_options() +export_windows_symbols() + +# --- Dependencies --- +set(THIS_PACKAGE_INCLUDE_DEPENDS + controller_interface + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + control_msgs + geometry_msgs + nav_msgs + tf2_msgs + tf2_geometry_msgs + std_msgs + lifecycle_msgs + generate_parameter_library +) + +find_package(ament_cmake REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +# --- Parameter Library --- +generate_parameter_library(generic_steering_controller_parameters + config/generic_steering_controller_parameters.yaml +) + +# --- Main Controller Library --- +add_library(generic_steering_controller SHARED + src/generic_steering_controller.cpp +) +target_compile_features(generic_steering_controller PUBLIC cxx_std_17) +target_include_directories(generic_steering_controller PUBLIC + "$" + "$" +) +target_link_libraries(generic_steering_controller PUBLIC + generic_steering_controller_parameters + controller_interface::controller_interface + hardware_interface::hardware_interface + pluginlib::pluginlib + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + realtime_tools::realtime_tools + ${control_msgs_TARGETS} + ${geometry_msgs_TARGETS} + ${nav_msgs_TARGETS} + ${tf2_msgs_TARGETS} + ${tf2_geometry_msgs_TARGETS} + ${std_msgs_TARGETS} + ${lifecycle_msgs_TARGETS} +) + +# --- Plugin Export for the REAL Controller --- +pluginlib_export_plugin_description_file(controller_interface generic_steering_controller.xml) + +# --- Testing Block --- +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + + # Build the Mock Plugin as a separate SHARED library + add_library(mock_kinematic_plugin SHARED + test/mock_kinematic_plugin/mock_kinematic_plugin.cpp + ) + target_include_directories(mock_kinematic_plugin PUBLIC + $ + $ + $ + ) + target_link_libraries(mock_kinematic_plugin PUBLIC + pluginlib::pluginlib + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + ${nav_msgs_TARGETS} + ${control_msgs_TARGETS} + ${geometry_msgs_TARGETS} + ) + + # Build the Test Executable + ament_add_gmock( + test_generic_steering_controller + test/test_generic_steering_controller.cpp + test/test_generic_steering_controller.hpp + ) + target_include_directories(test_generic_steering_controller PRIVATE + $ + $ + ) + target_link_libraries(test_generic_steering_controller + generic_steering_controller + generic_steering_controller_parameters + ) + + # Install the mock plugin so it can be found by pluginlib + install(TARGETS mock_kinematic_plugin + LIBRARY DESTINATION lib + ) + + # Export the mock plugin description + pluginlib_export_plugin_description_file( + generic_steering_controller + test/mock_kinematic_plugin/mock_kinematic_plugin.xml + ) +endif() +# --- Install Rules --- +install( + DIRECTORY include/ + DESTINATION include +) + +install( + TARGETS + generic_steering_controller + generic_steering_controller_parameters + EXPORT export_generic_steering_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +# Install the REAL controller's plugin description +install( + FILES generic_steering_controller.xml + DESTINATION share/${PROJECT_NAME} +) + +# --- Ament Export --- +ament_export_include_directories(include) +ament_export_targets(export_generic_steering_controller HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/generic_steering_controller/config/generic_steering_controller_parameters.yaml b/generic_steering_controller/config/generic_steering_controller_parameters.yaml new file mode 100644 index 0000000000..3887b0d13a --- /dev/null +++ b/generic_steering_controller/config/generic_steering_controller_parameters.yaml @@ -0,0 +1,61 @@ +generic_steering_controller_parameters: + plugin_name: + type: string + default_value: "mock_kinematic_plugin/MockKinematicModel" + description: "Name of the kinematics plugin to use" + ref_timeout: + type: double + default_value: 0.5 + description: "Timeout for reference commands (seconds)" + enable_odom_tf: + type: bool + default_value: true + description: "Whether to publish odometry as TF" + wheelbase: + type: double + default_value: 1.0 + description: "Wheelbase for kinematic plugin (meters)" + wheel_radius: + type: double + default_value: 0.15 + description: "Wheel radius for kinematic plugin (meters)" + odom_frame_id: + type: string + default_value: "odom" + description: "Frame ID for odometry" + base_frame_id: + type: string + default_value: "base_link" + description: "Frame ID for robot base" + open_loop: + type: bool + default_value: false + description: "Use open loop kinematics" + reduce_wheel_speed_until_steering_reached: + type: bool + default_value: false + description: "Reduce wheel speed until steering angle is reached" + position_feedback: + type: bool + default_value: false + description: "Use position feedback for traction wheels" + traction_joints_names: + type: string_array + default_value: ["left_wheel_joint", "right_wheel_joint"] + description: "Names of the traction (drive) joints" + steering_joints_names: + type: string_array + default_value: ["steering_joint"] + description: "Names of the steering joints" + pose_covariance_diagonal: { + type: double_array, + default_value: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0], + description: "diagonal values of pose covariance matrix.", + read_only: false, + } + twist_covariance_diagonal: { + type: double_array, + default_value: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0], + description: "diagonal values of twist covariance matrix.", + read_only: false, + } diff --git a/generic_steering_controller/generic_steering_controller.xml b/generic_steering_controller/generic_steering_controller.xml new file mode 100644 index 0000000000..8f110a1fcc --- /dev/null +++ b/generic_steering_controller/generic_steering_controller.xml @@ -0,0 +1,10 @@ + + + + Generic, plugin-based steering controller for arbitrary kinematics. + + + diff --git a/generic_steering_controller/include/generic_steering_controller/generic_steering_controller.hpp b/generic_steering_controller/include/generic_steering_controller/generic_steering_controller.hpp new file mode 100644 index 0000000000..05f624d358 --- /dev/null +++ b/generic_steering_controller/include/generic_steering_controller/generic_steering_controller.hpp @@ -0,0 +1,166 @@ +// Copyright (c) 2025 Berkan Tali +// +// 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 GENERIC_STEERING_CONTROLLER__GENERIC_STEERING_CONTROLLER_HPP_ +#define GENERIC_STEERING_CONTROLLER__GENERIC_STEERING_CONTROLLER_HPP_ + +// C++ Standard Library +#include +#include +#include +#include + +// ros2_control and controller_interface +#include "controller_interface/chainable_controller_interface.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "lifecycle_msgs/msg/state.hpp" +// pluginlib +#include "pluginlib/class_loader.hpp" + +#include +#include +#include "control_msgs/msg/steering_controller_status.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "tf2_msgs/msg/tf_message.hpp" + +// rclcpp and realtime_tools +#include "rclcpp/duration.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_box.hpp" +#include "realtime_tools/realtime_publisher.hpp" +#include "realtime_tools/realtime_thread_safe_box.hpp" +// Project-specific +#include "generic_steering_controller/generic_steering_controller_parameters.hpp" +#include "generic_steering_controller/kinematic_model_base.hpp" + +namespace generic_steering_controller +{ +/** + * @brief A generic steering controller that uses kinematic plugins. + * + * This controller subscribes to a command topic and uses a dynamically loaded + * plugin to calculate wheel and steering commands for various robot kinematics. + */ +class GenericSteeringController : public controller_interface::ChainableControllerInterface +{ +public: + GenericSteeringController(); + + virtual void initialize_implementation_parameter_listener(); + + 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 configure_odometry(); + + 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; + + using ControllerTwistReferenceMsg = geometry_msgs::msg::TwistStamped; + using ControllerStateMsgOdom = nav_msgs::msg::Odometry; + using ControllerStateMsgTf = tf2_msgs::msg::TFMessage; + using GenericSteeringControllerStateMsg = control_msgs::msg::SteeringControllerStatus; + + inline std::unordered_map get_state_interface_map() const + { + std::unordered_map current_map; + for (const auto & iface : state_interfaces_) + { + current_map[iface.get_name()] = iface.get_value(); + } + return current_map; + } + +protected: + controller_interface::CallbackReturn set_interface_numbers( + size_t nr_state_itfs, size_t nr_cmd_itfs, size_t nr_ref_itfs); + + std::shared_ptr param_listener_; + generic_steering_controller_parameters::Params params_; + + // the RT Box containing the command message + realtime_tools::RealtimeThreadSafeBox input_ref_; + ControllerTwistReferenceMsg current_ref_; + rclcpp::Duration ref_timeout_ = rclcpp::Duration::from_seconds(0.0); // 0ms + + // Command subscribers and Controller State publisher + rclcpp::Subscription::SharedPtr ref_subscriber_twist_ = nullptr; + using ControllerStatePublisherOdom = realtime_tools::RealtimePublisher; + using ControllerStatePublisherTf = realtime_tools::RealtimePublisher; + + rclcpp::Publisher::SharedPtr odom_s_publisher_; + rclcpp::Publisher::SharedPtr tf_odom_s_publisher_; + + std::unique_ptr rt_odom_state_publisher_; + std::unique_ptr rt_tf_odom_state_publisher_; + + std::vector on_export_reference_interfaces() override; + + bool on_set_chained_mode(bool chained_mode) override; + + /// Odometry: + + GenericSteeringControllerStateMsg published_state_; + + using ControllerStatePublisher = + realtime_tools::RealtimePublisher; + rclcpp::Publisher::SharedPtr controller_s_publisher_; + std::unique_ptr controller_state_publisher_; + + // name constants for state interfaces + size_t nr_state_itfs_; + // name constants for command interfaces + size_t nr_cmd_itfs_; + // name constants for reference interfaces + size_t nr_ref_itfs_; + + // last velocity commands for open loop odometry + double last_linear_velocity_ = 0.0; + double last_angular_velocity_ = 0.0; + + std::vector traction_joints_state_names_; + std::vector steering_joints_state_names_; + bool open_loop_; + + void reference_callback(const std::shared_ptr msg); + +private: + // callback for topic interface + + pluginlib::ClassLoader kinematic_loader; + std::shared_ptr kinematic_model_; + std::unordered_map state_map; + std::string kinematic_plugin_name; +}; + +} // namespace generic_steering_controller + +#endif // GENERIC_STEERING_CONTROLLER__GENERIC_STEERING_CONTROLLER_HPP_ diff --git a/generic_steering_controller/include/generic_steering_controller/kinematic_model_base.hpp b/generic_steering_controller/include/generic_steering_controller/kinematic_model_base.hpp new file mode 100644 index 0000000000..718a9461aa --- /dev/null +++ b/generic_steering_controller/include/generic_steering_controller/kinematic_model_base.hpp @@ -0,0 +1,113 @@ +// Copyright (c) 2025 Berkan Tali +// +// 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 GENERIC_STEERING_CONTROLLER__KINEMATIC_MODEL_BASE_HPP_ +#define GENERIC_STEERING_CONTROLLER__KINEMATIC_MODEL_BASE_HPP_ + +// C++ Standard Library +#include +#include +#include +#include + +// ROS Messages +#include "control_msgs/msg/steering_controller_status.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "nav_msgs/msg/odometry.hpp" + +// rclcpp +#include "rclcpp/duration.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +namespace kinematic_model +{ +/** + * @brief Abstract base class for kinematic model plugins. + * + * This class defines the standard interface for all kinematic models that can be loaded + * by the GenericSteeringController. + */ +class KinematicModelBase +{ +public: + virtual ~KinematicModelBase() = default; + + /** + * @brief Configure the kinematic model plugin. + * @param node The node interface for the controller. + * @param traction_joint_names A list of names for the traction (drive) joints. + * @param steering_joint_names A list of names for the steering joints. + */ + virtual void configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, + const std::vector & traction_joint_names, + const std::vector & steering_joint_names) = 0; + + /** + * @brief Update the internal reference command from the subscriber. + * @param twist_msg The command message from the subscriber. + * @param time The current time. + */ + virtual void update_reference( + const geometry_msgs::msg::Twist & twist_msg, const rclcpp::Time & time) = 0; + /** + * @brief Get the latest odometry message calculated by the plugin. + * @param time The current time. + * @return A unique_ptr to the odometry message. + */ + virtual std::unique_ptr get_odometry_message( + const rclcpp::Duration & time) = 0; + + /** + * \brief Calculates inverse kinematics for the desired linear and angular velocities + * \param v_bx Desired linear velocity of the robot in x_b-axis direction + * \param omega_bz Desired angular velocity of the robot around x_z-axis + * \param open_loop If false, the IK will be calculated using measured steering angle + * \param reduce_wheel_speed_until_steering_reached Reduce wheel speed until the steering angle + * has been reached + * \return Tuple of velocity commands and steering commands + */ + virtual std::tuple, std::vector> get_commands( + const double v_bx, const double omega_bz, const bool open_loop = true, + const bool reduce_wheel_speed_until_steering_reached = false) = 0; + + /** + * @brief Update all joint states at once. + * @param all_states Vector containing all measured joint states (traction first, then steering). + */ + virtual void update_states(const std::unordered_map & all_states) + { + states = all_states; + } + +protected: + // Node for plugins to get parameters, etc. + rclcpp_lifecycle::LifecycleNode::SharedPtr node_; + + // Store joint names for later use + std::vector traction_joint_names_; + std::vector steering_joint_names_; + + // Store the latest state from the hardware + std::vector traction_states_; + std::vector steering_states_; + + std::unordered_map states; +}; + +} // namespace kinematic_model + +#endif // GENERIC_STEERING_CONTROLLER__KINEMATIC_MODEL_BASE_HPP_ diff --git a/generic_steering_controller/package.xml b/generic_steering_controller/package.xml new file mode 100644 index 0000000000..9df95c36d5 --- /dev/null +++ b/generic_steering_controller/package.xml @@ -0,0 +1,35 @@ + + + + generic_steering_controller + 0.0.0 + A generic, plugin-based steering controller for various vehicle kinematics + silanus23 + + Apache-2.0 + + ament_cmake + + controller_interface + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + control_msgs + geometry_msgs + nav_msgs + tf2_msgs + tf2_geometry_msgs + std_msgs + lifecycle_msgs + generate_parameter_library + ament_lint_auto + ament_lint_common + ament_cmake_gmock + + + ament_cmake + + + diff --git a/generic_steering_controller/src/generic_steering_controller.cpp b/generic_steering_controller/src/generic_steering_controller.cpp new file mode 100644 index 0000000000..a651522e7d --- /dev/null +++ b/generic_steering_controller/src/generic_steering_controller.cpp @@ -0,0 +1,572 @@ +// Copyright (c) 2025 Berkan Tali +// +// 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 "generic_steering_controller/generic_steering_controller.hpp" +#include + +#include +#include +#include +#include +#include + +#include +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +namespace +{ // utility + +using ControllerTwistReferenceMsg = + generic_steering_controller::GenericSteeringController::ControllerTwistReferenceMsg; + +void reset_controller_reference_msg( + ControllerTwistReferenceMsg & msg, const std::shared_ptr & node) +{ + msg.header.stamp = node->now(); + msg.twist.linear.x = std::numeric_limits::quiet_NaN(); + msg.twist.linear.y = std::numeric_limits::quiet_NaN(); + 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 = std::numeric_limits::quiet_NaN(); +} + +} // namespace + +namespace generic_steering_controller +{ +GenericSteeringController::GenericSteeringController() +: controller_interface::ChainableControllerInterface(), + kinematic_loader("generic_steering_controller", "kinematic_model::KinematicModelBase") +{ +} + +controller_interface::CallbackReturn GenericSteeringController::on_init() +{ + try + { + param_listener_ = + std::make_shared(get_node()); + initialize_implementation_parameter_listener(); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during controller's init with message: %s \n", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn GenericSteeringController::set_interface_numbers( + size_t nr_state_itfs = 2, size_t nr_cmd_itfs = 2, size_t nr_ref_itfs = 2) +{ + nr_state_itfs_ = nr_state_itfs; + nr_cmd_itfs_ = nr_cmd_itfs; + nr_ref_itfs_ = nr_ref_itfs; + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn GenericSteeringController::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + params_ = param_listener_->get_params(); + + if (params_.traction_joints_names.empty()) + { + RCLCPP_ERROR(get_node()->get_logger(), "No traction joints specified!"); + return controller_interface::CallbackReturn::ERROR; + } + if (params_.steering_joints_names.empty()) + { + RCLCPP_ERROR(get_node()->get_logger(), "No steering joints specified!"); + return controller_interface::CallbackReturn::ERROR; + } + RCLCPP_INFO(get_node()->get_logger(), "Params created"); + + traction_joints_state_names_ = params_.traction_joints_names; + steering_joints_state_names_ = params_.steering_joints_names; + + auto subscribers_qos = rclcpp::SystemDefaultsQoS(); + subscribers_qos.keep_last(1); + subscribers_qos.best_effort(); + + ref_timeout_ = rclcpp::Duration::from_seconds(params_.ref_timeout); + ref_subscriber_twist_ = get_node()->create_subscription( + "~/reference", subscribers_qos, + std::bind(&GenericSteeringController::reference_callback, this, std::placeholders::_1)); + + reset_controller_reference_msg(current_ref_, get_node()); + input_ref_.set(current_ref_); + + try + { + odom_s_publisher_ = get_node()->create_publisher( + "~/odometry", rclcpp::SystemDefaultsQoS()); + rt_odom_state_publisher_ = std::make_unique(odom_s_publisher_); + } + catch (const std::exception & e) + { + fprintf( + stderr, "Exception thrown during publisher creation at configure stage with message : %s \n", + e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + rt_odom_state_publisher_->lock(); + rt_odom_state_publisher_->msg_.header.stamp = get_node()->now(); + rt_odom_state_publisher_->msg_.header.frame_id = params_.odom_frame_id; + rt_odom_state_publisher_->msg_.child_frame_id = params_.base_frame_id; + rt_odom_state_publisher_->msg_.pose.pose.position.z = 0; + rt_odom_state_publisher_->unlock(); + + auto & covariance = rt_odom_state_publisher_->msg_.twist.covariance; + 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; + covariance[diagonal_index] = params_.pose_covariance_diagonal[index]; + covariance[diagonal_index] = params_.twist_covariance_diagonal[index]; + } + + try + { + // Tf State publisher + tf_odom_s_publisher_ = get_node()->create_publisher( + "~/tf_odometry", rclcpp::SystemDefaultsQoS()); + rt_tf_odom_state_publisher_ = + std::make_unique(tf_odom_s_publisher_); + } + catch (const std::exception & e) + { + fprintf( + stderr, "Exception thrown during publisher creation at configure stage with message : %s \n", + e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + rt_tf_odom_state_publisher_->lock(); + rt_tf_odom_state_publisher_->msg_.transforms.resize(1); + rt_tf_odom_state_publisher_->msg_.transforms[0].header.stamp = get_node()->now(); + rt_tf_odom_state_publisher_->msg_.transforms[0].header.frame_id = params_.odom_frame_id; + rt_tf_odom_state_publisher_->msg_.transforms[0].child_frame_id = params_.base_frame_id; + rt_tf_odom_state_publisher_->msg_.transforms[0].transform.translation.z = 0.0; + rt_tf_odom_state_publisher_->unlock(); + + try + { + // State publisher + controller_s_publisher_ = get_node()->create_publisher( + "~/controller_state", rclcpp::SystemDefaultsQoS()); + controller_state_publisher_ = + std::make_unique(controller_s_publisher_); + } + catch (const std::exception & e) + { + fprintf( + stderr, "Exception thrown during publisher creation at configure stage with message : %s \n", + e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + controller_state_publisher_->lock(); + controller_state_publisher_->msg_.header.stamp = get_node()->now(); + controller_state_publisher_->msg_.header.frame_id = params_.odom_frame_id; + controller_state_publisher_->unlock(); + RCLCPP_INFO(get_node()->get_logger(), "configure successful"); + size_t total_state_interfaces = + traction_joints_state_names_.size() + steering_joints_state_names_.size(); + size_t total_command_interfaces = + params_.traction_joints_names.size() + params_.steering_joints_names.size(); + + set_interface_numbers(total_state_interfaces, total_command_interfaces, 2); + open_loop_ = params_.open_loop; + try + { + kinematic_model_ = kinematic_loader.createSharedInstance(params_.plugin_name); + kinematic_model_->configure( + std::dynamic_pointer_cast(get_node()), + params_.traction_joints_names, params_.steering_joints_names); + } + catch (const std::exception & e) + { + RCLCPP_ERROR(get_node()->get_logger(), "Failed to load kinematic plugin: %s", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +void GenericSteeringController::reference_callback( + const std::shared_ptr msg) +{ + if (msg->header.stamp.sec == 0 && msg->header.stamp.nanosec == 0u) + { + RCLCPP_WARN( + get_node()->get_logger(), + "Timestamp in header is missing, using current time as command timestamp."); + msg->header.stamp = get_node()->now(); + } + const auto age_of_last_command = get_node()->now() - msg->header.stamp; + + if (ref_timeout_ == rclcpp::Duration::from_seconds(0) || age_of_last_command <= ref_timeout_) + { + input_ref_.set(*msg); + } + else + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Received message has timestamp %.10f older for %.10f which is more then allowed timeout " + "(%.4f).", + rclcpp::Time(msg->header.stamp).seconds(), age_of_last_command.seconds(), + ref_timeout_.seconds()); + } +} + +controller_interface::InterfaceConfiguration +GenericSteeringController::command_interface_configuration() const +{ + controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + command_interfaces_config.names.reserve(nr_cmd_itfs_); + for (size_t i = 0; i < params_.traction_joints_names.size(); i++) + { + command_interfaces_config.names.push_back( + params_.traction_joints_names[i] + "/" + hardware_interface::HW_IF_VELOCITY); + } + + for (size_t i = 0; i < params_.steering_joints_names.size(); i++) + { + command_interfaces_config.names.push_back( + params_.steering_joints_names[i] + "/" + hardware_interface::HW_IF_POSITION); + } + return command_interfaces_config; +} + +controller_interface::InterfaceConfiguration +GenericSteeringController::state_interface_configuration() const +{ + RCLCPP_INFO(get_node()->get_logger(), "State interface conf"); + controller_interface::InterfaceConfiguration state_interfaces_config; + state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + state_interfaces_config.names.reserve(nr_state_itfs_); + const auto traction_wheels_feedback = params_.position_feedback + ? hardware_interface::HW_IF_POSITION + : hardware_interface::HW_IF_VELOCITY; + + for (size_t i = 0; i < traction_joints_state_names_.size(); i++) + { + state_interfaces_config.names.push_back( + traction_joints_state_names_[i] + "/" + traction_wheels_feedback); + } + + // Add steering joint interfaces + for (size_t i = 0; i < steering_joints_state_names_.size(); ++i) + { + state_interfaces_config.names.push_back( + steering_joints_state_names_[i] + "/" + hardware_interface::HW_IF_POSITION); + } + + return state_interfaces_config; + + return state_interfaces_config; +} + +std::vector +GenericSteeringController::on_export_reference_interfaces() +{ + RCLCPP_INFO(get_node()->get_logger(), "ref int exp conf: %zu", nr_ref_itfs_); + reference_interfaces_.resize(nr_ref_itfs_, std::numeric_limits::quiet_NaN()); + + std::vector reference_interfaces; + reference_interfaces.reserve(nr_ref_itfs_); + + reference_interfaces.push_back( + hardware_interface::CommandInterface( + get_node()->get_name() + std::string("/linear"), hardware_interface::HW_IF_VELOCITY, + &reference_interfaces_[0])); + + reference_interfaces.push_back( + hardware_interface::CommandInterface( + get_node()->get_name() + std::string("/angular"), hardware_interface::HW_IF_VELOCITY, + &reference_interfaces_[1])); + + return reference_interfaces; +} + +bool GenericSteeringController::on_set_chained_mode(bool /*chained_mode*/) { return true; } + +controller_interface::CallbackReturn GenericSteeringController::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // Reset reference message to NaN + reset_controller_reference_msg(current_ref_, get_node()); + input_ref_.try_set(current_ref_); + + // Set all command interfaces to NaN for safety + for (auto & command_interface : command_interfaces_) + { + if (!command_interface.set_value(std::numeric_limits::quiet_NaN())) + { + RCLCPP_WARN( + get_node()->get_logger(), + "Failed to set NaN value for command interface '%s' during activation.", + command_interface.get_name().c_str()); + } + } + + RCLCPP_INFO(get_node()->get_logger(), "GenericSteeringController activated"); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn GenericSteeringController::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // Set all command interfaces to NaN for safety + for (auto & command_interface : command_interfaces_) + { + if (!command_interface.set_value(std::numeric_limits::quiet_NaN())) + { + RCLCPP_WARN( + get_node()->get_logger(), + "Failed to set NaN value for command interface '%s' during deactivation.", + command_interface.get_name().c_str()); + } + } + + RCLCPP_INFO(get_node()->get_logger(), "GenericSteeringController deactivated"); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type GenericSteeringController::update_reference_from_subscribers( + const rclcpp::Time & time, const rclcpp::Duration & /*period*/) +{ + auto current_ref_msg = input_ref_.try_get(); + if (current_ref_msg.has_value()) + { + current_ref_ = current_ref_msg.value(); + } + const auto age_of_last_command = time - current_ref_.header.stamp; + + if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0)) + { + if (!std::isnan(current_ref_.twist.linear.x) && !std::isnan(current_ref_.twist.linear.y)) + { + reference_interfaces_[0] = current_ref_.twist.linear.x; + reference_interfaces_[1] = current_ref_.twist.angular.z; + + if (ref_timeout_ == rclcpp::Duration::from_seconds(0)) + { + current_ref_.twist.linear.x = std::numeric_limits::quiet_NaN(); + current_ref_.twist.angular.z = std::numeric_limits::quiet_NaN(); + input_ref_.try_set(current_ref_); + } + } + } + else + { + if (!std::isnan(current_ref_.twist.linear.x) && !std::isnan(current_ref_.twist.angular.z)) + { + reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); + reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); + + current_ref_.twist.linear.x = std::numeric_limits::quiet_NaN(); + current_ref_.twist.angular.z = std::numeric_limits::quiet_NaN(); + input_ref_.try_set(current_ref_); + } + } + return controller_interface::return_type::OK; +} + +// Call values from plugin and pass them to hardware interfaces +controller_interface::return_type GenericSteeringController::update_and_write_commands( + const rclcpp::Time & time, const rclcpp::Duration & period) +{ + // Check kinematic plugin + if (!kinematic_model_) + { + RCLCPP_ERROR(get_node()->get_logger(), "Kinematic model plugin is not loaded!"); + return controller_interface::return_type::ERROR; + } + + // Update kinematic model states if not open loop + if (!open_loop_) + { + state_map = get_state_interface_map(); + kinematic_model_->update_states(state_map); + } + + // If reference is valid, compute and send commands + if (!std::isnan(reference_interfaces_[0]) && !std::isnan(reference_interfaces_[1])) + { + auto [traction_commands, steering_commands] = kinematic_model_->get_commands( + reference_interfaces_[0], reference_interfaces_[1], params_.open_loop, + params_.reduce_wheel_speed_until_steering_reached); + + // Write traction commands + for (size_t i = 0; i < params_.traction_joints_names.size(); i++) + { + if (!command_interfaces_[i].set_value(traction_commands[i])) + { + RCLCPP_WARN( + get_node()->get_logger(), "Unable to set traction command at index %zu: value = %f", i, + traction_commands[i]); + } + } + // Write steering commands + for (size_t i = 0; i < params_.steering_joints_names.size(); i++) + { + size_t idx = i + params_.traction_joints_names.size(); + if (!command_interfaces_[idx].set_value(steering_commands[i])) + { + RCLCPP_WARN( + get_node()->get_logger(), "Unable to set steering command at index %zu: value = %f", i, + steering_commands[i]); + } + } + } + else + { + // Reference is invalid: zero traction commands + for (size_t i = 0; i < params_.traction_joints_names.size(); i++) + { + if (!command_interfaces_[i].set_value(0.0)) + { + RCLCPP_WARN( + get_node()->get_logger(), "Unable to set traction command to zero at index %zu", i); + } + } + // Optionally zero steering commands (uncomment if desired) + // for (size_t i = 0; i < params_.steering_joints_names.size(); i++) { + // size_t idx = i + params_.traction_joints_names.size(); + // if (!command_interfaces_[idx].set_value(0.0)) { + // RCLCPP_WARN(get_node()->get_logger(), + // "Unable to set steering command to zero at index %zu", i); + // } + // } + } + + // Publish controller state message + if (controller_state_publisher_->trylock()) + { + controller_state_publisher_->msg_.header.stamp = time; + controller_state_publisher_->msg_.traction_wheels_position.clear(); + controller_state_publisher_->msg_.traction_wheels_velocity.clear(); + controller_state_publisher_->msg_.traction_command.clear(); + controller_state_publisher_->msg_.steer_positions.clear(); + controller_state_publisher_->msg_.steering_angle_command.clear(); + + auto number_of_traction_wheels = params_.traction_joints_names.size(); + auto number_of_steering_wheels = params_.steering_joints_names.size(); + + // Read state interfaces using get_optional and log missing values + for (size_t i = 0; i < number_of_traction_wheels; ++i) + { + auto value_op = state_interfaces_[i].get_optional(); + if (params_.position_feedback) + { + controller_state_publisher_->msg_.traction_wheels_position.push_back( + value_op.value_or(0.0)); + } + else + { + controller_state_publisher_->msg_.traction_wheels_velocity.push_back( + value_op.value_or(0.0)); + } + if (!value_op.has_value()) + { + RCLCPP_DEBUG( + get_node()->get_logger(), "Unable to retrieve %s feedback data for traction wheel %zu", + params_.position_feedback ? "position" : "velocity", i); + } + } + + for (size_t i = 0; i < number_of_steering_wheels; ++i) + { + size_t steering_base_index = number_of_traction_wheels; + auto steer_state_op = state_interfaces_[steering_base_index + i].get_optional(); + controller_state_publisher_->msg_.steer_positions.push_back(steer_state_op.value_or(0.0)); + if (!steer_state_op.has_value()) + { + RCLCPP_DEBUG( + get_node()->get_logger(), + "Unable to retrieve position feedback data for steering wheel %zu", i); + } + + auto steer_cmd_op = command_interfaces_[number_of_traction_wheels + i].get_optional(); + controller_state_publisher_->msg_.steering_angle_command.push_back( + steer_cmd_op.value_or(0.0)); + if (!steer_cmd_op.has_value()) + { + RCLCPP_DEBUG( + get_node()->get_logger(), + "Unable to retrieve command interface value for steering wheel %zu", i); + } + } + + controller_state_publisher_->unlockAndPublish(); + } + + // Publish odometry and tf messages (unchanged) + auto odom_msg = kinematic_model_->get_odometry_message(period); + const auto & q = odom_msg->pose.pose.orientation; + tf2::Quaternion quat(q.x, q.y, q.z, q.w); + double roll, pitch, yaw; + tf2::Matrix3x3(quat).getRPY(roll, pitch, yaw); + + tf2::Quaternion orientation; + orientation.setRPY(0.0, 0.0, yaw); + + if (rt_odom_state_publisher_->trylock()) + { + rt_odom_state_publisher_->msg_.header = odom_msg->header; + rt_odom_state_publisher_->msg_.pose = odom_msg->pose; + rt_odom_state_publisher_->unlockAndPublish(); + } + + if (params_.enable_odom_tf && rt_tf_odom_state_publisher_->trylock()) + { + rt_tf_odom_state_publisher_->msg_.transforms.front().header.stamp = time; + rt_tf_odom_state_publisher_->msg_.transforms.front().transform.translation.x = + odom_msg->pose.pose.position.x; + rt_tf_odom_state_publisher_->msg_.transforms.front().transform.translation.y = + odom_msg->pose.pose.position.y; + rt_tf_odom_state_publisher_->msg_.transforms.front().transform.rotation = + odom_msg->pose.pose.orientation; + rt_tf_odom_state_publisher_->unlockAndPublish(); + } + + // Clear reference after use + reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); + reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); + + return controller_interface::return_type::OK; +} + +void GenericSteeringController::initialize_implementation_parameter_listener() +{ + RCLCPP_DEBUG(get_node()->get_logger(), "Generic steering controller parameters initialized"); +} + +} // namespace generic_steering_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + generic_steering_controller::GenericSteeringController, + controller_interface::ChainableControllerInterface) diff --git a/generic_steering_controller/test/mock_kinematic_plugin/mock_kinematic_plugin.cpp b/generic_steering_controller/test/mock_kinematic_plugin/mock_kinematic_plugin.cpp new file mode 100644 index 0000000000..961f725644 --- /dev/null +++ b/generic_steering_controller/test/mock_kinematic_plugin/mock_kinematic_plugin.cpp @@ -0,0 +1,19 @@ +// Copyright (c) 2025 Berkan Tali +// +// 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 "mock_kinematic_plugin.hpp" +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + mock_kinematic_plugin::MockKinematicModel, kinematic_model::KinematicModelBase) diff --git a/generic_steering_controller/test/mock_kinematic_plugin/mock_kinematic_plugin.hpp b/generic_steering_controller/test/mock_kinematic_plugin/mock_kinematic_plugin.hpp new file mode 100644 index 0000000000..8064614cae --- /dev/null +++ b/generic_steering_controller/test/mock_kinematic_plugin/mock_kinematic_plugin.hpp @@ -0,0 +1,57 @@ +// Copyright (c) 2025 Berkan Tali +// +// 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. + +#pragma once + +#include +#include +#include +#include +#include "generic_steering_controller/kinematic_model_base.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +namespace mock_kinematic_plugin +{ + +class MockKinematicModel : public kinematic_model::KinematicModelBase +{ +public: + MockKinematicModel() = default; + ~MockKinematicModel() override = default; + + void update_reference(const geometry_msgs::msg::Twist &, const rclcpp::Time &) override {} + + void configure( + const std::shared_ptr &, const std::vector &, + const std::vector &) override + { + } + + void update_states(const std::unordered_map &) override {} + + std::tuple, std::vector> get_commands( + double linear, double angular, bool, bool) override + { + return std::make_tuple( + std::vector{linear, linear}, std::vector{angular, angular}); + } + + std::unique_ptr get_odometry_message(const rclcpp::Duration &) override + { + return std::make_unique(); + } +}; + +} // namespace mock_kinematic_plugin diff --git a/generic_steering_controller/test/mock_kinematic_plugin/mock_kinematic_plugin.xml b/generic_steering_controller/test/mock_kinematic_plugin/mock_kinematic_plugin.xml new file mode 100644 index 0000000000..b1c3bee1f2 --- /dev/null +++ b/generic_steering_controller/test/mock_kinematic_plugin/mock_kinematic_plugin.xml @@ -0,0 +1,10 @@ + + + + Minimal mock kinematic model for testing. + + + diff --git a/generic_steering_controller/test/test_generic_steering_controller.cpp b/generic_steering_controller/test/test_generic_steering_controller.cpp new file mode 100644 index 0000000000..1016c0fb9f --- /dev/null +++ b/generic_steering_controller/test/test_generic_steering_controller.cpp @@ -0,0 +1,112 @@ +// Copyright (c) 2025 Berkan Tali +// +// 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_generic_steering_controller.hpp" +#include "gtest/gtest.h" + +class GenericSteeringControllerTest +: public GenericSteeringControllerFixture +{ +}; + +TEST_F(GenericSteeringControllerTest, check_exported_interfaces) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size()); + EXPECT_EQ( + cmd_if_conf.names[CMD_TRACTION_RIGHT_WHEEL], + traction_joints_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + cmd_if_conf.names[CMD_TRACTION_LEFT_WHEEL], + traction_joints_names_[1] + "/" + traction_interface_name_); + EXPECT_EQ( + cmd_if_conf.names[CMD_STEER_WHEEL], steering_joints_names_[0] + "/" + steering_interface_name_); + + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size()); + EXPECT_EQ( + state_if_conf.names[STATE_TRACTION_RIGHT_WHEEL], + controller_->traction_joints_state_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + state_if_conf.names[STATE_TRACTION_LEFT_WHEEL], + controller_->traction_joints_state_names_[1] + "/" + traction_interface_name_); + EXPECT_EQ( + state_if_conf.names[STATE_STEER_WHEEL], + controller_->steering_joints_state_names_[0] + "/" + steering_interface_name_); + + auto reference_interfaces = controller_->export_reference_interfaces(); + ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); + for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) + { + const std::string ref_itf_prefix_name = + std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; + EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), ref_itf_prefix_name); + EXPECT_EQ( + reference_interfaces[i]->get_name(), + ref_itf_prefix_name + "/" + hardware_interface::HW_IF_VELOCITY); + EXPECT_EQ(reference_interfaces[i]->get_interface_name(), hardware_interface::HW_IF_VELOCITY); + } +} + +TEST_F(GenericSteeringControllerTest, CommandTimeoutSetsZeroVelocity) +{ + SetUpController(); + controller_->get_node()->set_parameter(rclcpp::Parameter("ref_timeout", 0.5)); + + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + controller_->on_activate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + + auto msg = std::make_shared(); + + auto now = controller_->get_node()->now(); + msg->header.stamp = now - rclcpp::Duration::from_seconds(0.1); // 0.1s ago, within 0.5s timeout + msg->twist.linear.x = 1.0; + msg->twist.angular.z = 0.5; + + // Fresh reference (within timeout) + controller_->reference_callback(msg); + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[1])); + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)); + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[1])); + + // Timeout case (older than timeout) + msg->header.stamp = controller_->get_node()->now() - rclcpp::Duration::from_seconds(1.0); + msg->twist.linear.x = 1.0; + msg->twist.angular.z = 0.5; + controller_->reference_callback(msg); + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[1])); + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)); + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[1])); +} + +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/generic_steering_controller/test/test_generic_steering_controller.hpp b/generic_steering_controller/test/test_generic_steering_controller.hpp new file mode 100644 index 0000000000..a62a8c80fd --- /dev/null +++ b/generic_steering_controller/test/test_generic_steering_controller.hpp @@ -0,0 +1,307 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// 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_GENERIC_STEERING_CONTROLLER_HPP_ +#define TEST_GENERIC_STEERING_CONTROLLER_HPP_ + +#include + +#include +#include +#include +#include +#include + +#include "generic_steering_controller/generic_steering_controller.hpp" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "pluginlib/class_list_macros.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" + +using ControllerStateMsg = + generic_steering_controller::GenericSteeringController::GenericSteeringControllerStateMsg; +using ControllerReferenceMsg = + generic_steering_controller::GenericSteeringController::ControllerTwistReferenceMsg; + +// NOTE: Testing steering_controllers_library for Ackermann vehicle configuration only + +// name constants for state interfaces +static constexpr size_t STATE_TRACTION_RIGHT_WHEEL = 0; +static constexpr size_t STATE_TRACTION_LEFT_WHEEL = 1; +static constexpr size_t STATE_STEER_WHEEL = 2; + +static constexpr size_t CMD_TRACTION_RIGHT_WHEEL = 0; +static constexpr size_t CMD_TRACTION_LEFT_WHEEL = 1; +static constexpr size_t CMD_STEER_WHEEL = 2; + +static constexpr size_t NR_STATE_ITFS = 3; +static constexpr size_t NR_CMD_ITFS = 3; +static constexpr size_t NR_REF_ITFS = 2; + +static constexpr double WHEELBASE_ = 3.24644; +static constexpr double WHEELS_TRACK_ = 2.12321; +static constexpr double WHEELS_RADIUS_ = 0.45; + +namespace +{ +constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; +constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; +} // namespace + +// subclassing and friending so we can access member variables +class TestableGenericSteeringController +: public generic_steering_controller::GenericSteeringController +{ + FRIEND_TEST(GenericSteeringControllerTest, check_exported_interfaces); + FRIEND_TEST(GenericSteeringControllerTest, CommandTimeoutSetsZeroVelocity); + // FRIEND_TEST(SteeringControllersLibraryTest, test_position_feedback_ref_timeout); + // FRIEND_TEST(SteeringControllersLibraryTest, test_velocity_feedback_ref_timeout); + +public: + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override + { + return generic_steering_controller::GenericSteeringController::on_configure(previous_state); + } + + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override + { + auto ref_itfs = on_export_reference_interfaces(); + return generic_steering_controller::GenericSteeringController::on_activate(previous_state); + } + + /** + * @brief wait_for_command blocks until a new ControllerReferenceMsg is received. + * Requires that the executor is not spinned elsewhere between the + * message publication and the call to this function. + */ + void wait_for_command( + 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)); + } + } + + void wait_for_commands( + rclcpp::Executor & executor, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + wait_for_command(executor, timeout); + } + + // implementing methods which are declared virtual in the steering_controllers_library.hpp + void initialize_implementation_parameter_listener() override + { + param_listener_ = + std::make_shared(get_node()); + } +}; + +// We are using template class here for easier reuse of Fixture in specializations of controllers +template +class GenericSteeringControllerFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() {} + + void SetUp() + { + // initialize controller + controller_ = std::make_unique(); + + // Create the node for parameters (this is just for publishing commands) + command_publisher_node_ = std::make_shared("command_publisher"); + + command_publisher_ = command_publisher_node_->create_publisher( + "/test_generic_steering_controller/reference", rclcpp::SystemDefaultsQoS()); + } + + void SetUpController(const std::string controller_name = "test_generic_steering_controller") + { + ASSERT_EQ( + controller_->init(controller_name, "", 0, "", controller_->define_custom_node_options()), + controller_interface::return_type::OK); + + // No parameter setting! Use YAML defaults. + + if (position_feedback_ == true) + { + traction_interface_name_ = "position"; + } + else + { + traction_interface_name_ = "velocity"; + } + + // Manually create interfaces using test fixture values + std::vector command_ifs; + command_itfs_.reserve(joint_command_values_.size()); + command_ifs.reserve(joint_command_values_.size()); + + command_itfs_.emplace_back( + hardware_interface::CommandInterface( + traction_joints_names_[0], traction_interface_name_, + &joint_command_values_[CMD_TRACTION_RIGHT_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + command_itfs_.emplace_back( + hardware_interface::CommandInterface( + traction_joints_names_[1], traction_interface_name_, + &joint_command_values_[CMD_TRACTION_LEFT_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + command_itfs_.emplace_back( + hardware_interface::CommandInterface( + steering_joints_names_[0], steering_interface_name_, + &joint_command_values_[CMD_STEER_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + std::vector state_ifs; + state_itfs_.reserve(joint_state_values_.size()); + state_ifs.reserve(joint_state_values_.size()); + + state_itfs_.emplace_back( + hardware_interface::StateInterface( + traction_joints_names_[0], traction_interface_name_, + &joint_state_values_[STATE_TRACTION_RIGHT_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); + + state_itfs_.emplace_back( + hardware_interface::StateInterface( + traction_joints_names_[1], traction_interface_name_, + &joint_state_values_[STATE_TRACTION_LEFT_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); + + state_itfs_.emplace_back( + hardware_interface::StateInterface( + steering_joints_names_[0], steering_interface_name_, + &joint_state_values_[STATE_STEER_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); + + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + } + void subscribe_and_get_messages(ControllerStateMsg & msg) + { + // create a new subscriber + ControllerStateMsg::SharedPtr received_msg; + rclcpp::Node test_subscription_node("test_subscription_node"); + auto subs_callback = [&](const ControllerStateMsg::SharedPtr cb_msg) { received_msg = cb_msg; }; + auto subscription = test_subscription_node.create_subscription( + "/test_generic_steering_controller/controller_state", 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_subscription_node.get_node_base_interface()); + + // call update to publish the test value + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + // call update to publish the test value + // since update doesn't guarantee a published message, republish until received + int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop + while (max_sub_check_loop_count--) + { + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{5}; + const auto until = test_subscription_node.get_clock()->now() + timeout; + while (!received_msg && test_subscription_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + // check if message has been received + if (received_msg.get()) + { + break; + } + } + ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " + "controller/broadcaster update loop"; + ASSERT_TRUE(received_msg); + + // take message from subscription + msg = *received_msg; + } + + void publish_commands(const double linear = 0.1, const double angular = 0.2) + { + auto wait_for_topic = [&](const auto topic_name) + { + size_t wait_count = 0; + while (command_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(100)); + ++wait_count; + } + }; + + wait_for_topic(command_publisher_->get_topic_name()); + + ControllerReferenceMsg msg; + msg.twist.linear.x = linear; + msg.twist.angular.z = angular; + + command_publisher_->publish(msg); + } + +protected: + // Controller-related parameters + double reference_timeout_ = 2.0; + bool open_loop_ = false; + unsigned int velocity_rolling_window_size_ = 10; + bool position_feedback_ = false; + std::vector traction_joints_names_ = {"left_wheel_joint", "right_wheel_joint"}; + std::vector steering_joints_names_ = {"steering_joint"}; + std::vector joint_names_ = { + traction_joints_names_[0], traction_joints_names_[1], steering_joints_names_[0]}; + + std::vector traction_joints_preceding_names_ = { + "pid_controller/rear_right_wheel_joint", "pid_controller/rear_left_wheel_joint"}; + std::vector steering_joints_preceding_names_ = { + "pid_controller/front_right_steering_joint", "pid_controller/front_left_steering_joint"}; + + std::array joint_state_values_ = {{0.5, 0.5, 0.0}}; + std::array joint_command_values_ = {{1.1, 3.3, 2.2}}; + + std::array joint_reference_interfaces_ = {{"linear", "angular"}}; + std::string steering_interface_name_ = "position"; + // defined in setup + std::string traction_interface_name_ = ""; + std::string preceding_prefix_ = "pid_controller"; + + std::vector state_itfs_; + std::vector command_itfs_; + + // Test related parameters + std::unique_ptr controller_; + rclcpp::Node::SharedPtr command_publisher_node_; + rclcpp::Publisher::SharedPtr command_publisher_; +}; + +#endif // TEST_GENERIC_STEERING_CONTROLLER_HPP_