minor refactor

Robotic-Decision-Making-Lab · Jun 10, 2024 · 3570acb · 3570acb
1 parent d40d537
commit 3570acb
Show file tree

Hide file tree

Showing 10 changed files with 73 additions and 126 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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,21 +232,13 @@ 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:
                 # for i in range(des_marker_corners.shape[0]):
                 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
@@ -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
@@ -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