From bec826996e3fc7a0d01a6aa0611bf32a39a0d818 Mon Sep 17 00:00:00 2001 From: silanus23 Date: Sun, 20 Jul 2025 14:34:13 +0300 Subject: [PATCH 01/13] Add proposal for refactored steering controller library and six wheeler steering controller library --- doc/images/six_wheeler_steering.drawio | 164 ++++++++++++++++++ doc/images/six_wheeler_steering.svg | 4 + doc/six_wheeler_steering_controller.rst | 134 ++++++++++++++ .../doc/refactored_steering_library.rst | 95 ++++++++++ 4 files changed, 397 insertions(+) create mode 100644 doc/images/six_wheeler_steering.drawio create mode 100644 doc/images/six_wheeler_steering.svg create mode 100644 doc/six_wheeler_steering_controller.rst create mode 100644 steering_controllers_library/doc/refactored_steering_library.rst 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_wheeler_steering_controller.rst b/doc/six_wheeler_steering_controller.rst new file mode 100644 index 0000000000..a34774c2cd --- /dev/null +++ b/doc/six_wheeler_steering_controller.rst @@ -0,0 +1,134 @@ +.. _doc_six_wheel_steering_controller: + +six_wheel_steering_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. + * - ``steering_joints`` + - A list of the four corner steering joint names from the robot's URDF model. + * - ``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/steering_controllers_library/doc/refactored_steering_library.rst b/steering_controllers_library/doc/refactored_steering_library.rst new file mode 100644 index 0000000000..68a2032dbc --- /dev/null +++ b/steering_controllers_library/doc/refactored_steering_library.rst @@ -0,0 +1,95 @@ +.. _proposal_refactored_steering_library: + +Proposal: A Refactored, Plugin-Based Steering Controller Library +================================================================ + +Motivation +---------- + +The current ``steering_controllers_library`` has a monolithic design that tightly couples the core controller logic with specific kinematic implementations (Bicycle, Tricycle, Ackermann). This architecture presents several challenges: + +1. **Lack of Extensibility:** Adding support for new, more complex kinematic models, such as the six-wheel, four-wheel-steering rover discussed in the six_wheel_steering_controllers.srt, requires modifying the core library itself. This is not a scalable solution. +2. **Configuration Complexity:** The parameter interface is ambiguous and not easily adaptable to different vehicle types. +3. **High Maintenance Burden:** The central library becomes increasingly complex with each new model, making it difficult to maintain and debug. + +This document proposes a new architecture that addresses these issues by creating a truly **kinematics-agnostic** steering controller. + +Proposed Architecture: A New, Additive Implementation +------------------------------------------------------ + +The proposal is to introduce a new, refactored controller, ``generic_steering_controller``, to exist **alongside** the current library. The existing ``steering_controllers_library`` and its controllers will remain untouched, ensuring zero disruption for current users. + +The new controller will be built on ``pluginlib``, separating the generic control logic from the specific mathematical models. + +Core Components +--------------- + +**1. The GenericSteeringController** + +This will be the main controller class, inheriting from ``controller_interface::ChainableControllerInterface``. Its responsibilities will be limited to: + +* Interfacing with the ``ros2_control`` framework and hardware interfaces. +* Managing ROS 2 parameters, subscribers, and publishers. +* Loading the appropriate kinematic model plugin at runtime based on a YAML parameter. +* Calling the plugin's methods within the real-time update loop. + +The controller itself will have **zero knowledge** of any specific robot kinematics. + +**2. The KinematicModelBase Interface** + +A new abstract base class, ``KinematicModelBase``, will be created to define the "contract" that all kinematic plugins must adhere to. This ensures a standard interface between the generic controller and any kinematic model. +.. note:: + To enforce a clean public API and proper separation of concerns, this base class will be defined in its own header file (e.g., ``kinematic_model_base.hpp``). + + + +.. code-block:: cpp + + // A simplified view of the proposed interface + namespace generic_steering_controller + { + class KinematicModelBase + { + public: + virtual ~KinematicModelBase() = default; + + // Called once to allow the plugin to configure itself + virtual void configure( + const rclcpp::Node::SharedPtr & node, + const std::vector & traction_joints, + const std::vector & steering_joints) = 0; + + // Called in the real-time loop to calculate commands + virtual std::pair, std::vector> + get_commands(double linear_vel, double angular_vel) = 0; + + // Called in the real-time loop to update odometry + virtual void update_odometry(const rclcpp::Duration & period) = 0; + }; + } + +Configuration Example +--------------------- + +The configuration will be clean and two-tiered. The main controller reads its parameters, and the loaded plugin reads its own. + +.. code-block:: yaml + + generic_steering_controller: + ros__parameters: + # 1. Controller loads this plugin by name + kinematics_plugin_name: "six_wheel_kinematics/SixWheelKinematics" + + # Controller gets the joint lists + traction_joints: ["j1", "j2", "j3", "j4", "j5", "j6"] + steering_joints: ["s1", "s2", "s3", "s4"] + + # 2. The loaded plugin reads its own specific parameters + six_wheel_kinematics: + ros__parameters: + d1: 0.4 + d2: 0.5 + d3: 0.5 + d4: 0.45 + wheel_radius: 0.15 + From dcd90cf0eac1b8821d0262b897464adcb1d2f94f Mon Sep 17 00:00:00 2001 From: silanus Date: Mon, 21 Jul 2025 09:52:39 +0300 Subject: [PATCH 02/13] Update doc/six_wheeler_steering_controller.rst Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --- doc/six_wheeler_steering_controller.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/six_wheeler_steering_controller.rst b/doc/six_wheeler_steering_controller.rst index a34774c2cd..2188b45f3c 100644 --- a/doc/six_wheeler_steering_controller.rst +++ b/doc/six_wheeler_steering_controller.rst @@ -77,7 +77,7 @@ Controller Parameters * - Parameter - Description * - ``traction_joints`` - - A list of the six drive wheel joint names from the robot's URDF model. + - 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. * - ``d1`` From 00ad534ec51f649c30948220972f1087c35f4239 Mon Sep 17 00:00:00 2001 From: silanus Date: Mon, 21 Jul 2025 09:52:51 +0300 Subject: [PATCH 03/13] Update doc/six_wheeler_steering_controller.rst Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --- doc/six_wheeler_steering_controller.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/six_wheeler_steering_controller.rst b/doc/six_wheeler_steering_controller.rst index 2188b45f3c..3962d23b66 100644 --- a/doc/six_wheeler_steering_controller.rst +++ b/doc/six_wheeler_steering_controller.rst @@ -79,7 +79,7 @@ Controller Parameters * - ``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. + - 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`` From e9520ef7f0d75025674e823a46fd87248d75c1d0 Mon Sep 17 00:00:00 2001 From: silanus Date: Mon, 21 Jul 2025 09:52:57 +0300 Subject: [PATCH 04/13] Update steering_controllers_library/doc/refactored_steering_library.rst Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --- .../doc/refactored_steering_library.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/steering_controllers_library/doc/refactored_steering_library.rst b/steering_controllers_library/doc/refactored_steering_library.rst index 68a2032dbc..f08adca22a 100644 --- a/steering_controllers_library/doc/refactored_steering_library.rst +++ b/steering_controllers_library/doc/refactored_steering_library.rst @@ -8,7 +8,7 @@ Motivation The current ``steering_controllers_library`` has a monolithic design that tightly couples the core controller logic with specific kinematic implementations (Bicycle, Tricycle, Ackermann). This architecture presents several challenges: -1. **Lack of Extensibility:** Adding support for new, more complex kinematic models, such as the six-wheel, four-wheel-steering rover discussed in the six_wheel_steering_controllers.srt, requires modifying the core library itself. This is not a scalable solution. +1. **Lack of Extensibility:** Adding support for new, more complex kinematic models, such as the six-wheel, four-wheel-steering rover discussed in the six_wheel_steering_controllers.rst, requires modifying the core library itself. This is not a scalable solution. 2. **Configuration Complexity:** The parameter interface is ambiguous and not easily adaptable to different vehicle types. 3. **High Maintenance Burden:** The central library becomes increasingly complex with each new model, making it difficult to maintain and debug. From 2e6bfc113515e0c29bbde02de7d4221e3ab9dbdc Mon Sep 17 00:00:00 2001 From: silanus23 Date: Mon, 21 Jul 2025 09:54:01 +0300 Subject: [PATCH 05/13] deleted design proposal Signed-off-by: silanus23 --- .../doc/refactored_steering_library.rst | 95 ------------------- 1 file changed, 95 deletions(-) delete mode 100644 steering_controllers_library/doc/refactored_steering_library.rst diff --git a/steering_controllers_library/doc/refactored_steering_library.rst b/steering_controllers_library/doc/refactored_steering_library.rst deleted file mode 100644 index f08adca22a..0000000000 --- a/steering_controllers_library/doc/refactored_steering_library.rst +++ /dev/null @@ -1,95 +0,0 @@ -.. _proposal_refactored_steering_library: - -Proposal: A Refactored, Plugin-Based Steering Controller Library -================================================================ - -Motivation ----------- - -The current ``steering_controllers_library`` has a monolithic design that tightly couples the core controller logic with specific kinematic implementations (Bicycle, Tricycle, Ackermann). This architecture presents several challenges: - -1. **Lack of Extensibility:** Adding support for new, more complex kinematic models, such as the six-wheel, four-wheel-steering rover discussed in the six_wheel_steering_controllers.rst, requires modifying the core library itself. This is not a scalable solution. -2. **Configuration Complexity:** The parameter interface is ambiguous and not easily adaptable to different vehicle types. -3. **High Maintenance Burden:** The central library becomes increasingly complex with each new model, making it difficult to maintain and debug. - -This document proposes a new architecture that addresses these issues by creating a truly **kinematics-agnostic** steering controller. - -Proposed Architecture: A New, Additive Implementation ------------------------------------------------------- - -The proposal is to introduce a new, refactored controller, ``generic_steering_controller``, to exist **alongside** the current library. The existing ``steering_controllers_library`` and its controllers will remain untouched, ensuring zero disruption for current users. - -The new controller will be built on ``pluginlib``, separating the generic control logic from the specific mathematical models. - -Core Components ---------------- - -**1. The GenericSteeringController** - -This will be the main controller class, inheriting from ``controller_interface::ChainableControllerInterface``. Its responsibilities will be limited to: - -* Interfacing with the ``ros2_control`` framework and hardware interfaces. -* Managing ROS 2 parameters, subscribers, and publishers. -* Loading the appropriate kinematic model plugin at runtime based on a YAML parameter. -* Calling the plugin's methods within the real-time update loop. - -The controller itself will have **zero knowledge** of any specific robot kinematics. - -**2. The KinematicModelBase Interface** - -A new abstract base class, ``KinematicModelBase``, will be created to define the "contract" that all kinematic plugins must adhere to. This ensures a standard interface between the generic controller and any kinematic model. -.. note:: - To enforce a clean public API and proper separation of concerns, this base class will be defined in its own header file (e.g., ``kinematic_model_base.hpp``). - - - -.. code-block:: cpp - - // A simplified view of the proposed interface - namespace generic_steering_controller - { - class KinematicModelBase - { - public: - virtual ~KinematicModelBase() = default; - - // Called once to allow the plugin to configure itself - virtual void configure( - const rclcpp::Node::SharedPtr & node, - const std::vector & traction_joints, - const std::vector & steering_joints) = 0; - - // Called in the real-time loop to calculate commands - virtual std::pair, std::vector> - get_commands(double linear_vel, double angular_vel) = 0; - - // Called in the real-time loop to update odometry - virtual void update_odometry(const rclcpp::Duration & period) = 0; - }; - } - -Configuration Example ---------------------- - -The configuration will be clean and two-tiered. The main controller reads its parameters, and the loaded plugin reads its own. - -.. code-block:: yaml - - generic_steering_controller: - ros__parameters: - # 1. Controller loads this plugin by name - kinematics_plugin_name: "six_wheel_kinematics/SixWheelKinematics" - - # Controller gets the joint lists - traction_joints: ["j1", "j2", "j3", "j4", "j5", "j6"] - steering_joints: ["s1", "s2", "s3", "s4"] - - # 2. The loaded plugin reads its own specific parameters - six_wheel_kinematics: - ros__parameters: - d1: 0.4 - d2: 0.5 - d3: 0.5 - d4: 0.45 - wheel_radius: 0.15 - From 62405c61743d49504d1795b5d85960a81b146ef8 Mon Sep 17 00:00:00 2001 From: silanus23 Date: Mon, 21 Jul 2025 10:08:16 +0300 Subject: [PATCH 06/13] Name clarification Signed-off-by: silanus23 --- ...steering_controller.rst => six_wheel_rover_controller.rst} | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) rename doc/{six_wheeler_steering_controller.rst => six_wheel_rover_controller.rst} (98%) diff --git a/doc/six_wheeler_steering_controller.rst b/doc/six_wheel_rover_controller.rst similarity index 98% rename from doc/six_wheeler_steering_controller.rst rename to doc/six_wheel_rover_controller.rst index 3962d23b66..921b4333f5 100644 --- a/doc/six_wheeler_steering_controller.rst +++ b/doc/six_wheel_rover_controller.rst @@ -1,6 +1,6 @@ -.. _doc_six_wheel_steering_controller: +.. _doc_six_wheel_rover_controller: -six_wheel_steering_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. From ac39d6fc28363ff49a667732f90b459d4f0f0e20 Mon Sep 17 00:00:00 2001 From: silanus23 Date: Thu, 21 Aug 2025 18:01:31 +0300 Subject: [PATCH 07/13] Last versions main files --- .../generic_steering_controller.hpp | 153 ++++++ .../kinematic_model_base.hpp | 99 ++++ .../src/generic_steering_controller.cpp | 510 ++++++++++++++++++ 3 files changed, 762 insertions(+) create mode 100644 generic_steering_controller/include/generic_steering_controller/generic_steering_controller.hpp create mode 100644 generic_steering_controller/include/generic_steering_controller/kinematic_model_base.hpp create mode 100644 generic_steering_controller/src/generic_steering_controller.cpp 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..d1047b0532 --- /dev/null +++ b/generic_steering_controller/include/generic_steering_controller/generic_steering_controller.hpp @@ -0,0 +1,153 @@ +#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 "nav_msgs/msg/odometry.hpp" +#include "tf2_msgs/msg/tf_message.hpp" +#include "control_msgs/msg/steering_controller_status.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include +#include + + +// rclcpp and realtime_tools +#include "rclcpp/duration.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_publisher.hpp" +#include "realtime_tools/realtime_box.hpp" +#include "realtime_tools/realtime_thread_safe_box.hpp" +// Project-specific +#include "generic_steering_controller/kinematic_model_base.hpp" +#include "generic_steering_controller/generic_steering_controller_parameters.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..ed6b2f3067 --- /dev/null +++ b/generic_steering_controller/include/generic_steering_controller/kinematic_model_base.hpp @@ -0,0 +1,99 @@ +#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 generic_steering_controller + +#endif // GENERIC_STEERING_CONTROLLER__KINEMATIC_MODEL_BASE_HPP_ 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..b45a810025 --- /dev/null +++ b/generic_steering_controller/src/generic_steering_controller.cpp @@ -0,0 +1,510 @@ +#include +#include "generic_steering_controller/generic_steering_controller.hpp" + +#include +#include +#include +#include +#include + +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include + +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*/) +{ + // Get the latest reference message from the subscriber + 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_msg.value().header.stamp; + + // Check if the reference is fresh and valid + 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.angular.z) + /* && !std::isnan(current_ref_.twist.linear.y) */) // Uncomment if needed + { + reference_interfaces_[0] = current_ref_.twist.linear.x; + reference_interfaces_[1] = current_ref_.twist.angular.z; + + RCLCPP_DEBUG(get_node()->get_logger(), + "Accepted reference: linear.x=%.3f angular.z=%.3f age=%.3f", + current_ref_.twist.linear.x, current_ref_.twist.angular.z, age_of_last_command.seconds()); + + 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_); + RCLCPP_DEBUG(get_node()->get_logger(), "Cleared reference after use (timeout==0)"); + } + } + } 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_); + + RCLCPP_WARN(get_node()->get_logger(), + "Reference timed out (age=%.3f > timeout=%.3f), clearing reference.", + age_of_last_command.seconds(), ref_timeout_.seconds()); + } + } + 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"); +} + +} + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + generic_steering_controller::GenericSteeringController, + controller_interface::ChainableControllerInterface) From 24d2d04e00bbcc2c8cfbee6357ce5769916982bf Mon Sep 17 00:00:00 2001 From: silanus23 Date: Thu, 21 Aug 2025 18:02:27 +0300 Subject: [PATCH 08/13] Mock plugins to test --- .../mock_kinematic_plugin.cpp | 5 +++ .../mock_kinematic_plugin.hpp | 45 +++++++++++++++++++ .../mock_kinematic_plugin.xml | 10 +++++ 3 files changed, 60 insertions(+) create mode 100644 generic_steering_controller/test/mock_kinematic_plugin/mock_kinematic_plugin.cpp create mode 100644 generic_steering_controller/test/mock_kinematic_plugin/mock_kinematic_plugin.hpp create mode 100644 generic_steering_controller/test/mock_kinematic_plugin/mock_kinematic_plugin.xml 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..6f71c293a5 --- /dev/null +++ b/generic_steering_controller/test/mock_kinematic_plugin/mock_kinematic_plugin.cpp @@ -0,0 +1,5 @@ +#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..b77bd0e1f9 --- /dev/null +++ b/generic_steering_controller/test/mock_kinematic_plugin/mock_kinematic_plugin.hpp @@ -0,0 +1,45 @@ +#pragma once + +#include +#include +#include +#include +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "generic_steering_controller/kinematic_model_base.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_plugi +n 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..12a1f1a147 --- /dev/null +++ b/generic_steering_controller/test/mock_kinematic_plugin/mock_kinematic_plugin.xml @@ -0,0 +1,10 @@ + + + + Minimal mock kinematic model for testing. + + + \ No newline at end of file From 5644666483db45b4016b8a9d8c22aac3582c2398 Mon Sep 17 00:00:00 2001 From: silanus23 Date: Thu, 21 Aug 2025 18:02:49 +0300 Subject: [PATCH 09/13] Unit tests added --- .../test/test_generic_steering_controller.cpp | 94 ++++++ .../test/test_generic_steering_controller.hpp | 301 ++++++++++++++++++ 2 files changed, 395 insertions(+) create mode 100644 generic_steering_controller/test/test_generic_steering_controller.cpp create mode 100644 generic_steering_controller/test/test_generic_steering_controller.hpp 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..e98e48602e --- /dev/null +++ b/generic_steering_controller/test/test_generic_steering_controller.cpp @@ -0,0 +1,94 @@ +#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; + + controller_->reference_callback(msg); + controller_->update(now, rclcpp::Duration::from_seconds(0.01)); + + EXPECT_EQ(controller_->last_linear_velocity_, 1.0); + EXPECT_EQ(controller_->last_angular_velocity_, 0.5); + + 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); + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)); + + EXPECT_EQ(controller_->last_linear_velocity_, 1.0); + EXPECT_EQ(controller_->last_angular_velocity_, 0.5); +} + +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..060a8013de --- /dev/null +++ b/generic_steering_controller/test/test_generic_steering_controller.hpp @@ -0,0 +1,301 @@ +// 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 "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "generic_steering_controller/generic_steering_controller.hpp" +#include "pluginlib/class_list_macros.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_ From 94f257e129f7acb52267518c860b21367a1a210a Mon Sep 17 00:00:00 2001 From: silanus23 Date: Thu, 21 Aug 2025 18:03:12 +0300 Subject: [PATCH 10/13] Package defining --- generic_steering_controller/CMakeLists.txt | 139 ++++++++++++++++++ ...eneric_steering_controller_parameters.yaml | 61 ++++++++ .../generic_steering_controller.xml | 10 ++ generic_steering_controller/package.xml | 35 +++++ 4 files changed, 245 insertions(+) create mode 100644 generic_steering_controller/CMakeLists.txt create mode 100644 generic_steering_controller/config/generic_steering_controller_parameters.yaml create mode 100644 generic_steering_controller/generic_steering_controller.xml create mode 100644 generic_steering_controller/package.xml 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..59c6bdd7e3 --- /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, + } \ No newline at end of file diff --git a/generic_steering_controller/generic_steering_controller.xml b/generic_steering_controller/generic_steering_controller.xml new file mode 100644 index 0000000000..ab56faf4b8 --- /dev/null +++ b/generic_steering_controller/generic_steering_controller.xml @@ -0,0 +1,10 @@ + + + + Generic, plugin-based steering controller for arbitrary kinematics. + + + \ No newline at end of file diff --git a/generic_steering_controller/package.xml b/generic_steering_controller/package.xml new file mode 100644 index 0000000000..61dac21fb2 --- /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 + + + \ No newline at end of file From d517688d8ab5e2cfc410a85d1f6c393edda312f5 Mon Sep 17 00:00:00 2001 From: silanus23 Date: Thu, 21 Aug 2025 18:04:40 +0300 Subject: [PATCH 11/13] Temporary signings --- .../generic_steering_controller.hpp | 14 ++++++++++++++ .../kinematic_model_base.hpp | 14 ++++++++++++++ .../src/generic_steering_controller.cpp | 14 ++++++++++++++ .../mock_kinematic_plugin.cpp | 14 ++++++++++++++ .../mock_kinematic_plugin.hpp | 14 ++++++++++++++ .../test/test_generic_steering_controller.cpp | 14 ++++++++++++++ 6 files changed, 84 insertions(+) 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 index d1047b0532..fc7c3aef48 100644 --- a/generic_steering_controller/include/generic_steering_controller/generic_steering_controller.hpp +++ b/generic_steering_controller/include/generic_steering_controller/generic_steering_controller.hpp @@ -1,3 +1,17 @@ +// 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_ 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 index ed6b2f3067..4353160a5b 100644 --- a/generic_steering_controller/include/generic_steering_controller/kinematic_model_base.hpp +++ b/generic_steering_controller/include/generic_steering_controller/kinematic_model_base.hpp @@ -1,3 +1,17 @@ +// 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_ diff --git a/generic_steering_controller/src/generic_steering_controller.cpp b/generic_steering_controller/src/generic_steering_controller.cpp index b45a810025..d2cf83120f 100644 --- a/generic_steering_controller/src/generic_steering_controller.cpp +++ b/generic_steering_controller/src/generic_steering_controller.cpp @@ -1,3 +1,17 @@ +// 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 #include "generic_steering_controller/generic_steering_controller.hpp" 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 index 6f71c293a5..140e49475b 100644 --- a/generic_steering_controller/test/mock_kinematic_plugin/mock_kinematic_plugin.cpp +++ b/generic_steering_controller/test/mock_kinematic_plugin/mock_kinematic_plugin.cpp @@ -1,3 +1,17 @@ +// 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" 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 index b77bd0e1f9..64303654b0 100644 --- a/generic_steering_controller/test/mock_kinematic_plugin/mock_kinematic_plugin.hpp +++ b/generic_steering_controller/test/mock_kinematic_plugin/mock_kinematic_plugin.hpp @@ -1,3 +1,17 @@ +// 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 diff --git a/generic_steering_controller/test/test_generic_steering_controller.cpp b/generic_steering_controller/test/test_generic_steering_controller.cpp index e98e48602e..1f67355cb8 100644 --- a/generic_steering_controller/test/test_generic_steering_controller.cpp +++ b/generic_steering_controller/test/test_generic_steering_controller.cpp @@ -1,3 +1,17 @@ +// 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" From 216c7b80db8d5baf50200c1338f9874d1817d944 Mon Sep 17 00:00:00 2001 From: silanus23 Date: Thu, 21 Aug 2025 20:38:09 +0300 Subject: [PATCH 12/13] Fixes before publish --- .../src/generic_steering_controller.cpp | 27 ++++++------------- .../mock_kinematic_plugin.hpp | 3 +-- .../test/test_generic_steering_controller.cpp | 19 +++++++------ 3 files changed, 20 insertions(+), 29 deletions(-) diff --git a/generic_steering_controller/src/generic_steering_controller.cpp b/generic_steering_controller/src/generic_steering_controller.cpp index d2cf83120f..859e785941 100644 --- a/generic_steering_controller/src/generic_steering_controller.cpp +++ b/generic_steering_controller/src/generic_steering_controller.cpp @@ -329,31 +329,21 @@ controller_interface::CallbackReturn GenericSteeringController::on_deactivate( controller_interface::return_type GenericSteeringController::update_reference_from_subscribers( const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { - // Get the latest reference message from the subscriber 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_msg.value().header.stamp; + const auto age_of_last_command = time - current_ref_.header.stamp; - // Check if the reference is fresh and valid 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.angular.z) - /* && !std::isnan(current_ref_.twist.linear.y) */) // Uncomment if needed - { + 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; - RCLCPP_DEBUG(get_node()->get_logger(), - "Accepted reference: linear.x=%.3f angular.z=%.3f age=%.3f", - current_ref_.twist.linear.x, current_ref_.twist.angular.z, age_of_last_command.seconds()); - 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_); - RCLCPP_DEBUG(get_node()->get_logger(), "Cleared reference after use (timeout==0)"); } } } else { @@ -364,10 +354,6 @@ controller_interface::return_type GenericSteeringController::update_reference_fr 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_); - - RCLCPP_WARN(get_node()->get_logger(), - "Reference timed out (age=%.3f > timeout=%.3f), clearing reference.", - age_of_last_command.seconds(), ref_timeout_.seconds()); } } return controller_interface::return_type::OK; @@ -444,9 +430,11 @@ controller_interface::return_type GenericSteeringController::update_and_write_co 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)); + 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)); + 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(), @@ -465,7 +453,8 @@ controller_interface::return_type GenericSteeringController::update_and_write_co } 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)); + 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); 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 index 64303654b0..8f85a9f19a 100644 --- a/generic_steering_controller/test/mock_kinematic_plugin/mock_kinematic_plugin.hpp +++ b/generic_steering_controller/test/mock_kinematic_plugin/mock_kinematic_plugin.hpp @@ -55,5 +55,4 @@ class MockKinematicModel : public kinematic_model::KinematicModelBase }; -} // namespace mock_kinematic_plugi -n +} // namespace mock_kinematic_plugin diff --git a/generic_steering_controller/test/test_generic_steering_controller.cpp b/generic_steering_controller/test/test_generic_steering_controller.cpp index 1f67355cb8..4d158590c4 100644 --- a/generic_steering_controller/test/test_generic_steering_controller.cpp +++ b/generic_steering_controller/test/test_generic_steering_controller.cpp @@ -81,21 +81,24 @@ TEST_F(GenericSteeringControllerTest, CommandTimeoutSetsZeroVelocity) msg->twist.linear.x = 1.0; msg->twist.angular.z = 0.5; + // Fresh reference (within timeout) controller_->reference_callback(msg); - controller_->update(now, rclcpp::Duration::from_seconds(0.01)); - - EXPECT_EQ(controller_->last_linear_velocity_, 1.0); - EXPECT_EQ(controller_->last_angular_velocity_, 0.5); + 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_EQ(controller_->last_linear_velocity_, 1.0); - EXPECT_EQ(controller_->last_angular_velocity_, 0.5); + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[1])); } int main(int argc, char ** argv) From 3e8df3734feae5ea0d25eabad874cb8b62880e15 Mon Sep 17 00:00:00 2001 From: silanus23 Date: Thu, 21 Aug 2025 21:52:10 +0300 Subject: [PATCH 13/13] Lint problem --- doc/six_wheel_rover_controller.rst | 1 - ...eneric_steering_controller_parameters.yaml | 2 +- .../generic_steering_controller.xml | 2 +- .../generic_steering_controller.hpp | 27 +- .../kinematic_model_base.hpp | 4 +- generic_steering_controller/package.xml | 2 +- .../src/generic_steering_controller.cpp | 235 +++++++++++------- .../mock_kinematic_plugin.cpp | 4 +- .../mock_kinematic_plugin.hpp | 21 +- .../mock_kinematic_plugin.xml | 2 +- .../test/test_generic_steering_controller.cpp | 19 +- .../test/test_generic_steering_controller.hpp | 116 +++++---- 12 files changed, 249 insertions(+), 186 deletions(-) diff --git a/doc/six_wheel_rover_controller.rst b/doc/six_wheel_rover_controller.rst index 921b4333f5..074c41db08 100644 --- a/doc/six_wheel_rover_controller.rst +++ b/doc/six_wheel_rover_controller.rst @@ -131,4 +131,3 @@ Example Configuration # ---- ODOMETRY CONFIGURATION ---- odom_frame_id: "odom" base_frame_id: "base_link" - diff --git a/generic_steering_controller/config/generic_steering_controller_parameters.yaml b/generic_steering_controller/config/generic_steering_controller_parameters.yaml index 59c6bdd7e3..3887b0d13a 100644 --- a/generic_steering_controller/config/generic_steering_controller_parameters.yaml +++ b/generic_steering_controller/config/generic_steering_controller_parameters.yaml @@ -58,4 +58,4 @@ generic_steering_controller_parameters: default_value: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0], description: "diagonal values of twist covariance matrix.", read_only: false, - } \ No newline at end of file + } diff --git a/generic_steering_controller/generic_steering_controller.xml b/generic_steering_controller/generic_steering_controller.xml index ab56faf4b8..8f110a1fcc 100644 --- a/generic_steering_controller/generic_steering_controller.xml +++ b/generic_steering_controller/generic_steering_controller.xml @@ -7,4 +7,4 @@ Generic, plugin-based steering controller for arbitrary kinematics. - \ No newline at end of file + 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 index fc7c3aef48..05f624d358 100644 --- a/generic_steering_controller/include/generic_steering_controller/generic_steering_controller.hpp +++ b/generic_steering_controller/include/generic_steering_controller/generic_steering_controller.hpp @@ -16,10 +16,10 @@ #define GENERIC_STEERING_CONTROLLER__GENERIC_STEERING_CONTROLLER_HPP_ // C++ Standard Library +#include #include #include #include -#include // ros2_control and controller_interface #include "controller_interface/chainable_controller_interface.hpp" @@ -28,25 +28,23 @@ // pluginlib #include "pluginlib/class_loader.hpp" - -#include "nav_msgs/msg/odometry.hpp" -#include "tf2_msgs/msg/tf_message.hpp" +#include +#include #include "control_msgs/msg/steering_controller_status.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" -#include -#include - +#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_publisher.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/kinematic_model_base.hpp" #include "generic_steering_controller/generic_steering_controller_parameters.hpp" +#include "generic_steering_controller/kinematic_model_base.hpp" namespace generic_steering_controller { @@ -94,7 +92,8 @@ class GenericSteeringController : public controller_interface::ChainableControll inline std::unordered_map get_state_interface_map() const { std::unordered_map current_map; - for (const auto & iface : state_interfaces_) { + for (const auto & iface : state_interfaces_) + { current_map[iface.get_name()] = iface.get_value(); } return current_map; @@ -104,7 +103,6 @@ class GenericSteeringController : public controller_interface::ChainableControll 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_; @@ -132,7 +130,8 @@ class GenericSteeringController : public controller_interface::ChainableControll GenericSteeringControllerStateMsg published_state_; - using ControllerStatePublisher = realtime_tools::RealtimePublisher; + using ControllerStatePublisher = + realtime_tools::RealtimePublisher; rclcpp::Publisher::SharedPtr controller_s_publisher_; std::unique_ptr controller_state_publisher_; @@ -154,7 +153,7 @@ class GenericSteeringController : public controller_interface::ChainableControll void reference_callback(const std::shared_ptr msg); private: -// callback for topic interface + // callback for topic interface pluginlib::ClassLoader kinematic_loader; std::shared_ptr kinematic_model_; @@ -164,4 +163,4 @@ class GenericSteeringController : public controller_interface::ChainableControll } // namespace generic_steering_controller -#endif //GENERIC_STEERING_CONTROLLER__GENERIC_STEERING_CONTROLLER_HPP_ +#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 index 4353160a5b..718a9461aa 100644 --- a/generic_steering_controller/include/generic_steering_controller/kinematic_model_base.hpp +++ b/generic_steering_controller/include/generic_steering_controller/kinematic_model_base.hpp @@ -84,7 +84,7 @@ class KinematicModelBase 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). */ @@ -108,6 +108,6 @@ class KinematicModelBase std::unordered_map states; }; -} // namespace generic_steering_controller +} // 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 index 61dac21fb2..9df95c36d5 100644 --- a/generic_steering_controller/package.xml +++ b/generic_steering_controller/package.xml @@ -32,4 +32,4 @@ ament_cmake - \ No newline at end of file + diff --git a/generic_steering_controller/src/generic_steering_controller.cpp b/generic_steering_controller/src/generic_steering_controller.cpp index 859e785941..a651522e7d 100644 --- a/generic_steering_controller/src/generic_steering_controller.cpp +++ b/generic_steering_controller/src/generic_steering_controller.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include "generic_steering_controller/generic_steering_controller.hpp" +#include #include #include @@ -21,9 +21,9 @@ #include #include +#include #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include namespace { // utility @@ -55,11 +55,14 @@ GenericSteeringController::GenericSteeringController() controller_interface::CallbackReturn GenericSteeringController::on_init() { - try { + try + { param_listener_ = std::make_shared(get_node()); initialize_implementation_parameter_listener(); - } catch (const std::exception & e) { + } + catch (const std::exception & e) + { fprintf(stderr, "Exception thrown during controller's init with message: %s \n", e.what()); return controller_interface::CallbackReturn::ERROR; } @@ -81,11 +84,13 @@ controller_interface::CallbackReturn GenericSteeringController::on_configure( { params_ = param_listener_->get_params(); - if (params_.traction_joints_names.empty()) { + 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()) { + if (params_.steering_joints_names.empty()) + { RCLCPP_ERROR(get_node()->get_logger(), "No steering joints specified!"); return controller_interface::CallbackReturn::ERROR; } @@ -106,11 +111,14 @@ controller_interface::CallbackReturn GenericSteeringController::on_configure( reset_controller_reference_msg(current_ref_, get_node()); input_ref_.set(current_ref_); - try { + try + { odom_s_publisher_ = get_node()->create_publisher( - "~/odometry", rclcpp::SystemDefaultsQoS()); + "~/odometry", rclcpp::SystemDefaultsQoS()); rt_odom_state_publisher_ = std::make_unique(odom_s_publisher_); - } catch (const std::exception & e) { + } + catch (const std::exception & e) + { fprintf( stderr, "Exception thrown during publisher creation at configure stage with message : %s \n", e.what()); @@ -126,21 +134,24 @@ controller_interface::CallbackReturn GenericSteeringController::on_configure( auto & covariance = rt_odom_state_publisher_->msg_.twist.covariance; constexpr size_t NUM_DIMENSIONS = 6; - for (size_t index = 0; index < 6; ++index) { + 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 { + 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) { + } + catch (const std::exception & e) + { fprintf( stderr, "Exception thrown during publisher creation at configure stage with message : %s \n", e.what()); @@ -155,13 +166,16 @@ controller_interface::CallbackReturn GenericSteeringController::on_configure( rt_tf_odom_state_publisher_->msg_.transforms[0].transform.translation.z = 0.0; rt_tf_odom_state_publisher_->unlock(); - try { + 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) { + } + catch (const std::exception & e) + { fprintf( stderr, "Exception thrown during publisher creation at configure stage with message : %s \n", e.what()); @@ -173,33 +187,34 @@ controller_interface::CallbackReturn GenericSteeringController::on_configure( 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(); + 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 { + 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) { + 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) { + 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."); @@ -207,9 +222,12 @@ void GenericSteeringController::reference_callback( } 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_) { + if (ref_timeout_ == rclcpp::Duration::from_seconds(0) || age_of_last_command <= ref_timeout_) + { input_ref_.set(*msg); - } else { + } + else + { RCLCPP_ERROR( get_node()->get_logger(), "Received message has timestamp %.10f older for %.10f which is more then allowed timeout " @@ -225,17 +243,18 @@ 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++) { + 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++) { + 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 @@ -246,17 +265,19 @@ GenericSteeringController::state_interface_configuration() const 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; + 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++) { + 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) { + 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); } @@ -288,7 +309,7 @@ GenericSteeringController::on_export_reference_interfaces() return reference_interfaces; } -bool GenericSteeringController::on_set_chained_mode(bool /*chained_mode*/) {return true;} +bool GenericSteeringController::on_set_chained_mode(bool /*chained_mode*/) { return true; } controller_interface::CallbackReturn GenericSteeringController::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) @@ -298,9 +319,12 @@ controller_interface::CallbackReturn GenericSteeringController::on_activate( 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(), + 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()); } @@ -314,9 +338,12 @@ 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(), + 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()); } @@ -330,24 +357,31 @@ controller_interface::return_type GenericSteeringController::update_reference_fr const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { auto current_ref_msg = input_ref_.try_get(); - if (current_ref_msg.has_value()) { + 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)) { + 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)) { + 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)) { + } + 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(); @@ -364,44 +398,57 @@ controller_interface::return_type GenericSteeringController::update_and_write_co const rclcpp::Time & time, const rclcpp::Duration & period) { // Check kinematic plugin - if (!kinematic_model_) { + 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_) { + 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])) { + 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]); + 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++) { + 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]); + 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 { + } + 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); + 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) @@ -415,7 +462,8 @@ controller_interface::return_type GenericSteeringController::update_and_write_co } // Publish controller state message - if (controller_state_publisher_->trylock()) { + 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(); @@ -427,36 +475,46 @@ controller_interface::return_type GenericSteeringController::update_and_write_co 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) { + 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 (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", + 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) { + 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(), + 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(), + 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); } } @@ -474,13 +532,15 @@ controller_interface::return_type GenericSteeringController::update_and_write_co tf2::Quaternion orientation; orientation.setRPY(0.0, 0.0, yaw); - if (rt_odom_state_publisher_->trylock()) { + 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()) { + 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; @@ -500,11 +560,10 @@ controller_interface::return_type GenericSteeringController::update_and_write_co 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" 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 index 140e49475b..961f725644 100644 --- a/generic_steering_controller/test/mock_kinematic_plugin/mock_kinematic_plugin.cpp +++ b/generic_steering_controller/test/mock_kinematic_plugin/mock_kinematic_plugin.cpp @@ -15,5 +15,5 @@ #include "mock_kinematic_plugin.hpp" #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(mock_kinematic_plugin::MockKinematicModel, - kinematic_model::KinematicModelBase) +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 index 8f85a9f19a..8064614cae 100644 --- a/generic_steering_controller/test/mock_kinematic_plugin/mock_kinematic_plugin.hpp +++ b/generic_steering_controller/test/mock_kinematic_plugin/mock_kinematic_plugin.hpp @@ -14,13 +14,13 @@ #pragma once -#include -#include #include +#include #include -#include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "nav_msgs/msg/odometry.hpp" +#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 { @@ -34,25 +34,24 @@ class MockKinematicModel : public kinematic_model::KinematicModelBase 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 {} + 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}); + 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 index 12a1f1a147..b1c3bee1f2 100644 --- a/generic_steering_controller/test/mock_kinematic_plugin/mock_kinematic_plugin.xml +++ b/generic_steering_controller/test/mock_kinematic_plugin/mock_kinematic_plugin.xml @@ -7,4 +7,4 @@ Minimal mock kinematic model for testing. - \ No newline at end of file + diff --git a/generic_steering_controller/test/test_generic_steering_controller.cpp b/generic_steering_controller/test/test_generic_steering_controller.cpp index 4d158590c4..1016c0fb9f 100644 --- a/generic_steering_controller/test/test_generic_steering_controller.cpp +++ b/generic_steering_controller/test/test_generic_steering_controller.cpp @@ -16,7 +16,7 @@ #include "gtest/gtest.h" class GenericSteeringControllerTest - : public GenericSteeringControllerFixture +: public GenericSteeringControllerFixture { }; @@ -35,9 +35,7 @@ TEST_F(GenericSteeringControllerTest, check_exported_interfaces) 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_); - + 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()); @@ -51,9 +49,10 @@ TEST_F(GenericSteeringControllerTest, check_exported_interfaces) state_if_conf.names[STATE_STEER_WHEEL], controller_->steering_joints_state_names_[0] + "/" + steering_interface_name_); - auto reference_interfaces = controller_->export_reference_interfaces(); + 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) { + 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); @@ -69,15 +68,17 @@ TEST_F(GenericSteeringControllerTest, CommandTimeoutSetsZeroVelocity) SetUpController(); controller_->get_node()->set_parameter(rclcpp::Parameter("ref_timeout", 0.5)); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), + 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->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; diff --git a/generic_steering_controller/test/test_generic_steering_controller.hpp b/generic_steering_controller/test/test_generic_steering_controller.hpp index 060a8013de..a62a8c80fd 100644 --- a/generic_steering_controller/test/test_generic_steering_controller.hpp +++ b/generic_steering_controller/test/test_generic_steering_controller.hpp @@ -23,15 +23,15 @@ #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/state.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" -#include "generic_steering_controller/generic_steering_controller.hpp" -#include "pluginlib/class_list_macros.hpp" +#include "rclcpp_lifecycle/state.hpp" using ControllerStateMsg = generic_steering_controller::GenericSteeringController::GenericSteeringControllerStateMsg; @@ -65,12 +65,12 @@ constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; // subclassing and friending so we can access member variables class TestableGenericSteeringController - : public generic_steering_controller::GenericSteeringController +: 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); + // FRIEND_TEST(SteeringControllersLibraryTest, test_position_feedback_ref_timeout); + // FRIEND_TEST(SteeringControllersLibraryTest, test_velocity_feedback_ref_timeout); public: controller_interface::CallbackReturn on_configure( @@ -96,7 +96,8 @@ class TestableGenericSteeringController const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) { auto until = get_node()->get_clock()->now() + timeout; - while (get_node()->get_clock()->now() < until) { + while (get_node()->get_clock()->now() < until) + { executor.spin_some(); std::this_thread::sleep_for(std::chrono::microseconds(10)); } @@ -118,7 +119,7 @@ class TestableGenericSteeringController }; // We are using template class here for easier reuse of Fixture in specializations of controllers -template +template class GenericSteeringControllerFixture : public ::testing::Test { public: @@ -126,51 +127,54 @@ class GenericSteeringControllerFixture : public ::testing::Test void SetUp() { - // initialize controller + // initialize controller controller_ = std::make_unique(); - // Create the node for parameters (this is just for publishing commands) + // 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()); + "/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); + controller_->init(controller_name, "", 0, "", controller_->define_custom_node_options()), + controller_interface::return_type::OK); - // No parameter setting! Use YAML defaults. + // No parameter setting! Use YAML defaults. - if (position_feedback_ == true) { + if (position_feedback_ == true) + { traction_interface_name_ = "position"; - } else { + } + else + { traction_interface_name_ = "velocity"; } - // Manually create interfaces using test fixture values + // 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])); + 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])); + 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])); + 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; @@ -178,24 +182,23 @@ class GenericSteeringControllerFixture : public ::testing::Test 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])); + 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])); + 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])); + 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) @@ -203,7 +206,7 @@ class GenericSteeringControllerFixture : public ::testing::Test // 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 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; @@ -216,21 +219,24 @@ class GenericSteeringControllerFixture : public ::testing::Test // 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--) { + 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) { + 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()) { + 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"; + "controller/broadcaster update loop"; ASSERT_TRUE(received_msg); // take message from subscription @@ -240,18 +246,20 @@ class GenericSteeringControllerFixture : public ::testing::Test 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) { - 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; + 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()); @@ -268,10 +276,8 @@ class GenericSteeringControllerFixture : public ::testing::Test 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 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]};