From 3570acb3574689af8c126fc51a3f67aef5c11056 Mon Sep 17 00:00:00 2001
From: Rakesh Vivekanandan <rakeshvivek97@gmail.com>
Date: Mon, 10 Jun 2024 15:24:08 -0700
Subject: [PATCH] minor refactor

---
 docking_control/package.xml                   |  1 -
 .../src/docking_control/auto_dock.py          |  8 ++-
 .../src/docking_control/auv_hinsdale.py       | 20 +++----
 .../src/docking_control/marker_detection.py   | 51 ++---------------
 .../src/docking_control/mission_control.py    | 56 +++++++++----------
 .../src/docking_control/mpc_acados.py         |  6 --
 docking_control/src/docking_control/video.py  | 18 +++---
 docking_control/utils/decode_sonar_log.py     | 24 ++++----
 docking_control/utils/rrt.py                  |  6 +-
 docking_control/utils/sonar_to_grid_map.py    |  9 ++-
 10 files changed, 73 insertions(+), 126 deletions(-)

diff --git a/docking_control/package.xml b/docking_control/package.xml
index 6173d3a..684e5e7 100644
--- a/docking_control/package.xml
+++ b/docking_control/package.xml
@@ -35,7 +35,6 @@
 
   <exec_depend>mavros</exec_depend>
   <exec_depend>mavros_extras</exec_depend>
-  <exec_depend>tf_transformations</exec_depend>
 
   <!-- <build_depend>octomap</build_depend>
   <build_depend>octomap-msgs</build_depend>
diff --git a/docking_control/src/docking_control/auto_dock.py b/docking_control/src/docking_control/auto_dock.py
index 4832320..2e3a209 100755
--- a/docking_control/src/docking_control/auto_dock.py
+++ b/docking_control/src/docking_control/auto_dock.py
@@ -3,7 +3,9 @@
 import numpy as np
 import rospy
 
-# sys.path.insert(0, '/home/darth/workspace/bluerov2_ws/src/bluerov2_dock/src/bluerov2_dock')
+# sys.path.insert(
+#     0, "/home/darth/workspace/bluerov2_ws/src/bluerov2_dock/src/bluerov2_dock"
+# )
 
 from auv_hinsdale import AUV
 
@@ -58,7 +60,7 @@ def __init__(self):
         self.opt_data = {}
 
     def compute_wave_number(self, g, h, T):
-        """This function computes the wave number for a given wave period and water depth.
+        """Computes the wave number for a given wave period and water depth.
 
         Args:
             g: aceeleration due to gravity
@@ -178,7 +180,7 @@ def run_mpc(self, x0, xr):
             self.comp_time = time.perf_counter() - process_t0
 
             print(
-                f"T = {round(self.t_span[self.time_id],3)}s, Time Index = {self.time_id}"
+                f"T = {round(self.t_span[self.time_id],3)}s"
             )
             print(f"Computation Time = {round(self.comp_time,3)}s")
             print("----------------------------------------------")
diff --git a/docking_control/src/docking_control/auv_hinsdale.py b/docking_control/src/docking_control/auv_hinsdale.py
index ab14ace..4953543 100755
--- a/docking_control/src/docking_control/auv_hinsdale.py
+++ b/docking_control/src/docking_control/auv_hinsdale.py
@@ -42,7 +42,8 @@ def __init__(self, vehicle_dynamics):
         self.ocean_current_data = []
         self.nu_w = np.zeros((6, 1))
 
-        # Precompute the total mass matrix (w/added mass) inverted for future dynamic calls
+        # Precompute the total mass matrix (w/added mass) inverted
+        # for future dynamic calls
         self.mass_inv = inv(self.rb_mass + self.added_mass)
         self.rb_mass_inv = inv(self.rb_mass)
         self.added_mass_inv = inv(self.added_mass)
@@ -52,7 +53,7 @@ def load_params(cls, filename):
         f = open(filename, "r")
         params = yaml.load(f.read(), Loader=yaml.SafeLoader)
 
-        l = params["l"]
+        length = params["l"]
         m = params["m"]
         Ixx = params["Ixx"]
         Iyy = params["Iyy"]
@@ -67,7 +68,7 @@ def load_params(cls, filename):
         rb_mass[3:6, 0:3] = m * skew_r_gb
 
         vehicle_dynamics = {}
-        vehicle_dynamics["vehicle_length"] = l
+        vehicle_dynamics["vehicle_length"] = length
         vehicle_dynamics["vehicle_mass"] = m
         vehicle_dynamics["rb_mass"] = rb_mass
         vehicle_dynamics["added_mass"] = -diag(SX(params["m_added"]))
@@ -86,8 +87,8 @@ def load_params(cls, filename):
         return cls(vehicle_dynamics)
 
     def compute_transformation_matrix(self, x):
-        """This function computes the transformation matrix from Body to NED frame
-        based on Fossen's book.
+        """This function computes the transformation matrix from Body to Inertial
+        frame based on Fossen's book.
 
         Args:
             x: vehicle pose 6 dof
@@ -262,7 +263,7 @@ def compute_nonlinear_dynamics(
         eta = x[0:6, :]
         nu_r = x[6:12, :]
 
-        # Gets the transformation matrix to convert from Body to NED frame
+        # Gets the transformation matrix to convert from Body to Inretial frame
         tf_mtx = self.compute_transformation_matrix(eta)
         inv(tf_mtx)
 
@@ -271,9 +272,6 @@ def compute_nonlinear_dynamics(
         # nu_c = vertcat(f, SX.zeros(3,1))
         # nu_c_dot = SX.zeros((6,1))
 
-        # Converts ocean current disturbances to Body frame
-        # nu_c = mtimes(tf_mtx_inv, nu_c_ned)
-
         # Computes total vehicle velocity
         nu = nu_r + nu_c
 
@@ -286,10 +284,6 @@ def compute_nonlinear_dynamics(
         # nu_c_dot = jacobian(nu_c, t)
 
         # Kinematic Equation
-        # Convert the relative velocity from Body to NED and add it with the
-        # ocean current velocity in NED to get the total velocity of the vehicle in NED
-
-        # eta_dot = mtimes(tf_mtx, nu_r) + nu_c_ned
         eta_dot = if_else(
             complete_model, mtimes(tf_mtx, (nu_r + nu_c)), mtimes(tf_mtx, nu_r)
         )
diff --git a/docking_control/src/docking_control/marker_detection.py b/docking_control/src/docking_control/marker_detection.py
index a69bd2a..4e8f668 100755
--- a/docking_control/src/docking_control/marker_detection.py
+++ b/docking_control/src/docking_control/marker_detection.py
@@ -46,7 +46,9 @@
 
 class Aruco:
     def __init__(self):
-        """This class is used to detect ArUco markers and estimate the pose of the ROV."""
+        """This class is used to detect ArUco markers and estimate the pose
+        of the ROV.
+        """
         self.desired_markers = [2, 7, 8, 9, 10, 11]
         # self.desired_markers = [1, 4]
 
@@ -138,8 +140,8 @@ def rotation_matrix_to_euler(self, R):
         def isRotationMatrix(R):
             Rt = np.transpose(R)
             shouldBeIdentity = np.dot(Rt, R)
-            I = np.identity(3, dtype=R.dtype)
-            n = np.linalg.norm(I - shouldBeIdentity)
+            mat_eye = np.identity(3, dtype=R.dtype)
+            n = np.linalg.norm(mat_eye - shouldBeIdentity)
             return n < 1e-6
 
         assert isRotationMatrix(R)
@@ -230,11 +232,6 @@ def marker_detection(self, timerEvent):
                 frame, des_marker_corners, detected_marker_ids
             )
 
-            # if detected_marker_ids.shape[0] > 0:
-            #     print(detected_marker_ids.shape)
-            #     print(des_marker_corners.shape)
-            #     print("Desired markers are present")
-
             # Marker Pose Estimation
             i = 0
             if detected_marker_ids.shape[0] > 0:
@@ -242,9 +239,6 @@ def marker_detection(self, timerEvent):
                 marker_id = detected_marker_ids[i]
                 marker_id_str = "marker_{0}".format(marker_id)
                 marker_size = self.marker_size[marker_id]
-                # rospy.logwarn(f"{marker_id} was detected and is used to estimate pose")
-                # print(i)
-                # rospy.loginfo("[Aruco][marker_detection] Markers detected")
 
                 marker_points = np.array(
                     [
@@ -263,8 +257,6 @@ def marker_detection(self, timerEvent):
                 rvec = np.array(rvec).T
                 tvec = np.array(tvec).T
 
-                # rospy.logwarn(f"Rotation: {rvec} \n Translation: {tvec}")
-
                 # Calculates rodrigues matrix
                 rmat, _ = cv2.Rodrigues(rvec)
                 # Convert rmat to euler
@@ -287,18 +279,6 @@ def marker_detection(self, timerEvent):
                 tf_cam_to_marker.transform.rotation.z = quat[2]
                 tf_cam_to_marker.transform.rotation.w = quat[3]
 
-                # self.tf_broadcaster.sendTransform(tf_cam_to_marker)
-
-                # rospy.loginfo(f"TF between camera_link and {marker_id_str} was broadcasted")
-                # rospy.logwarn(f"TF between cam and marker: {tf_cam_to_marker}")
-
-                # # transform lookup: base_link -> map
-                # try:
-                #     tf_base_to_map = self.tf_buffer.lookup_transform("map", "base_link", rospy.Time())
-                # except TransformException as e:
-                #     rospy.logwarn("[Aruco][marker_detection] Transform unavailable: {0}".format(e))
-                #     return
-
                 # transform lookup: base_link -> camera_link
                 try:
                     tf_base_to_camera = self.tf_buffer.lookup_transform(
@@ -352,30 +332,12 @@ def arr_to_pose(arr, frame_id):
                     pose.pose.orientation.w = quat[3]
                     return pose
 
-                # tf_base_to_map = transform_to_arr(tf_base_to_camera) @ transform_to_arr(tf_cam_to_marker) @ transform_to_arr(tf_marker_to_map)
                 tf_base_to_marker = transform_to_arr(
                     tf_base_to_camera
                 ) @ transform_to_arr(tf_cam_to_marker)
-                # rospy.logwarn(f"TF base->camera: {tf_base_to_camera}")
-                # rospy.logwarn(f"TF camera-> marker: {tf_cam_to_marker}")
-                # rospy.logwarn(f"TF base->marker: {tf_base_to_marker}")
                 tf_base_to_map = tf_base_to_marker @ transform_to_arr(tf_marker_to_map)
-                # rospy.logwarn(f"TF marker->map: {tf_marker_to_map}")
-                # rospy.logwarn(f"TF base->map: {tf_base_to_map}")
 
                 rov_pose = arr_to_pose(np.linalg.inv(tf_base_to_map), "map")
-                # rospy.logwarn(f"Not reaching here")
-
-                # rov_pose = PoseStamped()
-                # rov_pose.header.frame_id = "map"
-                # rov_pose.header.stamp = rospy.Time.now()
-                # rov_pose.pose.position.x = tf_base_to_map.transform.translation.x
-                # rov_pose.pose.position.y = tf_base_to_map.transform.translation.y
-                # rov_pose.pose.position.z = tf_base_to_map.transform.translation.z
-                # rov_pose.pose.orientation.x = tf_base_to_map.transform.rotation.x
-                # rov_pose.pose.orientation.y = tf_base_to_map.transform.rotation.y
-                # rov_pose.pose.orientation.z = tf_base_to_map.transform.rotation.z
-                # rov_pose.pose.orientation.w = tf_base_to_map.transform.rotation.w
 
                 rov_orientation = R.from_quat(
                     [
@@ -425,10 +387,7 @@ def moving_average(data):
                 rov_pose.pose.orientation.z = quat[2]
                 rov_pose.pose.orientation.w = quat[3]
 
-                # rospy.loginfo("[Aruco][marker_detection] ROV Pose: {0}".format(rov_pose))
-                # rospy.logwarn(f"Pose: {rov_pose}")
                 self.vision_pose_pub.publish(rov_pose)
-                # rospy.logwarn("Published")
 
         cv2.circle(
             frame,
diff --git a/docking_control/src/docking_control/mission_control.py b/docking_control/src/docking_control/mission_control.py
index a9ac138..6def9d1 100755
--- a/docking_control/src/docking_control/mission_control.py
+++ b/docking_control/src/docking_control/mission_control.py
@@ -6,18 +6,7 @@
 import sys
 import pandas as pd
 import os
-
-sys.path.insert(
-    0, "/home/darth/workspace/bluerov2_ws/src/bluerov2_dock/src/bluerov2_dock"
-)
-
-try:
-    pass
-except BaseException:
-    pass
-
 from auto_dock import MPControl
-
 from std_msgs.msg import Float32MultiArray, MultiArrayDimension
 from geometry_msgs.msg import PoseStamped, WrenchStamped
 from sensor_msgs.msg import Joy, BatteryState, FluidPressure
@@ -90,7 +79,6 @@ def __init__(self) -> None:
     def load_pwm_lookup(self):
         """Load the lookup table for converting thrust to pwm values"""
         cwd = os.path.dirname(__file__)
-        # csv = pd.read_csv("/home/darth/workspace/bluerov2_ws/src/bluerov2_dock/data/T200_data_16V.csv")
         csv = pd.read_csv(cwd + "/../../data/T200_data_16V.csv")
 
         thrust_vals = csv["Force"].tolist()
@@ -129,11 +117,15 @@ def initialize_subscribers(self):
         self.rov_pose_sub = rospy.Subscriber(
             "/bluerov2_dock/vision_pose/pose", PoseStamped, self.rov_pose_cb
         )
-        # self.rov_vel_sub = rospy.Subscriber('/mavros/local_position/velocity_body', TwistStamped, self.rov_vel_cb)
+        # self.rov_vel_sub = rospy.Subscriber(
+        #     "/mavros/local_position/velocity_body", TwistStamped, self.rov_vel_cb
+        # )
 
     def initialize_publishers(self):
         # Set up publishers
-        # self.control_pub = rospy.Publisher('/mavros/rc/override', OverrideRCIn, queue_size=1)
+        # self.control_pub = rospy.Publisher(
+        #     "/mavros/rc/override", OverrideRCIn, queue_size=1
+        # )
         self.control_pub = rospy.Publisher(
             "/bluerov2_dock/pwm", OverrideRCIn, queue_size=1
         )
@@ -189,12 +181,6 @@ def rov_pose_cb(self, pose):
             pose: PoseStamped message
         """
 
-        # try:
-        #     pose = self.tf_buffer.transform(data, "map_ned")
-        # except TransformException as e:
-        #     rospy.logwarn("[BlueROV2][rov_pose_cb] Could not transform pose data from map to map_ned: {}".format(e))
-        #     return
-
         try:
             x = pose.pose.orientation.x
             y = pose.pose.orientation.y
@@ -323,23 +309,23 @@ def thrust_to_pwm(self, thrust):
 
         except Exception:
             rospy.logerr_throttle(
-                10,
-                "[BlueROV2][thrust_to_pwm] Error in thrust to pwm conversion. Setting neutral pwm",
+                10, "[BlueROV2][thrust_to_pwm] Error in thrust to pwm conversion."
             )
             pwm = [self.neutral_pwm for _ in range(8)]
 
         return pwm
 
     def calculate_pwm_from_thrust_curve(self, force):
-        # The thrust curve is only accurate over the following force range, so we restrict the input
-        # forces to that range
+        # The thrust curve is only accurate over the following force range,
+        # so we restrict the input forces to that range
         min_force = -40.0
         max_force = 60.0
 
         force = np.clip(force, min_force, max_force)
 
-        # Coefficients for the 4th-degree polynomial fit to the thrust curve for the T200 run using
-        # a battery at 18v. The polynomial was identified using Matlab's `fit` function.
+        # Coefficients for the 4th-degree polynomial fit to the
+        # thrust curve for the T200 run using a battery at 18v.
+        # The polynomial was identified using Matlab's `fit` function.
         p00 = 1498
         p01 = 12.01
         p02 = -0.04731
@@ -401,9 +387,13 @@ def controller(self, joy):
                     )
                     self.auto_control(joy)
                 else:
+                    msg = """
+                    [BlueROV2][controller] Unable to switch to
+                    RC Passthrough mode. Cannot enable AUTO mode!
+                    """
                     rospy.logwarn_throttle(
                         10,
-                        "[BlueROV2][controller] Unable to switch to RC Passthrough mode. Cannot enable AUTO mode!",
+                        msg,
                     )
                     self.mode_flag = "manual"
             else:
@@ -449,9 +439,13 @@ def auto_control(self, joy):
         try:
             forces, wrench, converge_flag = self.mpc.run_mpc(x0, xr)
             if converge_flag:
+                msg = """
+                [BlueROV2][auto_control] ROV reached dock successfully!
+                Disarming now...
+                """
                 rospy.loginfo_throttle(
                     10,
-                    "[BlueROV2][auto_control] ROV reached dock successfully! Disarming now...",
+                    msg,
                 )
                 response = self.rc_passthrough_srv(False)
                 self.rc_passthrough_flag = False
@@ -514,8 +508,9 @@ def manual_control(self, joy):
 
         self.mpc.mpc.reset()
 
-        # Create a copy of axes as a list instead of a tuple so you can modify the values
-        # The RCOverrideOut message type also expects a list
+        # Create a copy of axes as a list instead of a tuple so you can
+        # modify the values.
+        # RCOverrideOut message type also expects a list
         temp_axes = list(axes)
         temp_axes[3] *= -1  # fixes reversed yaw axis
         temp_axes[0] *= -1  # fixes reversed lateral L/R axis
@@ -548,7 +543,6 @@ def manual_control(self, joy):
 
         self.light_level = override[8]
 
-        # rospy.loginfo("[BlueROV2][manual_control] Joy PWM Values: {}".format(override[0:9]))
         # Send joystick data as rc output into rc override topic
         self.control_pub.publish(override)
 
diff --git a/docking_control/src/docking_control/mpc_acados.py b/docking_control/src/docking_control/mpc_acados.py
index cb5b8c0..999defe 100755
--- a/docking_control/src/docking_control/mpc_acados.py
+++ b/docking_control/src/docking_control/mpc_acados.py
@@ -2,12 +2,6 @@
 import yaml
 from acados_template import AcadosOcp, AcadosOcpSolver, AcadosModel
 from casadi import evalf, SX, mtimes, pinv
-import sys
-
-sys.path.insert(
-    0, "/home/darth/workspace/bluerov2_ws/src/bluerov2_dock/src/bluerov2_dock"
-)
-
 from auv_hinsdale import AUV
 
 
diff --git a/docking_control/src/docking_control/video.py b/docking_control/src/docking_control/video.py
index ddad5ce..0a387dc 100755
--- a/docking_control/src/docking_control/video.py
+++ b/docking_control/src/docking_control/video.py
@@ -1,15 +1,15 @@
 #!/usr/bin/env python3
 
 import gi
+
+gi.require_version("Gst", "1.0")
+from gi.repository import Gst
+
 import numpy as np
 import rospy
 from cv_bridge import CvBridge
-
 from sensor_msgs.msg import Image
 
-gi.require_version("Gst", "1.0")
-from gi.repository import Gst
-
 
 class Video:
     """BlueRov video capture class constructor
@@ -50,17 +50,21 @@ def __init__(self):
         self.video_source = "udpsrc port={}".format(self.port)
 
         # RTSP video stream
-        # self.video_source = 'rtspsrc location=rtsp://192.168.2.2:8554/video_stream__dev_video2 latency=0'
+        # self.video_source = "rtspsrc location=rtsp://192.168.2.2:8554/" + \
+        #     "video_stream__dev_video2 latency=0"
 
         # [Rasp raw image](http://picamera.readthedocs.io/en/release-0.7/recipes2.html#raw-image-capture-yuv-format)
         # Cam -> CSI-2 -> H264 Raw (YUV 4-4-4 (12bits) I420)
         self.video_codec = (
             "! application/x-rtp, payload=96 ! rtph264depay ! h264parse ! avdec_h264"
         )
-        # self.video_codec = '! application/x-rtp, payload=96 ! rtph264depay ! h264parse ! v4l2h264dec'
+        # self.video_codec = (
+        #     "! application/x-rtp, payload=96 ! rtph264depay ! h264parse ! v4l2h264dec"
+        # )
         # self.video_codec = '! application/x-rtp, payload=96 ! rtpjpegdepay ! jpegdec'
 
-        # Python don't have nibble, convert YUV nibbles (4-4-4) to OpenCV standard BGR bytes (8-8-8)
+        # Python don't have nibble, convert YUV nibbles (4-4-4) to
+        # OpenCV standard BGR bytes (8-8-8)
         self.video_decode = "! videoconvert ! video/x-raw,format=(string)BGR"
 
         # Create a sink to get data
diff --git a/docking_control/utils/decode_sonar_log.py b/docking_control/utils/decode_sonar_log.py
index 3906440..e4fa0a6 100755
--- a/docking_control/utils/decode_sonar_log.py
+++ b/docking_control/utils/decode_sonar_log.py
@@ -1,12 +1,8 @@
 #!/usr/bin/env python3
 
-import struct, sys, re
-
-# 3.7 for dataclasses, 3.8 for walrus (:=) in recovery
-assert (
-    sys.version_info.major >= 3 and sys.version_info.minor >= 8
-), "Python version should be at least 3.8."
-
+import struct
+import sys
+import re
 from brping import PingParser
 from dataclasses import dataclass
 from typing import IO, Any, Set
@@ -292,17 +288,17 @@ def laser_to_occ_map(ang, dist):
                 intensities = np.array(list((itertools.chain(*intensities))))
                 angles = np.array(list((itertools.chain(*angles))))
 
-                for l in range(len(ranges)):
-                    if ranges[l] <= 5.5:
-                        intensities[l] = 0.0
+                for i in range(len(ranges)):
+                    if ranges[i] <= 5.5:
+                        intensities[i] = 0.0
 
                 dis = []
                 ang = []
 
-                for l in range(len(ranges)):
-                    if intensities[l] > 50.0:
-                        dis.append(ranges[l])
-                        ang.append(angles[l])
+                for i in range(len(ranges)):
+                    if intensities[i] > 50.0:
+                        dis.append(ranges[i])
+                        ang.append(angles[i])
 
                 dis = np.array(dis)
                 ang = np.array(ang)
diff --git a/docking_control/utils/rrt.py b/docking_control/utils/rrt.py
index 7b229c3..2f6ca05 100644
--- a/docking_control/utils/rrt.py
+++ b/docking_control/utils/rrt.py
@@ -18,8 +18,10 @@ def __init__(self, x, y):
 
     def __init__(self):
         """Initialize the RRT class"""
-        # self.filename = "/Users/rakeshvivekanandan/workspace/bluerov2_dock/data/occ_map_binary.npy"
-        self.filename = "/home/darth/workspace/bluerov2_ws/src/bluerov2_dock/data/occ_map_binary.npy"
+        # self.filename = "/Users/rakeshvivekanandan/workspace/" + \
+        #     "bluerov2_dock/data/occ_map_binary.npy"
+        self.filename = "/home/darth/workspace/bluerov2_ws/" + \
+            "src/bluerov2_dock/data/occ_map_binary.npy"
         self.load_map()
         self.bounds = np.array([[0, self.occ_map.shape[0]], [0, self.occ_map.shape[1]]])
         # self.min_rand = 0
diff --git a/docking_control/utils/sonar_to_grid_map.py b/docking_control/utils/sonar_to_grid_map.py
index 12e32c3..f98a875 100644
--- a/docking_control/utils/sonar_to_grid_map.py
+++ b/docking_control/utils/sonar_to_grid_map.py
@@ -109,11 +109,14 @@ def generate_ray_casting_grid_map(ox, oy, xy_resolution):
         # y coordinate of the the occupied area
         iy = int(np.floor((y - min_y) / xy_resolution))
 
-        # laser_beams = bresenham((center_x, center_y), (ix, iy))  # line form the lidar to the occupied point
+        # laser_beams = bresenham((center_x, center_y), (ix, iy))
         # for laser_beam in laser_beams:
-        #     if laser_beam[0] < occupancy_map.shape[0] and laser_beam[1] < occupancy_map.shape[1]:
+        #     if (
+        #         laser_beam[0] < occupancy_map.shape[0]
+        #         and laser_beam[1] < occupancy_map.shape[1]
+        #     ):
         #         if occupancy_map[laser_beam[0]][laser_beam[1]] != 1.0:
-        #             occupancy_map[laser_beam[0]][laser_beam[1]] = 0.0  # free area 0.0
+        #             occupancy_map[laser_beam[0]][laser_beam[1]] = 0.0
 
         if ix < max_x:
             occupancy_map[ix + 1][iy] = 1.0  # extend the occupied area