pre-commit: fixed buil dissues

borsesaurabh2022 · borsesaurabh2022 · commit ce51c35578d6 · 2024-06-21T16:59:22.000+02:00
diff --git a/robotino_simulation/robotino_simulation/extract_rosbag_data.py b/robotino_simulation/robotino_simulation/extract_rosbag_data.py
@@ -1,51 +1,49 @@
 #!/usr/bin/python3
+# Licensed under GPLv2. See LICENSE file. Copyright Carologistics.
+from decimal import Decimal
 
+import matplotlib.pyplot as plt
+import pandas as pd
 import rclpy
-from rclpy.node import Node
-from nav_msgs.msg import Path
-from tf2_msgs.msg import TFMessage
 from rcl_interfaces.msg import Log
-from decimal import Decimal
-from builtin_interfaces.msg import Time
-import pandas as pd
-import matplotlib.pyplot as plt
+from rclpy.node import Node
 from tf2_ros import TransformException
 from tf2_ros.buffer import Buffer
 from tf2_ros.transform_listener import TransformListener
-from PIL import Image
+
 
 class DataExtractorNode(Node):
     def __init__(self):
-        super().__init__('data_extractor')
+        super().__init__("data_extractor")
         self.declare_parameter("namespace", "robotinobase1")
         self.namespace = self.get_parameter("namespace").get_parameter_value().string_value
         self.goal_start_time = None
         self.goal_end_time = None
 
         # Subscribe to rosout topic
-        self.rosout_sub = self.create_subscription(Log, '/rosbag_rosout', self.rosout_callback, 10)
+        self.rosout_sub = self.create_subscription(Log, "/rosbag_rosout", self.rosout_callback, 10)
 
         # Subscribe to namespace plan topic
-        #self.plan_sub = self.create_subscription(Path, self.namespace+'/plan', self.plan_callback, 10)
+        # self.plan_sub = self.create_subscription(Path, self.namespace+'/plan', self.plan_callback, 10)
 
         # Call on_timer function every second
         self.timer = self.create_timer(0.1, self.on_timer)
 
         # Declare and acquire `target_frame` parameter
-        self.target_frame = self.namespace +'/base_link'
+        self.target_frame = self.namespace + "/base_link"
         self.tf_buffer = Buffer()
         self.tf_listener = TransformListener(self.tf_buffer, self)
 
         # Define the lookup table for the poses
         self.machine_pose = {
             "C-BS-I": (5.50, 2.50, 90),
             "C-BS-O": (5.50, 4.50, 270),
-            "C-RS2-I": (3.50, 5.50,180),
-            "C-RS2-O": (5.50, 5.50,0),
-            "C-CS1-I": (0.50, 5.50,180),
-            "C-CS1-O": (2.50, 5.50,0),
-            "C-CS2-I": (0.50, 1.50,180),
-            "C-CS2-O": (2.50, 1.50,0),
+            "C-RS2-I": (3.50, 5.50, 180),
+            "C-RS2-O": (5.50, 5.50, 0),
+            "C-CS1-I": (0.50, 5.50, 180),
+            "C-CS1-O": (2.50, 5.50, 0),
+            "C-CS2-I": (0.50, 1.50, 180),
+            "C-CS2-O": (2.50, 1.50, 0),
             "C-DS-I": (0.50, 0.50, 180),
             "C-DS-O": (2.50, 0.50, 0),
             "C-RS1-I": (1.50, 4.50, 270),
@@ -54,12 +52,12 @@ def __init__(self):
             "C-SS-O": (4.50, 3.50, 270),
             "M-BS-I": (-5.50, 2.50, 90),
             "M-BS-O": (-5.50, 4.50, 270),
-            "M-RS2-I": (-3.50, 5.50,0),
-            "M-RS2-O": (-5.50, 5.50,180),
-            "M-CS1-I": (-0.50, 5.50,0),
-            "M-CS1-O": (-2.50, 5.50,180),
-            "M-CS2-I": (-0.50, 1.50,0),
-            "M-CS2-O": (-2.50, 1.50,180),
+            "M-RS2-I": (-3.50, 5.50, 0),
+            "M-RS2-O": (-5.50, 5.50, 180),
+            "M-CS1-I": (-0.50, 5.50, 0),
+            "M-CS1-O": (-2.50, 5.50, 180),
+            "M-CS2-I": (-0.50, 1.50, 0),
+            "M-CS2-O": (-2.50, 1.50, 180),
             "M-DS-I": (-0.50, 0.50, 0),
             "M-DS-O": (-2.50, 0.50, 180),
             "M-RS1-I": (-1.50, 4.50, 270),
@@ -68,7 +66,17 @@ def __init__(self):
             "M-SS-O": (-4.50, 3.50, 270),
         }
 
-        self.columns = ["pose_start_x", "pose_start_y", "pose_end_x", "pose_end_y", "machine_start", "machine_end", "time_start", "time_end", "time_diff"]
+        self.columns = [
+            "pose_start_x",
+            "pose_start_y",
+            "pose_end_x",
+            "pose_end_y",
+            "machine_start",
+            "machine_end",
+            "time_start",
+            "time_end",
+            "time_diff",
+        ]
         self.table = pd.DataFrame(columns=self.columns)
 
         self.columns_ = ["pose_x", "pose_y"]
@@ -85,7 +93,7 @@ def __init__(self):
         self.record_data = False
 
     def rosout_callback(self, msg):
-        if msg.name == self.namespace + '.bt_navigator':
+        if msg.name == self.namespace + ".bt_navigator":
             if self.msg_check(msg.msg, "Begin navigating"):
                 self.goal_start_time = self.get_time(msg)
                 self.pose_data = self.machine_pose_check(msg.msg)
@@ -98,14 +106,14 @@ def rosout_callback(self, msg):
                     "pose_end_y": [self.pose_data[3]],
                     "machine_start": [self.machine_start],
                     "machine_end": [self.machine_end],
-                    "time_start": [self.goal_start_time]
+                    "time_start": [self.goal_start_time],
                 }
                 self.table = pd.concat([self.table, pd.DataFrame(data_row)], ignore_index=True)
 
             if msg.msg == "Goal succeeded":
                 self.goal_end_time = self.get_time(msg)
                 self.table.at[self.counter, "time_end"] = self.goal_end_time
-                #self.table.at[self.counter, "time_diff"] = self.goal_end_time - self.goal_start_time
+                # self.table.at[self.counter, "time_diff"] = self.goal_end_time - self.goal_start_time
                 self.machine_start = self.machine_end
                 self.counter += 1
                 if self.counter in [1, 5, 9, 13, 17]:
@@ -117,10 +125,9 @@ def rosout_callback(self, msg):
                 #     self.table.to_csv(self.namespace+"_pose_data.csv", index=False)
                 #     self.get_logger().info(f'Pose data saved, check rosbag status and shutdown the node')
 
-
     def msg_check(self, message, check):
         words = message.split()
-        first_two = ' '.join(words[:2])
+        first_two = " ".join(words[:2])
         return first_two == check
 
     def machine_pose_check(self, message):
@@ -132,7 +139,7 @@ def machine_pose_check(self, message):
         for key, value in self.machine_pose.items():
             if end_pose_x == value[0] and end_pose_y == value[1]:
                 machine_end = key
-                self.get_logger().info(f'machine end: {machine_end}')
+                self.get_logger().info(f"machine end: {machine_end}")
                 return [start_pose_x, start_pose_y, end_pose_x, end_pose_y, machine_end]
 
     def get_time(self, msg):
@@ -141,47 +148,44 @@ def get_time(self, msg):
     def on_timer(self):
         if self.record_data:
             try:
-                t = self.tf_buffer.lookup_transform('map', self.target_frame, rclpy.time.Time())
+                t = self.tf_buffer.lookup_transform("map", self.target_frame, rclpy.time.Time())
 
             except TransformException as ex:
-                self.get_logger().info(
-                    f'Could not transform {self.target_frame} to map: {ex}')
+                self.get_logger().info(f"Could not transform {self.target_frame} to map: {ex}")
                 return
             self.robot_pose_x.append(t.transform.translation.x)
             self.robot_pose_y.append(t.transform.translation.y)
 
-            data_row = {
-                    "pose_x": [t.transform.translation.x],
-                    "pose_y": [t.transform.translation.y]
-                }
+            data_row = {"pose_x": [t.transform.translation.x], "pose_y": [t.transform.translation.y]}
             self.table_ = pd.concat([self.table_, pd.DataFrame(data_row)], ignore_index=True)
 
-
         if self.plot_data:
             quo, rem = divmod(self.counter, 4)
 
             plt.figure(quo)
 
             plt.clf()
-            plt.plot(self.robot_pose_x, self.robot_pose_y,'-b')
-            plt.xlabel('X_Position')
-            plt.ylabel('Y_Position')
-            plt.title(self.namespace+'_iteration'+str(quo)+'_followed_Path')
+            plt.plot(self.robot_pose_x, self.robot_pose_y, "-b")
+            plt.xlabel("X_Position")
+            plt.ylabel("Y_Position")
+            plt.title(self.namespace + "_iteration" + str(quo) + "_followed_Path")
             plt.grid(True)
             plt.xlim(-6, 6)
             plt.ylim(0, 6)
-            plt.gca().set_aspect('equal')
+            plt.gca().set_aspect("equal")
 
-            plt.text(self.robot_pose_x[0], self.robot_pose_y[0], 'Start', fontsize=12, color='green', ha='right')
-            plt.text(self.robot_pose_x[-1], self.robot_pose_y[-1], 'End', fontsize=12, color='red', ha='right')
+            plt.text(self.robot_pose_x[0], self.robot_pose_y[0], "Start", fontsize=12, color="green", ha="right")
+            plt.text(self.robot_pose_x[-1], self.robot_pose_y[-1], "End", fontsize=12, color="red", ha="right")
 
-            plt.savefig(self.namespace+'_iteration'+str(quo)+'_followed_path'+'.png')  # Save the plot as PNG file
-            self.table_.to_csv(self.namespace+'_iteration'+str(quo)+'_pose_data.csv', index=False)
+            plt.savefig(
+                self.namespace + "_iteration" + str(quo) + "_followed_path" + ".png"
+            )  # Save the plot as PNG file
+            self.table_.to_csv(self.namespace + "_iteration" + str(quo) + "_pose_data.csv", index=False)
 
             self.columns_ = ["pose_x", "pose_y"]
             self.table_ = pd.DataFrame(columns=self.columns_)
 
-            self.get_logger().info(f'Plotting followed path data for iteration {quo}')
+            self.get_logger().info(f"Plotting followed path data for iteration {quo}")
 
             plt.close()
 
@@ -221,16 +225,18 @@ def on_timer(self):
     #             self.plan_pose_x = []
     #             self.plan_pose_y = []
 
+
 def main(args=None):
     rclpy.init(args=args)
     node = DataExtractorNode()
     try:
-        node.get_logger().info('Beginning data extraction node, shut down with CTRL-C')
+        node.get_logger().info("Beginning data extraction node, shut down with CTRL-C")
         rclpy.spin(node)
     except KeyboardInterrupt:
-        node.get_logger().info('Keyboard interrupt, shutting down.\n')
+        node.get_logger().info("Keyboard interrupt, shutting down.\n")
     node.destroy_node()
     rclpy.shutdown()
 
-if __name__ == '__main__':
+
+if __name__ == "__main__":
     main()
diff --git a/robotino_simulation/src/robotino_driver.cpp b/robotino_simulation/src/robotino_driver.cpp
@@ -118,7 +118,7 @@ void RobotinoDriver::init(
           publish_odom(time_stamp_, time_diff_);
         }
         last_sample_time_ = curr_time_;
-        }
+      }
       {
         std::lock_guard<std::mutex> lock(vel_msg_mutex_);
         this->cmd_vel_msg.linear.x = 0.0;
@@ -143,7 +143,7 @@ void RobotinoDriver::read_data() {
 }
 
 void RobotinoDriver::publish_data() {
-  //read_data();
+  // read_data();
   double curr_time = wb_robot_get_time();
 
   // Convert double time to seconds and nanoseconds
@@ -153,10 +153,10 @@ void RobotinoDriver::publish_data() {
   time_stamp.sec = sec;
   time_stamp.nanosec = nanosec;
 
-  if (odom_source_ == "gps"){
+  if (odom_source_ == "gps") {
     publish_odom_from_sensors(time_stamp);
   }
-  //publish_odom_from_sensors(time_stamp);
+  // publish_odom_from_sensors(time_stamp);
   publish_joint_state(time_stamp);
   publish_ir(time_stamp);
   publish_laser(time_stamp);
@@ -249,12 +249,16 @@ void RobotinoDriver::publish_odom(const TimeStamp &time_stamp,
 
   auto velocity = inverse_kinematics(w0, w1, w2);
   double phi = prev_odom_omega_ + (velocity[2] * time_diff);
-  double x = prev_odom_x_ + (((velocity[0] * cos(phi)) - (velocity[1] * sin(phi))) * time_diff);
-  //double x = prev_odom_x_ + (velocity[0]*time_diff);
-  //RCLCPP_INFO(node_->get_logger(), "Position_x: %f ", x);
-  double y = prev_odom_y_ + (((velocity[0] * sin(phi)) + (velocity[1] * cos(phi))) * time_diff);
-  //double y = prev_odom_y_ + (velocity[1]*time_diff);
-  //RCLCPP_INFO(node_->get_logger(), "Position_y: %f", y);
+  double x =
+      prev_odom_x_ +
+      (((velocity[0] * cos(phi)) - (velocity[1] * sin(phi))) * time_diff);
+  // double x = prev_odom_x_ + (velocity[0]*time_diff);
+  // RCLCPP_INFO(node_->get_logger(), "Position_x: %f ", x);
+  double y =
+      prev_odom_y_ +
+      (((velocity[0] * sin(phi)) + (velocity[1] * cos(phi))) * time_diff);
+  // double y = prev_odom_y_ + (velocity[1]*time_diff);
+  // RCLCPP_INFO(node_->get_logger(), "Position_y: %f", y);
 
   std::vector<double> q = {0.0, 0.0, sin(phi / 2.), cos(phi / 2.)};
   prev_odom_x_ = x;
@@ -335,7 +339,7 @@ std::vector<double> RobotinoDriver::kinematics() {
   double v_y = this->cmd_vel_msg.linear.y;
   double omega = this->cmd_vel_msg.angular.z;
 
-  double k = (60.0*GEER_RATIO*0.009375)/(2.0*M_PI*WHEEL_RADIUS);
+  double k = (60.0 * GEER_RATIO * 0.009375) / (2.0 * M_PI * WHEEL_RADIUS);
 
   omega = omega * WHEEL_DISTANCE;
   double m1 = (((sqrt(3.) / 2.) * v_x) - (0.5 * v_y) - omega) * k;
@@ -348,13 +352,15 @@ std::vector<double> RobotinoDriver::kinematics() {
 std::vector<double> RobotinoDriver::inverse_kinematics(const double &w0,
                                                        const double &w1,
                                                        const double &w2) {
-  double k_inv = (2*M_PI*WHEEL_RADIUS)/(60*GEER_RATIO*0.009375);
+  double k_inv = (2 * M_PI * WHEEL_RADIUS) / (60 * GEER_RATIO * 0.009375);
 
   double vx = ((w0 - w2) / sqrt(3.0)) * k_inv;
-  double vy = (-((1.0 / 3.0) * w0) + ((2.0 / 3.0) * w1)- ((1.0 / 3.0) * w2)) * k_inv;
+  double vy =
+      (-((1.0 / 3.0) * w0) + ((2.0 / 3.0) * w1) - ((1.0 / 3.0) * w2)) * k_inv;
   double const_val = 3.0 * WHEEL_DISTANCE;
   double omega = (-((1.0 / const_val) * w0) - ((1.0 / const_val) * w1) -
-                  ((1.0 / const_val) * w2)) * k_inv;
+                  ((1.0 / const_val) * w2)) *
+                 k_inv;
   return {vx, vy, omega};
 }
 
