pre-commit fix

src/hardware/sbg_translator/include/node_sbg_translator.hpp

@@ -73,9 +73,7 @@ class SBGTranslate : public rclcpp::Node {
         }
     }
 
-    double wrap_to_pi(double angle) {
-        return std::fmod(angle + M_PI, 2 * M_PI) - M_PI;
-    }
+    double wrap_to_pi(double angle) { return std::fmod(angle + M_PI, 2 * M_PI) - M_PI; }
 
     sensor_msgs::msg::Imu make_imu_msg(nav_msgs::msg::Odometry odom_msg) {
         sensor_msgs::msg::Imu imu_msg;

src/hardware/sbg_translator/src/node_sbg_translator.cpp

@@ -5,8 +5,8 @@ SBGTranslate::SBGTranslate() : Node("sbg_translator_node") {
     this->ekf_odom_sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
         "/imu/odometry", 1, std::bind(&SBGTranslate::ekf_odom_callback, this, _1));
 
-    this->imu_sub_ = this->create_subscription<sensor_msgs::msg::Imu>(
-        "/imu/data", 1, std::bind(&SBGTranslate::imu_callback, this, _1));
+    this->imu_sub_ = this->create_subscription<sensor_msgs::msg::Imu>("/imu/data", 1,
+                                                                      std::bind(&SBGTranslate::imu_callback, this, _1));
 
     // Odometry
     this->odom_pub_ = this->create_publisher<nav_msgs::msg::Odometry>("/sbg_translated/odometry", 1);
@@ -37,14 +37,14 @@ void SBGTranslate::imu_callback(sensor_msgs::msg::Imu::SharedPtr imu_data_msg) {
     imu_msg.orientation = euler_to_quat(0.0, 0.0, -yaw);
     imu_msg.angular_velocity.z = -imu_msg.angular_velocity.z;
     imu_msg.linear_acceleration.y = -imu_msg.linear_acceleration.y;
-    
+
     imu_pub_->publish(imu_msg);
 }
 
 // void SBGTranslate::ekf_odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg) {
 //     // flip x and y and invert the yaw
 //     nav_msgs::msg::Odometry odom_msg = *msg;
-    
+
 //     odom_msg.pose.pose.position.x = -msg->pose.pose.position.y;
 //     odom_msg.pose.pose.position.y = msg->pose.pose.position.x;
 
@@ -80,28 +80,26 @@ double SBGTranslate::filer_yaw(double x, double y) {
     //     std::accumulate(x_deltas_.begin(), x_deltas_.end(), 0.0) / x_deltas_.size()
     // );
     // use the difference between the first and last element
-    double yaw_vec_angle = atan2(
-        y_deltas_.back() - y_deltas_.front(),
-        x_deltas_.back() - x_deltas_.front()
-    );
+    double yaw_vec_angle = atan2(y_deltas_.back() - y_deltas_.front(), x_deltas_.back() - x_deltas_.front());
     yaw_vec_angle = wrap_to_pi(yaw_vec_angle);
 
     double K = yaw_cov_ / (yaw_cov_ + R);
 
     double yaw_update = state_[2] + K * (yaw_vec_angle - state_[2]);
     yaw_update = wrap_to_pi(yaw_update);
 
-    // RCLCPP_INFO(this->get_logger(), "Yaw measurement: %f, Yaw update: %f, cov: %f, K: %f", yaw_vec_angle, yaw_vec_angle - state_[2], yaw_cov_, K);
+    // RCLCPP_INFO(this->get_logger(), "Yaw measurement: %f, Yaw update: %f, cov: %f, K: %f", yaw_vec_angle,
+    // yaw_vec_angle - state_[2], yaw_cov_, K);
 
     // update covariance
     yaw_cov_ = (1 - K) * yaw_cov_;
 
     // prediction
     // x = x + v * dt
     double yaw_pred = yaw_update + last_yaw_rate_ * (this->now() - last_time_).seconds();
-    yaw_pred = wrap_to_pi(yaw_pred); // -pi to pi
+    yaw_pred = wrap_to_pi(yaw_pred);  // -pi to pi
 
-    yaw_cov_ += Q; // process noise, continues to increase the covariance if measurement is ignored
+    yaw_cov_ += Q;  // process noise, continues to increase the covariance if measurement is ignored
 
     // RCLCPP_INFO(this->get_logger(), "Yaw pred: %f, cov: %f", yaw_pred, yaw_cov_);
 
@@ -128,7 +126,7 @@ void SBGTranslate::ekf_odom_callback(const nav_msgs::msg::Odometry::SharedPtr ms
 
     // flip x and y and invert the yaw
     nav_msgs::msg::Odometry odom_msg = *msg;
-    
+
     odom_msg.pose.pose.position.x = -msg->pose.pose.position.y;
     odom_msg.pose.pose.position.y = msg->pose.pose.position.x;
 

src/operations/mission_controller/mission_controller/node_trackdrive_handler_nav.py

@@ -10,6 +10,7 @@
 
 import rclpy
 from rclpy.action import ActionClient
+from rclpy.node import Node
 
 from driverless_msgs.msg import Shutdown, State
 from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped
@@ -19,7 +20,6 @@
 from std_srvs.srv import Trigger
 
 from driverless_common.shutdown_node import ShutdownNode
-from rclpy.node import Node
 
 
 class TrackdriveHandler(ShutdownNode):
@@ -90,7 +90,7 @@ def state_callback(self, msg: State):
                 # reset odom and pose from camera
                 self.reset_odom_client.call_async(Trigger.Request())
                 self.reset_pose_client.call_async(Trigger.Request())
-            
+
             print("Waiting for pose to reset")
             time.sleep(2)