Program Listing for File swerve_drive_controller.hpp

↰ Return to documentation for file (include/ffw_swerve_drive_controller/swerve_drive_controller.hpp)

// Copyright 2025 ROBOTIS .co., Ltd.
//
// 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.

/*
 * Author: Geonhee Lee
 */

#ifndef FFW_SWERVE_DRIVE_CONTROLLER__SWERVE_DRIVE_CONTROLLER_HPP_
#define FFW_SWERVE_DRIVE_CONTROLLER__SWERVE_DRIVE_CONTROLLER_HPP_

#include <memory>
#include <string>
#include <vector>
#include <queue>
#include <cmath>
#include <functional>
#include <stdexcept>
#include <algorithm>

#include "controller_interface/controller_interface.hpp"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
#include "rclcpp_lifecycle/state.hpp"
#include "rclcpp/subscription.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "realtime_tools/realtime_buffer.hpp"
#include "realtime_tools/realtime_publisher.hpp"
#include "hardware_interface/loaned_command_interface.hpp"
#include "hardware_interface/loaned_state_interface.hpp"
#include "rcl_interfaces/msg/parameter_descriptor.hpp"
#include "rclcpp/parameter.hpp"

#include "ffw_swerve_drive_controller/odometry.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2_msgs/msg/tf_message.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "sensor_msgs/msg/joint_state.hpp"

#include "ffw_swerve_drive_controller/marker_visualize.hpp"
#include "ffw_swerve_drive_controller/speed_limiter.hpp"

// auto-generated by generate_parameter_library
#include "ffw_swerve_drive_controller/swerve_drive_controller_parameter.hpp"


namespace ffw_swerve_drive_controller
{

// Define aliases for convenience
using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
// Use Twist directly as the command message type
using CmdVelMsg = geometry_msgs::msg::Twist;
using Twist = geometry_msgs::msg::Twist;
using OdomStateMsg = nav_msgs::msg::Odometry;
using TfStateMsg = tf2_msgs::msg::TFMessage;
using OdomStatePublisher = realtime_tools::RealtimePublisher<OdomStateMsg>;
using TfStatePublisher = realtime_tools::RealtimePublisher<TfStateMsg>;

// Enum definitions
enum Rotation { CCW, CW, STOP };

// Structure to hold module information for easier access
struct ModuleHandles
{
  // Use reference_wrapper for safe access in RT loop
  std::reference_wrapper<const hardware_interface::LoanedStateInterface> steering_state_pos;
  std::reference_wrapper<hardware_interface::LoanedCommandInterface> steering_cmd_pos;
  // Wheel state might be needed for odometry (not implemented here)
  std::reference_wrapper<const hardware_interface::LoanedStateInterface> wheel_state_vel;
  std::reference_wrapper<hardware_interface::LoanedCommandInterface> wheel_cmd_vel;
  // Store parameters associated with this module
  double x_offset;
  double y_offset;
  double angle_offset;
  double steering_limit_lower;
  double steering_limit_upper;
};

class SwerveDriveController : public controller_interface::ControllerInterface
{
public:
  SwerveDriveController();

  CallbackReturn on_init() override;

  CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override;

  CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;

  CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;

  controller_interface::InterfaceConfiguration command_interface_configuration() const override;

  controller_interface::InterfaceConfiguration state_interface_configuration() const override;

  controller_interface::return_type update(
    const rclcpp::Time & time, const rclcpp::Duration & period) override;

  void reference_callback(const std::shared_ptr<CmdVelMsg> msg);

  // Parameters from ROS for swerve drive controller
  std::shared_ptr<ParamListener> param_listener_;
  Params params_;

protected:
  // Parameters
  std::vector<std::string> steering_joint_names_;
  std::vector<std::string> wheel_joint_names_;
  double wheel_radius_;
  std::vector<double> module_x_offsets_;
  std::vector<double> module_y_offsets_;
  std::vector<double> module_angle_offsets_;
  std::vector<double> module_steering_limit_lower_;
  std::vector<double> module_steering_limit_upper_;
  std::vector<double> module_wheel_speed_limit_lower_;
  std::vector<double> module_wheel_speed_limit_upper_;
  bool enabled_steering_flip_;
  bool enabled_steering_angular_velocity_limit_;
  bool enabled_steering_angular_limit_;
  bool enabled_open_loop_;
  uint is_rotation_direction_;
  std::vector<double> previoud_steering_commands_;
  double steering_angular_velocity_limit_;
  double steering_alignment_angle_error_threshold_;
  double steering_alignment_start_angle_error_threshold_;
  double steering_alignment_start_speed_error_threshold_;
  std::string odom_solver_method_str_;

  std::string cmd_vel_topic_;
  bool use_stamped_cmd_vel_;
  double cmd_vel_timeout_;
  rclcpp::Duration ref_timeout_;

  // Store number of modules
  size_t num_modules_ = 0;

  // Realtime buffer for incoming Twist commands
  realtime_tools::RealtimeBuffer<std::shared_ptr<CmdVelMsg>> cmd_vel_buffer_;
  // Subscriber
  rclcpp::Subscription<CmdVelMsg>::SharedPtr cmd_vel_subscriber_ = nullptr;

  // Interface handles organized by module
  std::vector<ModuleHandles> module_handles_;

  // Internal state or variables for control logic
  double target_vx_ = 0.0;
  double target_vy_ = 0.0;
  double target_wz_ = 0.0;
  rclcpp::Time last_cmd_vel_time_;

  // Odometry
  Odometry odometry_;
  rclcpp::Publisher<OdomStateMsg>::SharedPtr odom_s_publisher_ = nullptr;
  std::unique_ptr<OdomStatePublisher> rt_odom_state_publisher_ = nullptr;
  OdomStateMsg odom_msg_;

  // joint commander publisher
  using CommandedJointStatePublisher =
    realtime_tools::RealtimePublisher<sensor_msgs::msg::JointState>;
  rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr commanded_joint_state_publisher_ =
    nullptr;
  std::unique_ptr<CommandedJointStatePublisher> rt_commanded_joint_state_publisher_ = nullptr;
  sensor_msgs::msg::JointState joint_state_msg_;

  // Odometry Parameters
  std::string odom_frame_id_;
  std::string base_frame_id_;
  bool enable_odom_tf_;
  std::vector<double> pose_covariance_diagonal_;
  std::vector<double> twist_covariance_diagonal_;
  int velocity_rolling_window_size_;
  std::string odom_source_;

  // TF Broadcaster
  rclcpp::Publisher<TfStateMsg>::SharedPtr tf_odom_s_publisher_;
  std::unique_ptr<TfStatePublisher> rt_tf_odom_state_publisher_;
  TfStateMsg tf_msg_;

  // Visualization
  bool enable_visualization_;
  std::string visualization_marker_topic_;
  std::unique_ptr<MarkerVisualize> visualizer_ = nullptr;
  double visualization_update_time_;
  rclcpp::Time last_visualization_publish_time_;

  // speed limiters
  bool enabled_speed_limits_;
  bool publish_limited_velocity_;
  std::shared_ptr<rclcpp::Publisher<geometry_msgs::msg::Twist>> limited_velocity_publisher_ =
    nullptr;
  std::shared_ptr<realtime_tools::RealtimePublisher<geometry_msgs::msg::Twist>>
  realtime_limited_velocity_publisher_ =
    nullptr;
  geometry_msgs::msg::Twist limited_velocity_msg_;
  std::queue<Twist> previous_commands_;

  SpeedLimiter limiter_linear_x_;
  SpeedLimiter limiter_linear_y_;
  SpeedLimiter limiter_angular_z_;

  // Flag to check if stopping due to timeout
  bool enable_direct_joint_commands_ = false;
  double wheel_saturation_scale_factor_ = 1.0;
  bool enabled_wheel_saturation_scaling_ = false;

  // Open Loop ctrl
  std::vector<double> previous_wheel_directions_;

  static double normalize_angle(double angle_rad);

  double shortest_angular_distance(double from, double to);

  double normalize_angle_positive(double angle);

  // Define ControllerReferenceMsg alias globally (if needed outside class)
  // using ControllerReferenceMsg = geometry_msgs::msg::Twist;
  // Not strictly needed if CmdVelMsg is used
  // Utility function prototype (global scope)
  void reset_controller_reference_msg(
    const std::shared_ptr<geometry_msgs::msg::Twist> & msg);
};
}  // namespace ffw_swerve_drive_controller

#endif  // FFW_SWERVE_DRIVE_CONTROLLER__SWERVE_DRIVE_CONTROLLER_HPP_