diff --git a/.gitignore b/.gitignore index 60a6406..e8edffc 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 ce152c0..f960de7 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 932538c..3407653 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 85b996f..cf9f662 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 bc455d6..2c62b1b 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 @@ - +