From 0555f62264b386838d495c4cbbc3151a6b739582 Mon Sep 17 00:00:00 2001 From: Rsleiman Date: Wed, 26 Feb 2025 15:23:49 +0000 Subject: [PATCH 1/6] Created navigation.py file containing a Navigation Node. - Subscribes to Odometry data from /ego_racecar/Odom - Callback function: 1. Processes Odom data 2. Calculates the desired heading, steering angle, and velocity (and acceleration, but may be unnecessary) - Makes use of a temporary find_desired_velocity() function, waiting on that PR to be approved and merged to main. --- navigation.py | 104 ++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 104 insertions(+) create mode 100644 navigation.py diff --git a/navigation.py b/navigation.py new file mode 100644 index 0000000..182b6ef --- /dev/null +++ b/navigation.py @@ -0,0 +1,104 @@ +import rclpy +import math # TEMP +import numpy as np +from rclpy.node import Node +from nav_msgs.msg import Odometry +from F1TenthAlgorithms.desired_acceleration import find_desired_acceleration +from F1TenthAlgorithms.desired_heading import find_desired_heading +from F1TenthAlgorithms.proportional_steering_angle import find_proportional_control_steering_angle +from tf_transformations import euler_from_quaternion +from geometry_msgs.msg import Pose, Twist + +# N.B I'm still waiting on desired_velocity function to be merged into the main branch +# TEMPORARY VELOCITY FUNCTION: + + +def find_desired_velocity( + desired_x_position_m, current_x_position_m, + desired_y_position_m, current_y_position_m, + time_to_go_s, maximum_velocity_ms +): + return min( + math.sqrt( + (desired_x_position_m - current_x_position_m)**2 + + (desired_y_position_m - current_y_position_m)**2 + ) / time_to_go_s, + maximum_velocity_ms + ) + + +class Navigation(Node): + """ + Navigation node for Simple Drive Mode #TODO: May instead have all of SDM as one node + + Subscribes to odometry, + Calculates desired heading, steering angle, and velocity, given the target + """ + + def __init__(self): + super().__init__('navigation') + # Create subscriber + self.odometrySubscriber = self.create_subscription( + Odometry, 'odom', + self.listener_callback, 10 + ) + # TODO: Integrate with other tickets (Take in target position) + self.targetPosition_fix_m = [10, 10] + self.currentPosition_fix_m = None + + def listener_callback(self, msg: Odometry): + """ + Processes Odometry data + Calculates the desired heading, steering angle, and velocity + """ + self.currentPosition_fix_m = np.array([ + msg.pose.pose.position.x, + msg.pose.pose.position.y + ]) + self.currentLinearVelocity_fix_mps = msg.twist.twist.linear.x + self.currentAngularVelocity_fix_radps = msg.twist.twist.angular.z + + self.currentOrientation_fix_rad = np.array( + euler_from_quaternion(msg.pose.pose.orientation) + ) + + self.desiredHeading_fix = find_desired_heading( + self.currentPosition_fix_m[0], + self.currentPosition_fix_m[1], + self.targetPosition_fix_m[0], + self.targetPosition_fix_m[1] + ) + + self.desiredSteeringAngle_ego_rad = find_proportional_control_steering_angle( + 0.5, + self.desiredHeading_fix, + # TODO: Confirm that yaw is the correct orientation + self.currentOrientation_fix_rad[2]) + + self.desiredVelocity_ego_mps = find_desired_velocity( + self.targetPosition_fix_m[0], + self.currentPosition_fix_m[0], + self.targetPosition_fix_m[1], + self.currentPosition_fix_m[1], + 1, + 2 + ) + + self.desiredAcceleration = find_desired_acceleration( + self.currentLinearVelocity_fix_mps, + self.desiredVelocity_ego_mps + ) + + # TODO: Integrate with other tickets (publish to /AckermannDriveStamped) + + +def main(args=None): + rclpy.init(args=args) + navigation = Navigation() + rclpy.spin(navigation) + navigation.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() From 2fdfb95ac80a2ecf7cfc702700f47c075a2c093c Mon Sep 17 00:00:00 2001 From: Rsleiman Date: Mon, 3 Mar 2025 12:23:14 +0000 Subject: [PATCH 2/6] [F1T-11_nav_functions_version_2] Small edits to nav script - Adjusted import order - Rm temporary velocity function and math import - Removed contents of main function, but kept function to use as entrypoint - Added references to tickets for TODOs --- F1TenthAlgorithms/v_des.py | 28 ---------------------------- navigation.py | 37 ++++++++----------------------------- 2 files changed, 8 insertions(+), 57 deletions(-) delete mode 100644 F1TenthAlgorithms/v_des.py diff --git a/F1TenthAlgorithms/v_des.py b/F1TenthAlgorithms/v_des.py deleted file mode 100644 index 0665514..0000000 --- a/F1TenthAlgorithms/v_des.py +++ /dev/null @@ -1,28 +0,0 @@ -import math - -def find_desired_velocity(desired_x_position_m,current_x_position_m,desired_y_position_m,current_y_position_m,time_to_go_s,maximum_velocity_ms): - - """ - Calculate the desired velocity to go from the currect position to the desired position - - Parameters: - desired_x_position_m: - The x co ordinate of the desired point - current_x_position_m: - Current x coordinate - desired_y_position_m: - Y coodinate of the desired point - current_y_position_m: - Current y coordinate - time_to_go_s: - The time to do to travel to the deired point - maximum_velocity_ms: - The vehicles maximum speed - - Returns: - desired_velocity_m/s: - Velocity needed in m/s - - """ - - return max((math.sqrt((desired_x_position_m - current_x_position_m)**2 + (desired_y_position_m - current_y_position_m)**2))/time_to_go_s , maximum_velocity_ms) diff --git a/navigation.py b/navigation.py index 182b6ef..b2001b8 100644 --- a/navigation.py +++ b/navigation.py @@ -1,30 +1,12 @@ -import rclpy -import math # TEMP import numpy as np -from rclpy.node import Node -from nav_msgs.msg import Odometry from F1TenthAlgorithms.desired_acceleration import find_desired_acceleration from F1TenthAlgorithms.desired_heading import find_desired_heading from F1TenthAlgorithms.proportional_steering_angle import find_proportional_control_steering_angle +from F1TenthAlgorithms.desired_velocity import find_desired_velocity +import rclpy +from rclpy.node import Node +from nav_msgs.msg import Odometry from tf_transformations import euler_from_quaternion -from geometry_msgs.msg import Pose, Twist - -# N.B I'm still waiting on desired_velocity function to be merged into the main branch -# TEMPORARY VELOCITY FUNCTION: - - -def find_desired_velocity( - desired_x_position_m, current_x_position_m, - desired_y_position_m, current_y_position_m, - time_to_go_s, maximum_velocity_ms -): - return min( - math.sqrt( - (desired_x_position_m - current_x_position_m)**2 + - (desired_y_position_m - current_y_position_m)**2 - ) / time_to_go_s, - maximum_velocity_ms - ) class Navigation(Node): @@ -37,12 +19,13 @@ class Navigation(Node): def __init__(self): super().__init__('navigation') + # Create subscriber self.odometrySubscriber = self.create_subscription( Odometry, 'odom', self.listener_callback, 10 ) - # TODO: Integrate with other tickets (Take in target position) + # TODO: Integrate with other tickets (Take in target position) [F1T-16] self.targetPosition_fix_m = [10, 10] self.currentPosition_fix_m = None @@ -89,15 +72,11 @@ def listener_callback(self, msg: Odometry): self.desiredVelocity_ego_mps ) - # TODO: Integrate with other tickets (publish to /AckermannDriveStamped) + # TODO: Integrate with other tickets (publish to /AckermannDriveStamped) [F1T-16] def main(args=None): - rclpy.init(args=args) - navigation = Navigation() - rclpy.spin(navigation) - navigation.destroy_node() - rclpy.shutdown() + pass if __name__ == '__main__': From 649c1e68766991f641a7d7fb81d6c8b5c9f589dd Mon Sep 17 00:00:00 2001 From: Rsleiman Date: Mon, 3 Mar 2025 15:27:52 +0000 Subject: [PATCH 3/6] [F1T-11_nav_functions_version_2] Added doxygen comment to callback function - Also corrected topic parameter from /odom to /egoracecar/odom --- navigation.py | 21 +++++++++++++++------ 1 file changed, 15 insertions(+), 6 deletions(-) diff --git a/navigation.py b/navigation.py index b2001b8..88ac1cf 100644 --- a/navigation.py +++ b/navigation.py @@ -11,7 +11,8 @@ class Navigation(Node): """ - Navigation node for Simple Drive Mode #TODO: May instead have all of SDM as one node + Navigation node for Simple Drive Mode + #TODO: May instead have all of SDM as one node Subscribes to odometry, Calculates desired heading, steering angle, and velocity, given the target @@ -22,17 +23,25 @@ def __init__(self): # Create subscriber self.odometrySubscriber = self.create_subscription( - Odometry, 'odom', - self.listener_callback, 10 + Odometry, "/egoracecar/odom", + self.ego_odometry_callback, 10 ) # TODO: Integrate with other tickets (Take in target position) [F1T-16] self.targetPosition_fix_m = [10, 10] self.currentPosition_fix_m = None - def listener_callback(self, msg: Odometry): + def ego_odometry_callback(self, msg: Odometry): """ - Processes Odometry data - Calculates the desired heading, steering angle, and velocity + Processes Odometry data: + Calculates the desired heading, steering angle, and velocity based \ + on the race car's current position and the target position. + + Parameters: + msg: + Odometry message coming from /egoracecar/odom topic + + Returns: + None: """ self.currentPosition_fix_m = np.array([ msg.pose.pose.position.x, From c02c0a06266ac6af1c1c6bd0ee0610fd4226829e Mon Sep 17 00:00:00 2001 From: Rsleiman Date: Mon, 3 Mar 2025 15:45:06 +0000 Subject: [PATCH 4/6] [F1T-11_nav_functions_version_2] Added comments across the python script --- navigation.py | 22 +++++++++++++++------- 1 file changed, 15 insertions(+), 7 deletions(-) diff --git a/navigation.py b/navigation.py index 88ac1cf..de079a0 100644 --- a/navigation.py +++ b/navigation.py @@ -27,14 +27,14 @@ def __init__(self): self.ego_odometry_callback, 10 ) # TODO: Integrate with other tickets (Take in target position) [F1T-16] - self.targetPosition_fix_m = [10, 10] + self.targetPosition_fix_m = [10, 10] # Placeholder self.currentPosition_fix_m = None def ego_odometry_callback(self, msg: Odometry): """ Processes Odometry data: - Calculates the desired heading, steering angle, and velocity based \ - on the race car's current position and the target position. + Calculates the desired heading, steering angle, and velocity based\ + on the race car's current position to reach the target position. Parameters: msg: @@ -43,17 +43,22 @@ def ego_odometry_callback(self, msg: Odometry): Returns: None: """ + # Update current position using the values in msg. self.currentPosition_fix_m = np.array([ - msg.pose.pose.position.x, - msg.pose.pose.position.y + msg.pose.pose.position.x, msg.pose.pose.position.y ]) + + # Update current velocities using the values in msg. self.currentLinearVelocity_fix_mps = msg.twist.twist.linear.x self.currentAngularVelocity_fix_radps = msg.twist.twist.angular.z + # Update current orientation using the values in msg. + # Convert from quaternion to euler angles [roll, pitch, yaw] self.currentOrientation_fix_rad = np.array( euler_from_quaternion(msg.pose.pose.orientation) ) + # Calculate the desired heading self.desiredHeading_fix = find_desired_heading( self.currentPosition_fix_m[0], self.currentPosition_fix_m[1], @@ -61,12 +66,14 @@ def ego_odometry_callback(self, msg: Odometry): self.targetPosition_fix_m[1] ) + # Calculate the desired steering angle self.desiredSteeringAngle_ego_rad = find_proportional_control_steering_angle( 0.5, self.desiredHeading_fix, - # TODO: Confirm that yaw is the correct orientation - self.currentOrientation_fix_rad[2]) + self.currentOrientation_fix_rad[2] # [2] refers to yaw + ) + # Calculate the desired velocity self.desiredVelocity_ego_mps = find_desired_velocity( self.targetPosition_fix_m[0], self.currentPosition_fix_m[0], @@ -76,6 +83,7 @@ def ego_odometry_callback(self, msg: Odometry): 2 ) + # Calculate the desired acceleration self.desiredAcceleration = find_desired_acceleration( self.currentLinearVelocity_fix_mps, self.desiredVelocity_ego_mps From f5a7a0622ce74fb007fbfde5f024dd0ef0dcbc37 Mon Sep 17 00:00:00 2001 From: Rsleiman Date: Mon, 3 Mar 2025 16:18:51 +0000 Subject: [PATCH 5/6] [F1T-11_nav_functions_version_2] Configured __init__.py file, made changes to navigation.py - __init__.py file allows us to directly import functions from the F1TenthAlgorithms scripts - Changes were made to navigation.py to follow the coding conventions from notion --- F1TenthAlgorithms/__init__.py | 15 +++++++++++++-- navigation.py | 35 +++++++++++++++++++++++++++-------- 2 files changed, 40 insertions(+), 10 deletions(-) diff --git a/F1TenthAlgorithms/__init__.py b/F1TenthAlgorithms/__init__.py index 1558d16..8945352 100644 --- a/F1TenthAlgorithms/__init__.py +++ b/F1TenthAlgorithms/__init__.py @@ -1,2 +1,13 @@ -# Do imports to add necessary function to library -# TODO +from .desired_acceleration import find_desired_acceleration +from .desired_heading import find_desired_heading +from .proportional_steering_angle import find_proportional_control_steering_angle +from .desired_velocity import find_desired_velocity +from .desired_steering_angle import find_desired_steering_angle + +__all__ = [ + "find_desired_acceleration", + "find_desired_heading", + "find_proportional_control_steering_angle", + "find_desired_velocity", + "find_desired_steering_angle" +] diff --git a/navigation.py b/navigation.py index de079a0..dd54eeb 100644 --- a/navigation.py +++ b/navigation.py @@ -1,21 +1,34 @@ +# Built in Imports import numpy as np -from F1TenthAlgorithms.desired_acceleration import find_desired_acceleration -from F1TenthAlgorithms.desired_heading import find_desired_heading -from F1TenthAlgorithms.proportional_steering_angle import find_proportional_control_steering_angle -from F1TenthAlgorithms.desired_velocity import find_desired_velocity + +# Custom library imports +from F1TenthAlgorithms import find_desired_acceleration, find_desired_heading, \ + find_desired_velocity, find_proportional_control_steering_angle + +# ROS imports import rclpy from rclpy.node import Node from nav_msgs.msg import Odometry from tf_transformations import euler_from_quaternion +# Global Variables +# None + class Navigation(Node): """ + #TODO: Navigation functionality should probably be part of \ + # one entire SDM node [F1T-16] + Navigation node for Simple Drive Mode - #TODO: May instead have all of SDM as one node - Subscribes to odometry, - Calculates desired heading, steering angle, and velocity, given the target + Topics subscribed to: + /egoracecar/odom: + Odometry + + Topics published to: TODO: Integrate with other code [F1T-16] + : + """ def __init__(self): @@ -93,7 +106,13 @@ def ego_odometry_callback(self, msg: Odometry): def main(args=None): - pass + rclpy.init(args=args) + node = Navigation() + rclpy.spin(node) + + # Destroy the node explicitly + node.destroy_node() + rclpy.shutdown() if __name__ == '__main__': From 33a410203c59410d79457f285cec988fc5cded1a Mon Sep 17 00:00:00 2001 From: Rsleiman Date: Mon, 3 Mar 2025 16:49:57 +0000 Subject: [PATCH 6/6] [F1T-11_nav_functions_version_2] Fixed topic name - From /egoracecar/odom to /ego_racecar/odom --- navigation.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/navigation.py b/navigation.py index dd54eeb..d9d5674 100644 --- a/navigation.py +++ b/navigation.py @@ -23,7 +23,7 @@ class Navigation(Node): Navigation node for Simple Drive Mode Topics subscribed to: - /egoracecar/odom: + /ego_racecar/odom: Odometry Topics published to: TODO: Integrate with other code [F1T-16] @@ -36,7 +36,7 @@ def __init__(self): # Create subscriber self.odometrySubscriber = self.create_subscription( - Odometry, "/egoracecar/odom", + Odometry, "/ego_racecar/odom", self.ego_odometry_callback, 10 ) # TODO: Integrate with other tickets (Take in target position) [F1T-16] @@ -51,7 +51,7 @@ def ego_odometry_callback(self, msg: Odometry): Parameters: msg: - Odometry message coming from /egoracecar/odom topic + Odometry message coming from /ego_racecar/odom topic Returns: None: