From cac2ec74e9a468eccc27d86533c51dda4ebf512d Mon Sep 17 00:00:00 2001 From: Mehul Goel Date: Sun, 5 Jan 2025 12:13:38 -0800 Subject: [PATCH 1/4] pylint pt. 1 --- .gitignore | 4 +++- .../buggy/controller/controller_superclass.py | 1 - .../buggy/buggy/controller/stanley_controller.py | 14 +++++++------- rb_ws/src/buggy/buggy/util/trajectory.py | 3 --- rb_ws/src/buggy/launch/sim_2d_single.xml | 4 ++-- 5 files changed, 12 insertions(+), 14 deletions(-) diff --git a/.gitignore b/.gitignore index 60a64061..e8edffcc 100644 --- a/.gitignore +++ b/.gitignore @@ -5,4 +5,6 @@ rb_ws/bags docker-compose.yml~ *TEMP_DO_NOT_EDIT.txt rb_ws/src/buggy/bags/* -*.bag \ No newline at end of file +*.bag +.vision*/* +vision/data*/* \ No newline at end of file diff --git a/rb_ws/src/buggy/buggy/controller/controller_superclass.py b/rb_ws/src/buggy/buggy/controller/controller_superclass.py index ce152c07..f960de75 100644 --- a/rb_ws/src/buggy/buggy/controller/controller_superclass.py +++ b/rb_ws/src/buggy/buggy/controller/controller_superclass.py @@ -1,7 +1,6 @@ from abc import ABC, abstractmethod import sys -from sensor_msgs.msg import NavSatFix from nav_msgs.msg import Odometry diff --git a/rb_ws/src/buggy/buggy/controller/stanley_controller.py b/rb_ws/src/buggy/buggy/controller/stanley_controller.py index 932538c2..34076535 100644 --- a/rb_ws/src/buggy/buggy/controller/stanley_controller.py +++ b/rb_ws/src/buggy/buggy/controller/stanley_controller.py @@ -35,7 +35,7 @@ def __init__(self, start_index, namespace, node): self.debug_error_publisher = self.node.create_publisher( ROSPose, "auton/debug/error", 1 ) - + def compute_control(self, state_msg : Odometry, trajectory : Trajectory): """Computes the steering angle determined by Stanley controller. Does this by looking at the crosstrack error + heading error @@ -50,7 +50,7 @@ def compute_control(self, state_msg : Odometry, trajectory : Trajectory): if self.current_traj_index >= trajectory.get_num_points() - 1: self.node.get_logger.error("[Stanley]: Ran out of path to follow!") raise Exception("[Stanley]: Ran out of path to follow!") - + current_rospose = state_msg.pose.pose current_speed = np.sqrt( state_msg.twist.twist.linear.x**2 + state_msg.twist.twist.linear.y**2 @@ -58,7 +58,7 @@ def compute_control(self, state_msg : Odometry, trajectory : Trajectory): yaw_rate = state_msg.twist.twist.angular.z heading = current_rospose.orientation.z x, y = current_rospose.position.x, current_rospose.position.y #(Easting, Northing) - + front_x = x + StanleyController.WHEELBASE * np.cos(heading) front_y = y + StanleyController.WHEELBASE * np.sin(heading) @@ -76,12 +76,12 @@ def compute_control(self, state_msg : Odometry, trajectory : Trajectory): self.LOOK_AHEAD_DIST_CONST * current_speed, self.MIN_LOOK_AHEAD_DIST, self.MAX_LOOK_AHEAD_DIST) - + traj_dist = trajectory.get_distance_from_index(self.current_traj_index) + lookahead_dist ref_heading = trajectory.get_heading_by_index( trajectory.get_index_from_distance(traj_dist)) - + error_heading = ref_heading - heading error_heading = np.arctan2(np.sin(error_heading), np.cos(error_heading)) #Bounds error_heading @@ -103,7 +103,7 @@ def compute_control(self, state_msg : Odometry, trajectory : Trajectory): cross_track_component = -np.arctan2( StanleyController.CROSS_TRACK_GAIN * error_dist, current_speed + StanleyController.K_SOFT ) - + # Calculate yaw rate error r_meas = yaw_rate r_traj = current_speed * (trajectory.get_heading_by_index(trajectory.get_index_from_distance(traj_dist) + 0.05) @@ -118,7 +118,7 @@ def compute_control(self, state_msg : Odometry, trajectory : Trajectory): current_pose = Pose(current_rospose.position.x, current_rospose.position.y, heading) reference_error = current_pose.convert_point_from_global_to_local_frame(closest_position) reference_error -= np.array([StanleyController.WHEELBASE, 0]) #Translae back to back wheel to get accurate error - + error_pose = ROSPose() error_pose.position.x = reference_error[0] error_pose.position.y = reference_error[1] diff --git a/rb_ws/src/buggy/buggy/util/trajectory.py b/rb_ws/src/buggy/buggy/util/trajectory.py index 85b996f2..cf9f662c 100644 --- a/rb_ws/src/buggy/buggy/util/trajectory.py +++ b/rb_ws/src/buggy/buggy/util/trajectory.py @@ -1,7 +1,4 @@ import json -import time -import uuid -import matplotlib.pyplot as plt # from buggy.msg import TrajectoryMsg diff --git a/rb_ws/src/buggy/launch/sim_2d_single.xml b/rb_ws/src/buggy/launch/sim_2d_single.xml index bc455d62..2c62b1b1 100755 --- a/rb_ws/src/buggy/launch/sim_2d_single.xml +++ b/rb_ws/src/buggy/launch/sim_2d_single.xml @@ -13,11 +13,11 @@ - + From 354330b28427f03d9dc7420062edef5bde3da257 Mon Sep 17 00:00:00 2001 From: Mehul Goel Date: Sun, 5 Jan 2025 12:14:49 -0800 Subject: [PATCH 2/4] pylint?? --- rb_ws/src/buggy/buggy/controller/controller_node.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rb_ws/src/buggy/buggy/controller/controller_node.py b/rb_ws/src/buggy/buggy/controller/controller_node.py index ee1bb2fe..af60dc05 100644 --- a/rb_ws/src/buggy/buggy/controller/controller_node.py +++ b/rb_ws/src/buggy/buggy/controller/controller_node.py @@ -38,7 +38,7 @@ def __init__(self): start_index = self.cur_traj.get_index_from_distance(start_dist) self.declare_parameter("controller_name", "stanley") - match self.get_parameter("controller_name").value: + match (self.get_parameter("controller_name").value): case "stanley": self.controller = StanleyController(start_index = start_index, namespace = self.get_namespace(), node=self) #IMPORT STANLEY case _: From fa64058ccd2df4838ff921d697041f2f9fce5dd0 Mon Sep 17 00:00:00 2001 From: Mehul Goel Date: Sun, 5 Jan 2025 12:18:41 -0800 Subject: [PATCH 3/4] fixed for python 3.8 --- .../src/buggy/buggy/controller/controller_node.py | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/rb_ws/src/buggy/buggy/controller/controller_node.py b/rb_ws/src/buggy/buggy/controller/controller_node.py index af60dc05..132efce9 100644 --- a/rb_ws/src/buggy/buggy/controller/controller_node.py +++ b/rb_ws/src/buggy/buggy/controller/controller_node.py @@ -38,12 +38,14 @@ def __init__(self): start_index = self.cur_traj.get_index_from_distance(start_dist) self.declare_parameter("controller_name", "stanley") - match (self.get_parameter("controller_name").value): - case "stanley": - self.controller = StanleyController(start_index = start_index, namespace = self.get_namespace(), node=self) #IMPORT STANLEY - case _: - self.get_logger().error("Invalid Controller Name!") - raise Exception("Invalid Controller Argument") + + controller_name = self.get_parameter("controller_name").value + print(controller_name.lower) + if (controller_name.lower() == "stanley"): + self.controller = StanleyController(start_index = start_index, namespace = self.get_namespace(), node=self) #IMPORT STANLEY + else: + self.get_logger().error("Invalid Controller Name: " + controller_name.lower()) + raise Exception("Invalid Controller Argument") # Publishers self.init_check_publisher = self.create_publisher(Bool, From fcfefca10bbe739582e713036c6fd65834018af9 Mon Sep 17 00:00:00 2001 From: Mehul Goel Date: Sun, 5 Jan 2025 12:20:18 -0800 Subject: [PATCH 4/4] removed trailing whitespace --- .../buggy/buggy/controller/controller_node.py | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/rb_ws/src/buggy/buggy/controller/controller_node.py b/rb_ws/src/buggy/buggy/controller/controller_node.py index 132efce9..700bef06 100644 --- a/rb_ws/src/buggy/buggy/controller/controller_node.py +++ b/rb_ws/src/buggy/buggy/controller/controller_node.py @@ -25,7 +25,7 @@ def __init__(self): """ super().__init__('controller') self.get_logger().info('INITIALIZED.') - + #Parameters self.declare_parameter("dist", 0.0) #Starting Distance along path @@ -38,7 +38,7 @@ def __init__(self): start_index = self.cur_traj.get_index_from_distance(start_dist) self.declare_parameter("controller_name", "stanley") - + controller_name = self.get_parameter("controller_name").value print(controller_name.lower) if (controller_name.lower() == "stanley"): @@ -69,7 +69,7 @@ def __init__(self): timer_period = 0.01 # seconds (100 Hz) self.timer = self.create_timer(timer_period, self.loop) - + def odom_listener(self, msg : Odometry): ''' This is the subscriber that updates the buggies state for navigation @@ -77,7 +77,7 @@ def odom_listener(self, msg : Odometry): ''' with self.lock: self.odom = msg - + def traj_listener(self, msg): #TYPE UNKOWN AS OF NOW?? CUSTOM TYPE WHEN #TODO: FIGURE OUT BEFORE MERGE ''' This is the subscriber that updates the buggies trajectory for navigation @@ -99,11 +99,11 @@ def init_check(self): if (self.odom == None): self.get_logger().warn("WARNING: no available position estimate") return False - + elif (self.odom.pose.covariance[0] ** 2 + self.odom.pose.covariance[7] ** 2 > 1**2): self.get_logger().warn("checking position estimate certainty") return False - + #Originally under a lock, doesn't seem necessary? current_heading = self.odom.pose.pose.orientation.z closest_heading = self.cur_traj.get_heading_by_index(self.cur_traj.get_closest_index_on_path(self.odom.pose.pose.position.x, self.odom.pose.pose.position.y)) @@ -118,11 +118,11 @@ def init_check(self): current_heading = 2 * np.pi + current_heading if (closest_heading < 0): closest_heading = 2 * np.pi + closest_heading - + if (abs(current_heading - closest_heading) >= np.pi/2): self.get_logger().warn("WARNING: INCORRECT HEADING! restart stack. Current heading [-180, 180]: " + str(np.rad2deg(current_heading))) return False - + return True def loop(self): @@ -135,13 +135,13 @@ def loop(self): self.get_logger().info("Passed Initialization Check") else: return - + self.heading_publisher.publish(Float32(data=np.rad2deg(self.odom.pose.pose.orientation.z))) steering_angle = self.controller.compute_control(self.odom, self.cur_traj) steering_angle_deg = np.rad2deg(steering_angle) self.steer_publisher.publish(Float64(data=float(steering_angle_deg))) - + def main(args=None): rclpy.init(args=args)