Opencv testing

src/autonomous_kart/autonomous_kart/nodes/opencv_pathfinder/angle_calculator.py

@@ -1,6 +1,8 @@
 from typing import Tuple
 import cv2
+import time
 
+_start_time = None
 
 def calculate_track_angles(frame: cv2.Mat) -> Tuple[float, float]:
     """
@@ -10,4 +12,28 @@ def calculate_track_angles(frame: cv2.Mat) -> Tuple[float, float]:
     :param frame: Most recent image from camera
     :return: Tuple of [left angle (degrees), right angle (degrees)]. For example, return 75.0, 75.0
     """
-    return 75.0, 75.0  # dummy
+
+    '''
+    DUMMY CODE FOR SIMULATING OPENCV FAILURE BELOW
+    '''
+    global _start_time
+
+    if _start_time is None:
+        _start_time = time.time()
+
+    elapsed = time.time() - _start_time
+
+    # normal angles for first 5 seconds
+    if elapsed < 5.0:
+        # variation for realism!!
+        left_angle = 70.0 + (elapsed * 2)  
+        right_angle = 80.0 - (elapsed * 1)
+        return left_angle, right_angle
+    # return infinity (simulating detection failure)
+    else:
+        return float('inf'), float('inf')
+
+    '''
+    OG DUMMY CODE:
+    return 75.0, 75.0  # dummy
+    '''

src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder.py

@@ -1,18 +1,53 @@
-import random
 from typing import Tuple
+from std_msgs.msg import Float32
 
-
-def pathfinder(opencv_output: Tuple):
+def pathfinder(opencv_output: Tuple, current_speed: Float32, dt: Float32, max_accel:Float32, max_steering: Float32, max_speed_straight: Float32, max_speed_turning: Float32, max_speed: Float32, logger):
     """
     Calculate commands for steering and motor from opencv_pathfinder efficiently
     Part of hot loop so must be efficient.
-    TODO(Pathfinder Team): Calculate motor_speed & steering_angle
     :param opencv_output: Tuple of [left angle from center to base of track from image (float32), right angle ...]
+    :param current_speed: Current actual speed from motor node (% of total)
+    :param dt: Time elapsed time between last calculated speed command and current
+    :param max_accel: Max allowed acceleration of kart (m/s)
+    :param  max_steering: Max steering angle (degrees)
+    :param max_speed_straight: Max allowed speed on straights (m/s)
+    :param max_speed_turning: Max allowed speed while turning (m/s)
+    :param max_speed: Max possible speed (m/s)
+    :param logger: Allows for logging info
     :return: Returns commands to motor & steering in (speed % of total, steering angle (degrees from -90 to 90 with 0 as straight)
     """
 
-    # Dummy
-    motor_speed = 20 * random.random()
-    steering_angle = 180 * random.random() - 90
+    # convert current speed from motor node to m/s
+    current_speed *= max_speed
+
+    speed_command_ms = current_speed # base speed command in m/s
+
+    theta1 = -1 * opencv_output[0]
+    theta2 = opencv_output[1]
+
+    desired_heading = (theta1 + theta2) / 2 #average of both angles
+
+    if abs(desired_heading) < 10:
+            target_speed = max_speed_straight  #max speed on straights
+    else:
+        target_speed = max_speed_turning  #target speed on turns
+
+    if target_speed > current_speed:
+        speed_command_ms = min(speed_command_ms + max_accel * dt, target_speed)
+    else:
+        speed_command_ms = max(speed_command_ms - max_accel * dt, target_speed)
+
+    steering_command = max(min(desired_heading, max_steering), -max_steering)
+
+    # calculate speed command as % of total 
+    speed_command_percent = speed_command_ms / max_speed
+
+    logger.info(f"Theta1: {theta1:.1f}°, Theta2: {theta2:.1f}°")
+    logger.info(f"Desired Heading: {desired_heading:.1f}°")
+    logger.info(f"Target Speed: {target_speed} m/s | Current Speed: {current_speed:.1f} m/s | Commanded Speed: {speed_command_percent:.3f} of total")
+    logger.info(f"Steering Command: {steering_command:.1f}° {'Left' if steering_command < 0 else 'Right' if steering_command > 0 else 'Straight'}")
+    logger.info(f"dt {dt:.3f}")
+
+
 
-    return motor_speed, steering_angle
+    return float(speed_command_percent), float(steering_command)

src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder_node.py

@@ -1,19 +1,36 @@
 import rclpy
 from rclpy.node import Node
 from std_msgs.msg import Float32MultiArray, Float32
-
 from autonomous_kart.nodes.pathfinder.pathfinder import pathfinder
-
+import numpy as np
 
 class PathfinderNode(Node):
     def __init__(self):
         super().__init__('PathfinderNode')
         self.logger = self.get_logger()
         self.angles = None
+        self.current_speed = 0.0 
 
         self.cmd_count = 0
         self.last_log_time = self.get_clock().now()
-
+        self.last_path_time = self.get_clock().now()
+
+        # declare parameters w/ defaults (for pathfinding calculations)
+        self.declare_parameter('max_accel', 3.0)
+        self.declare_parameter('max_steering', 25.0)
+        self.declare_parameter('max_speed_straight', 30.0)
+        self.declare_parameter('max_speed_turning', 15.0)
+        self.declare_parameter('max_speed', 35.0)
+        self.declare_parameter('max_angle_age', 0.5)  # max acceptable age of saved angle (s) 
+
+        # get parameter values required for pathfinding calculations
+        self.max_accel = self.get_parameter('max_accel').value
+        self.max_steering = self.get_parameter('max_steering').value
+        self.max_speed_straight = self.get_parameter('max_speed_straight').value
+        self.max_speed_turning = self.get_parameter('max_speed_turning').value
+        self.max_speed = self.get_parameter('max_speed').value
+        self.max_angle_age = self.get_parameter('max_angle_age').value
+
         self.declare_parameter('system_frequency', 60)
         self.system_frequency = self.get_parameter('system_frequency').value
 
@@ -28,14 +45,22 @@ def __init__(self):
             5
         )
 
+        # Subscriber to motor node for current speed
+        self.motor_speed_subscriber = self.create_subscription(
+            Float32,
+            'motor_speed',
+            self.motor_speed_callback,
+            5
+        )
+
         # Publisher to motor
         self.motor_publisher = self.create_publisher(
             Float32,
             'cmd_vel',
             5
         )
 
-        # # Publisher to steering
+        # Publisher to steering
         self.steering_publisher = self.create_publisher(
             Float32,
             'cmd_turn',
@@ -54,10 +79,70 @@ def calculate_path_callback(self, msg: Float32MultiArray):
         self.cmd_count += 1
         self.angles = (msg.data[0], msg.data[1])
 
-        motor_speed, steering_angle = pathfinder(msg.data)
+        current_time = self.get_clock().now()
+        dt = (current_time - self.last_path_time).nanoseconds / 1e9
+
+        self.last_path_time = self.get_clock().now()
+
+        # check if passed in angles are valid
+        theta1 = msg.data[0]
+        theta2 = msg.data[1]
+
+        angles_valid = not (np.isinf(theta1) or np.isinf(theta2) or 
+                           theta1 is None or theta2 is None)
+
+        if angles_valid:
+            # store valid angle w/ timestamp
+            self.last_valid_angles = (theta1, theta2)
+            self.last_valid_timestamp = current_time
+            angles_to_use = (theta1, theta2)
+            self.logger.debug("Using fresh angles from OpenCV")
+        else:
+            # try to use last valid angles
+            self.logger.warn("OpenCV failed to detect angles")
+
+            if self.last_valid_angles is not None and self.last_valid_timestamp is not None:
+                # check age of last saved angle
+                angle_age = (current_time - self.last_valid_timestamp).nanoseconds / 1e9
+
+                if angle_age <= self.max_angle_age:
+                    angles_to_use = self.last_valid_angles
+                    self.logger.info(f"Using cached angles ({angle_age:.3f}s old)")
+                else:
+                    # saved angle too old -- slow down
+                    self.logger.warn(f"Saved angle too old ({angle_age:.3f}s > {self.max_angle_age}s) - slowing down | Current command {self.current_speed:.3f}")
+                    slowdown_speed = self.current_speed * 0.7  # reduce current speed 30%
+                    self.motor_publisher.publish(Float32(data=slowdown_speed))
+                    self.steering_publisher.publish(Float32(data=0.0))  # go straight
+                    return
+            else:
+                # no valid angles saved -- also slow down
+                self.logger.warn(f"No valid angles available - slowing down | Current command {self.current_speed:.3f}")
+                slowdown_speed = self.current_speed * 0.7
+                self.motor_publisher.publish(Float32(data=slowdown_speed))
+                self.steering_publisher.publish(Float32(data=0.0))
+                return
+
+        motor_speed, steering_angle = pathfinder(
+            angles_to_use, 
+            self.current_speed, 
+            dt, 
+            self.max_accel,
+            self.max_steering,
+            self.max_speed_straight,
+            self.max_speed_turning,
+            self.max_speed,
+            self.logger)
 
         self.steering_publisher.publish(Float32(data=steering_angle))
         self.motor_publisher.publish(Float32(data=motor_speed))
+
+    def motor_speed_callback(self, msg: Float32):
+        """
+        Updates current speed from motor feedback (required for smooth acceleration??)
+        :param msg: Float32 containing current motor speed
+        """
+        self.current_speed = msg.data
 
     def log_command_rate(self):
         """Log average commands per second every 5 seconds"""
@@ -80,7 +165,6 @@ def main(args=None):
     rclpy.init(args=args)
 
     node = PathfinderNode()
-
     try:
         rclpy.spin(node)
     except KeyboardInterrupt:
@@ -94,4 +178,4 @@ def main(args=None):
 
 
 if __name__ == '__main__':
-    main()
+    main()

src/autonomous_kart/autonomous_kart/params/controller.yaml

@@ -5,4 +5,9 @@ motor_node:
     max_linear_speed: 5.0      # m/s
     max_angular_speed: 3.0     # rad/s
     motor_timeout: 1.0         # seconds
+    max_accel: 3.0             # m/s^2
+    max_steering: 25           # degrees
+    max_speed_straight: 30.0   # m/s | max speed on straights
+    max_speed_turning: 15.0    # m/s | speed while completing turns
+    max_angle_age: 0.5         # max acceptable age of saved angle (s)  
     simulation_mode: false     # set in motor node