Bug Fixes

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

Author: nitin2606

swerve_drive_controller/include/swerve_drive_controller/swerve_drive_controller.hpp

@@ -14,14 +14,16 @@
 #ifndef SWERVE_DRIVE_CONTROLLER__SWERVE_DRIVE_CONTROLLER_HPP_
 #define SWERVE_DRIVE_CONTROLLER__SWERVE_DRIVE_CONTROLLER_HPP_
 
+#include "swerve_drive_controller/swerve_drive_kinematics.hpp"
+
 #include <chrono>
 #include <cmath>
-#include <hardware_interface/loaned_command_interface.hpp>
 #include <memory>
 #include <queue>
 #include <string>
 #include <vector>
 
+#include <hardware_interface/loaned_command_interface.hpp>
 #include "controller_interface/controller_interface.hpp"
 #include "geometry_msgs/msg/twist.hpp"
 #include "geometry_msgs/msg/twist_stamped.hpp"
@@ -32,7 +34,6 @@
 #include "realtime_tools/realtime_box.hpp"
 #include "realtime_tools/realtime_buffer.hpp"
 #include "realtime_tools/realtime_publisher.hpp"
-#include "swerve_drive_controller/swerve_drive_kinematics.hpp"
 #include "tf2_msgs/msg/tf_message.hpp"
 
 namespace swerve_drive_controller

swerve_drive_controller/include/swerve_drive_controller/swerve_drive_kinematics.hpp

@@ -16,14 +16,16 @@
 
 #include <array>
 #include <cmath>
-#include <geometry_msgs/msg/twist.hpp>
 #include <iostream>
 #include <memory>
-#include <nav_msgs/msg/odometry.hpp>
-#include <tf2_msgs/msg/tf_message.hpp>
 #include <utility>
 #include <vector>
 
+#include <geometry_msgs/msg/twist.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <tf2_msgs/msg/tf_message.hpp>
+
+
 namespace swerve_drive_controller
 {
 

swerve_drive_controller/src/swerve_drive_controller.cpp

@@ -293,9 +293,7 @@ CallbackReturn SwerveController::on_configure(const rclcpp_lifecycle::State & /*
           }
           received_velocity_msg_ptr_.writeFromNonRT(std::move(msg));
         });
-    }
-    else
-    {
+    }else{
       velocity_command_unstamped_subscriber_ = get_node()->create_subscription<Twist>(
         DEFAULT_COMMAND_UNSTAMPED_TOPIC, rclcpp::SystemDefaultsQoS(),
         [this, logger](const std::shared_ptr<Twist> msg) -> void
@@ -325,9 +323,7 @@ CallbackReturn SwerveController::on_configure(const rclcpp_lifecycle::State & /*
     if (tf_prefix == "/")
     {
       tf_prefix = "";
-    }
-    else
-    {
+    }else{
       tf_prefix = tf_prefix + "/";
     }
 
@@ -536,39 +532,31 @@ controller_interface::return_type SwerveController::update(
     {
       front_left_axle_handle_->set_position(front_left_angle);
       front_left_wheel_handle_->set_velocity(front_left_velocity);
-    }
-    else
-    {
+    }else{
       RCLCPP_ERROR(logger, "Front Left Axle Handle or Wheel Handle is NULLPTR");
     }
 
     if (front_right_axle_handle_ != nullptr && front_right_wheel_handle_ != nullptr)
     {
       front_right_axle_handle_->set_position(front_right_angle);
       front_right_wheel_handle_->set_velocity(front_right_velocity);
-    }
-    else
-    {
+    }else{
       RCLCPP_ERROR(logger, "Front Right Axle Handle or Wheel Handle is NULLPTR");
     }
 
     if (rear_left_axle_handle_ != nullptr && rear_left_wheel_handle_ != nullptr)
     {
       rear_left_axle_handle_->set_position(rear_left_angle);
       rear_left_wheel_handle_->set_velocity(rear_left_velocity);
-    }
-    else
-    {
+    }else{
       RCLCPP_ERROR(logger, "Rear Left Axle or Wheel Handle is NULLPTR");
     }
 
     if (rear_right_axle_handle_ != nullptr && rear_right_wheel_handle_ != nullptr)
     {
       rear_right_axle_handle_->set_position(rear_right_angle);
       rear_right_wheel_handle_->set_velocity(rear_right_velocity);
-    }
-    else
-    {
+    }else{
       RCLCPP_ERROR(logger, "Rear Right Axle or Wheel Handle is NULLPTR");
     }
   }
@@ -590,9 +578,7 @@ controller_interface::return_type SwerveController::update(
       front_left_angle, front_right_angle, rear_left_angle, rear_right_angle};
     odometry_ =
       swerveDriveKinematics_.update_odometry(velocity_array, steering_angles, update_dt.seconds());
-  }
-  else
-  {
+  }else{
     double front_left_wheel_angle = front_left_axle_handle_->get_feedback();
     double front_right_wheel_angle = front_right_axle_handle_->get_feedback();
     double rear_left_wheel_angle = rear_left_axle_handle_->get_feedback();