diff --git a/src/autonomous_kart/autonomous_kart/nodes/opencv_pathfinder/angle_calculator.py b/src/autonomous_kart/autonomous_kart/nodes/opencv_pathfinder/angle_calculator.py index 90984b83..c0728ddd 100644 --- a/src/autonomous_kart/autonomous_kart/nodes/opencv_pathfinder/angle_calculator.py +++ b/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 \ No newline at end of file + + ''' + 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 + ''' \ No newline at end of file diff --git a/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder.py b/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder.py index 5ee078f0..acb2bcf3 100644 --- a/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder.py +++ b/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 \ No newline at end of file + return float(speed_command_percent), float(steering_command) diff --git a/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder_node.py b/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder_node.py index 7415b082..7de8f495 100644 --- a/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder_node.py +++ b/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,6 +45,14 @@ 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, @@ -35,7 +60,7 @@ def __init__(self): 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() \ No newline at end of file diff --git a/src/autonomous_kart/autonomous_kart/params/controller.yaml b/src/autonomous_kart/autonomous_kart/params/controller.yaml index 3bd8bf1a..68b25d58 100644 --- a/src/autonomous_kart/autonomous_kart/params/controller.yaml +++ b/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 \ No newline at end of file