updated nodes again

catkin_ws/src/asclinic_pkg/src/nodes/CV_localization.py

@@ -51,7 +51,7 @@ def aruco_callback(msg_array):
     headings = []
 
     # Define detection threshold 
-    max_valid_distance = 4.0
+    max_valid_distance = 3.0
 
     for msg in msg_array.markers:
         marker_id = msg.id

catkin_ws/src/asclinic_pkg/src/nodes/aruco_detector.py

@@ -328,8 +328,10 @@ class ArucoDetector:
         #   saved during the calibration procedure.
         # > Note the that values hardcoded here may give
         #   meaningless results for your camera
-        self.intrinic_camera_matrix = np.array( [[1.32623821e+03, 0, 9.73271322e+02], [0, 1.32631111e+03, 4.81651983e+02], [0, 0, 1.00000000e+00]], dtype=float)
-        self.intrinic_camera_distortion  = np.array( [[-0.03796606, 0.1984374, -0.00882755, 0.00455549, -1.92404421]], dtype=float)
+        self.intrinic_camera_matrix = np.array( [[1.50262548e+03, 0.00000000e+00, 9.29351251e+02],
+                                                 [0.00000000e+00, 1.50705720e+03, 6.06739695e+02],
+                                                 [0.00000000e+00, 0.00000000e+00, 1.00000000e+00]], dtype=float)
+        self.intrinic_camera_distortion  = np.array( [[-0.05101587,  0.12287484,  0.02034384, -0.00123042, -0.10561441]], dtype=float)
 
         # Read the a camera frame as a double check of the properties
         # > Read the frame

catkin_ws/src/asclinic_pkg/src/nodes/motion_executor.py

@@ -29,7 +29,7 @@ def run_sequence(file_path):
             duration = dist / lin_speed if lin_speed != 0 else 0
             motion_type = f"Straight: {dist} m @ {lin_speed} m/s"
         elif angle != 0:
-            twist.angular.z = ang_speed
+            twist.angular.z = math.radians(ang_speed)
             duration = abs(angle / ang_speed) if ang_speed != 0 else 0
             motion_type = f"Rotate: {angle}° @ {ang_speed}°/s"
         else:

catkin_ws/src/asclinic_pkg/src/nodes/motion_sequence.csv

 distance,angle,linear_speed,angular_speed
+4,0,0.2,0
 0,180,0,100
\ No newline at end of file

catkin_ws/src/asclinic_pkg/src/nodes/odometry_node.py

@@ -10,7 +10,7 @@ import math
 class OdometryNode:
     def __init__(self):
         self.r = 0.072  # wheel radius (m)
-        self.b = 0.22   # wheelbase (m)
+        self.b = 0.22*(1190.71/180)   # wheelbase (m)
         self.last_ticks = None
         self.pose = [0.0, 0.0, 0.0]  # [x, y, phi]
         self.last_cmd_vel = Twist()  # stores last received twist
@@ -42,8 +42,8 @@ class OdometryNode:
         ang = self.last_cmd_vel.angular.z
 
         # Apply sign logic
-        sign_l = -1 if ang > 0 else 1 if ang < 0 else 0
-        sign_r = 1 if ang > 0 else -1 if ang < 0 else 0
+        sign_l = -1 if ang > 0 else 1 if ang < 0 or lin>0 else 0
+        sign_r = 1 if ang > 0 or lin>0 else -1 if ang < 0 else 0
 
         # Apply signs to ticks
         dtl = raw_dtl * sign_l
@@ -54,7 +54,7 @@ class OdometryNode:
             return
 
         # Odometry computation
-        ds = self.r * (dtl + dtr) / 2.0
+        ds = (self.r * (dtl + dtr) / 2.0)
         dphi = self.r * (dtr - dtl) / (2.0 * self.b)
 
         x, y, phi = self.pose

catkin_ws/src/asclinic_pkg/src/nodes/velocity_to_duty_node.py

@@ -13,7 +13,7 @@ class VelocityToDuty:
 
         # Tuning constants
         self.k_lin = 0.0090
-        self.k_ang = 1.89
+        self.k_ang = 0.03253
 
         # Hardware reversal (forward = negative duty)
         self.motor_invert_left = -1.0
@@ -43,7 +43,7 @@ class VelocityToDuty:
 
 
         # Log
-        rospy.loginfo("[VDC] Received Twist: linear=%.3f m/s, angular=%.3f deg/s", linear_vel, angular_vel)
+        #rospy.loginfo("[VDC] Received Twist: linear=%.3f m/s, angular=%.3f rad/s", linear_vel, angular_vel)
         rospy.loginfo("[VDC] Computed Duty: left=%.2f%%, right=%.2f%%", left_duty, right_duty)
 
         # Publish