Bug Fix

Signed-off-by: nitin <[email protected]>

Author: nitin2606

swerve_drive_controller/include/swerve_drive_controller/swerve_drive_controller.hpp

@@ -35,72 +35,78 @@
 #include "swerve_drive_controller/swerve_drive_kinematics.hpp"
 #include "tf2_msgs/msg/tf_message.hpp"
 
-namespace swerve_drive_controller {
+namespace swerve_drive_controller
+{
 
 using CallbackReturn = controller_interface::CallbackReturn;
 // using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
 
-class Wheel {
- public:
-  Wheel(std::reference_wrapper<hardware_interface::LoanedCommandInterface> velocity,
-        std::reference_wrapper<const hardware_interface::LoanedStateInterface> feedback,
-        std::string name);
+class Wheel
+{
+public:
+  Wheel(
+    std::reference_wrapper<hardware_interface::LoanedCommandInterface> velocity,
+    std::reference_wrapper<const hardware_interface::LoanedStateInterface> feedback,
+    std::string name);
 
   void set_velocity(double velocity);
   double get_feedback();
 
- private:
+private:
   std::reference_wrapper<hardware_interface::LoanedCommandInterface> velocity_;
   std::reference_wrapper<const hardware_interface::LoanedStateInterface> feedback_;
   std::string name;
 };
 
-class Axle {
- public:
-  Axle(std::reference_wrapper<hardware_interface::LoanedCommandInterface> position,
-       std::reference_wrapper<const hardware_interface::LoanedStateInterface> feedback,
-       std::string name);
+class Axle
+{
+public:
+  Axle(
+    std::reference_wrapper<hardware_interface::LoanedCommandInterface> position,
+    std::reference_wrapper<const hardware_interface::LoanedStateInterface> feedback,
+    std::string name);
 
   void set_position(double position);
   double get_feedback();
 
- private:
+private:
   std::reference_wrapper<hardware_interface::LoanedCommandInterface> position_;
   std::reference_wrapper<const hardware_interface::LoanedStateInterface> feedback_;
   std::string name;
 };
 
-class SwerveController : public controller_interface::ControllerInterface {
+class SwerveController : public controller_interface::ControllerInterface
+{
   using TwistStamped = geometry_msgs::msg::TwistStamped;
   using Twist = geometry_msgs::msg::Twist;
 
- public:
+public:
   SwerveController();
 
   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;
+  controller_interface::return_type update(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
 
   CallbackReturn on_init() override;
 
-  CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override;
+  CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override;
 
-  CallbackReturn on_activate(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;
+  CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
 
-  CallbackReturn on_cleanup(const rclcpp_lifecycle::State& previous_state) override;
+  CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override;
 
-  CallbackReturn on_error(const rclcpp_lifecycle::State& previous_state) override;
+  CallbackReturn on_error(const rclcpp_lifecycle::State & previous_state) override;
 
-  CallbackReturn on_shutdown(const rclcpp_lifecycle::State& previous_state) override;
+  CallbackReturn on_shutdown(const rclcpp_lifecycle::State & previous_state) override;
 
- protected:
-  std::shared_ptr<Wheel> get_wheel(const std::string& wheel_name);
-  std::shared_ptr<Axle> get_axle(const std::string& axle_name);
+protected:
+  std::shared_ptr<Wheel> get_wheel(const std::string & wheel_name);
+  std::shared_ptr<Axle> get_axle(const std::string & axle_name);
 
   // Handles for three wheels and their axles
   std::shared_ptr<Wheel> front_left_wheel_handle_;
@@ -148,7 +154,8 @@ class SwerveController : public controller_interface::ControllerInterface {
   rclcpp::Duration publish_period_ = rclcpp::Duration::from_nanoseconds(0);
   rclcpp::Time previous_publish_timestamp_{0, 0, RCL_CLOCK_UNINITIALIZED};
 
-  struct WheelParams {
+  struct WheelParams
+  {
     double x_offset = 0.0;  // Chassis Center to Axle Center
     double y_offset = 0.0;  // Axle Center to Wheel Center
     double radius = 0.0;    // Assumed to be the same for all wheels
@@ -168,11 +175,11 @@ class SwerveController : public controller_interface::ControllerInterface {
 
   std::shared_ptr<rclcpp::Publisher<nav_msgs::msg::Odometry>> odometry_publisher_ = nullptr;
   std::shared_ptr<realtime_tools::RealtimePublisher<nav_msgs::msg::Odometry>>
-      realtime_odometry_publisher_ = nullptr;
+    realtime_odometry_publisher_ = nullptr;
   std::shared_ptr<rclcpp::Publisher<tf2_msgs::msg::TFMessage>> odometry_transform_publisher_ =
-      nullptr;
+    nullptr;
   std::shared_ptr<realtime_tools::RealtimePublisher<tf2_msgs::msg::TFMessage>>
-      realtime_odometry_transform_publisher_ = nullptr;
+    realtime_odometry_transform_publisher_ = nullptr;
 
   bool is_halted_ = false;
 

swerve_drive_controller/include/swerve_drive_controller/swerve_drive_kinematics.hpp

@@ -24,13 +24,15 @@
 #include <utility>
 #include <vector>
 
-namespace swerve_drive_controller {
+namespace swerve_drive_controller
+{
 
 /**
  * @brief Struct to represent the kinematics command for a single wheel .
  */
 
-struct WheelCommand {
+struct WheelCommand
+{
   double steering_angle;  // Steering angle in radians
   double drive_velocity;  // Drive velocity in meters per second
 };
@@ -39,20 +41,22 @@ struct WheelCommand {
  * @brief Struct to represent the odometry state of the robot.
  */
 
-struct OdometryState {
+struct OdometryState
+{
   double x;      // X position in meters
   double y;      // Y position in meters
   double theta;  // Orientation (yaw) in radians
 };
 
-class SwerveDriveKinematics {
- public:
+class SwerveDriveKinematics
+{
+public:
   /**
    * @brief Constructor for the kinematics solver.
    * @param wheel_positions Array of (x, y) positions of the wheels relative to the robot's center.
    */
 
-  explicit SwerveDriveKinematics(const std::array<std::pair<double, double>, 4>& wheel_positions);
+  explicit SwerveDriveKinematics(const std::array<std::pair<double, double>, 4> & wheel_positions);
   /**
    * @brief Compute the wheel commands based on robot velocity commands.
    * @param linear_velocity_x Linear velocity in the x direction (m/s).
@@ -61,9 +65,8 @@ class SwerveDriveKinematics {
    * @return Array of wheel commands (steering angles and drive velocities).
    */
 
-  std::array<WheelCommand, 4> compute_wheel_commands(double linear_velocity_x,
-                                                     double linear_velocity_y,
-                                                     double angular_velocity_z);
+  std::array<WheelCommand, 4> compute_wheel_commands(
+    double linear_velocity_x, double linear_velocity_y, double angular_velocity_z);
 
   /**
    * @brief Update the odometry based on wheel velocities and elapsed time.
@@ -73,10 +76,11 @@ class SwerveDriveKinematics {
    * @return Updated odometry state.
    */
 
-  OdometryState update_odometry(const std::array<double, 4>& wheel_velocities_,
-                                const std::array<double, 4>& steering_angles_, double dt);
+  OdometryState update_odometry(
+    const std::array<double, 4> & wheel_velocities_, const std::array<double, 4> & steering_angles_,
+    double dt);
 
- private:
+private:
   std::array<std::pair<double, double>, 4> wheel_positions_;  // Wheel Positions
   OdometryState odometry_;                                    // Current Odometry of the robot