From 4e8fadb69172f872ff65a2c6d54db1901d0d5a1d Mon Sep 17 00:00:00 2001 From: Jackack Date: Thu, 6 Apr 2023 16:25:50 -0400 Subject: [PATCH 1/6] added command line argument for path selection. Default path is buggycourse_safe_1.json --- rb_ws/src/buggy/launch/main.launch | 3 ++- rb_ws/src/buggy/launch/sim_2d.launch | 4 ++-- rb_ws/src/buggy/scripts/auton/autonsystem.py | 4 +++- 3 files changed, 7 insertions(+), 4 deletions(-) diff --git a/rb_ws/src/buggy/launch/main.launch b/rb_ws/src/buggy/launch/main.launch index a0a2c812..685afeb4 100644 --- a/rb_ws/src/buggy/launch/main.launch +++ b/rb_ws/src/buggy/launch/main.launch @@ -3,6 +3,7 @@ + @@ -27,5 +28,5 @@ - + \ No newline at end of file diff --git a/rb_ws/src/buggy/launch/sim_2d.launch b/rb_ws/src/buggy/launch/sim_2d.launch index abaff00c..5aedda79 100644 --- a/rb_ws/src/buggy/launch/sim_2d.launch +++ b/rb_ws/src/buggy/launch/sim_2d.launch @@ -5,6 +5,7 @@ + @@ -12,6 +13,5 @@ - - + diff --git a/rb_ws/src/buggy/scripts/auton/autonsystem.py b/rb_ws/src/buggy/scripts/auton/autonsystem.py index 0338ed09..07297d53 100755 --- a/rb_ws/src/buggy/scripts/auton/autonsystem.py +++ b/rb_ws/src/buggy/scripts/auton/autonsystem.py @@ -152,12 +152,14 @@ def tick(self): arg_ctrl = sys.argv[1] arg_start_dist = sys.argv[2] + arg_path = sys.argv[3] start_dist = float(arg_start_dist) print("\n\nStarting Controller: " + str(arg_ctrl) + "\n\n") + print("\n\nUsing path: /rb_ws/src/buggy/paths/" + str(arg_path) + "\n\n") print("\n\nStarting at distance: " + str(arg_start_dist) + "\n\n") - trajectory = Trajectory("/rb_ws/src/buggy/paths/frew_parkinglot_1.json") + trajectory = Trajectory("/rb_ws/src/buggy/paths/" + arg_path) # calculate starting index start_index = trajectory.get_index_from_distance(start_dist) From 2ab29f6f86e192a2fe8bdf527820c6de46d472ba Mon Sep 17 00:00:00 2001 From: Jackack Date: Sat, 30 Sep 2023 16:30:10 -0400 Subject: [PATCH 2/6] added support for multi-buggy sim --- rb_ws/src/buggy/launch/sim_2d.launch | 20 +++++++++------- .../src/buggy/scripts/2d_sim/controller_2d.py | 9 ++++---- rb_ws/src/buggy/scripts/2d_sim/engine.py | 23 +++++++++++-------- rb_ws/src/buggy/scripts/auton/autonsystem.py | 18 ++++++++------- 4 files changed, 41 insertions(+), 29 deletions(-) diff --git a/rb_ws/src/buggy/launch/sim_2d.launch b/rb_ws/src/buggy/launch/sim_2d.launch index 5aedda79..8defa95f 100644 --- a/rb_ws/src/buggy/launch/sim_2d.launch +++ b/rb_ws/src/buggy/launch/sim_2d.launch @@ -3,15 +3,19 @@ - - - - - + + + + + + + - + + + - - + + diff --git a/rb_ws/src/buggy/scripts/2d_sim/controller_2d.py b/rb_ws/src/buggy/scripts/2d_sim/controller_2d.py index a51dacd0..9598c485 100755 --- a/rb_ws/src/buggy/scripts/2d_sim/controller_2d.py +++ b/rb_ws/src/buggy/scripts/2d_sim/controller_2d.py @@ -5,9 +5,9 @@ import numpy as np class Controller: - def __init__(self): - self.steering_publisher = rospy.Publisher("sim_2d/steering", Float64, queue_size=10) - self.velocity_publisher = rospy.Publisher("buggy/velocity", Float64, queue_size=10) + def __init__(self, buggy_name): + self.steering_publisher = rospy.Publisher(buggy_name + "/input/steering", Float64, queue_size=10) + self.velocity_publisher = rospy.Publisher(buggy_name + "/velocity", Float64, queue_size=10) self.steering_angle = 0 self.velocity = 0 @@ -45,7 +45,8 @@ def set_velocity(self, vel: float): if __name__ == "__main__": rospy.init_node("sim_2d_controller") - controller = Controller() + buggy_name = sys.argv[1] + controller = Controller(buggy_name) rate = rospy.Rate(5) i = 0 while not rospy.is_shutdown(): diff --git a/rb_ws/src/buggy/scripts/2d_sim/engine.py b/rb_ws/src/buggy/scripts/2d_sim/engine.py index 5d67ce14..f48f80ed 100755 --- a/rb_ws/src/buggy/scripts/2d_sim/engine.py +++ b/rb_ws/src/buggy/scripts/2d_sim/engine.py @@ -8,6 +8,7 @@ import numpy as np import utm import time +import sys class Simulator: @@ -21,7 +22,7 @@ class Simulator: START_LONG = -79.9409643423245 NOISE = True # Noisy outputs for nav/odom? - def __init__(self, heading: float): + def __init__(self, starting_pose, buggy_name): """ Args: heading (float): degrees start heading of buggy @@ -33,10 +34,10 @@ def __init__(self, heading: float): self.pose_publisher = rospy.Publisher("nav/odom", Odometry, queue_size=1) self.steering_subscriber = rospy.Subscriber( - "buggy/input/steering", Float64, self.update_steering_angle + buggy_name + "/input/steering", Float64, self.update_steering_angle ) self.velocity_subscriber = rospy.Subscriber( - "buggy/velocity", Float64, self.update_velocity + buggy_name + "velocity", Float64, self.update_velocity ) # to plot on Foxglove (no noise) @@ -49,9 +50,11 @@ def __init__(self, heading: float): "state/pose_navsat_noisy", NavSatFix, queue_size=1 ) - # Start position for Start of Course - self.e_utm = Simulator.UTM_EAST_ZERO + 60 - self.n_utm = Simulator.UTM_NORTH_ZERO + 150 + # (UTM east, UTM north, HEADING(degs)) + self.starting_poses = { + "Hill1_SC": (Simulator.UTM_EAST_ZERO + 60, Simulator.UTM_NORTH_ZERO + 150, -110), + "Hill1_NAND": (Simulator.UTM_EAST_ZERO + 55, Simulator.UTM_NORTH_ZERO + 155, -110), + } # Start position for End of Hill 2 # self.e_utm = Simulator.UTM_EAST_ZERO - 3 @@ -66,7 +69,7 @@ def __init__(self, heading: float): # self.e_utm = utm_coords[0] # self.n_utm = utm_coords[1] - self.heading = heading # degrees + self.e_utm, self.n_utm, self.heading = self.starting_poses[starting_pose] self.velocity = 15 # m/s self.steering_angle = 0 # degrees @@ -161,7 +164,6 @@ def step(self): def publish(self): """Publishes the pose the arrow in visualizer should be at""" p = Pose() - time_stamp = rospy.Time.now() with self.lock: @@ -292,5 +294,8 @@ def loop(self): if __name__ == "__main__": rospy.init_node("sim_2d_engine") - sim = Simulator(-110) + print("sim 2d eng args:") + print(sys.argv) + starting_pose = sys.argv[1] + sim = Simulator(starting_pose) sim.loop() diff --git a/rb_ws/src/buggy/scripts/auton/autonsystem.py b/rb_ws/src/buggy/scripts/auton/autonsystem.py index 07297d53..cae804cd 100755 --- a/rb_ws/src/buggy/scripts/auton/autonsystem.py +++ b/rb_ws/src/buggy/scripts/auton/autonsystem.py @@ -40,7 +40,7 @@ class AutonSystem: ticks = 0 - def __init__(self, trajectory, controller, brake_controller) -> None: + def __init__(self, trajectory, controller, brake_controller, buggy_name) -> None: self.trajectory = trajectory self.controller = controller self.brake_controller = brake_controller @@ -53,23 +53,23 @@ def __init__(self, trajectory, controller, brake_controller) -> None: rospy.Subscriber("nav/odom", Odometry, self.update_msg) self.covariance_warning_publisher = rospy.Publisher( - "buggy/debug/is_high_covariance", Bool, queue_size=1 + buggy_name + "/debug/is_high_covariance", Bool, queue_size=1 ) self.steer_publisher = rospy.Publisher( - "buggy/input/steering", Float64, queue_size=1 + buggy_name + "/input/steering", Float64, queue_size=1 ) self.brake_publisher = rospy.Publisher( - "buggy/input/brake", Float64, queue_size=1 + buggy_name + "/input/brake", Float64, queue_size=1 ) self.brake_debug_publisher = rospy.Publisher( - "auton/debug/brake", Float64, queue_size=1 + buggy_name + "/auton/debug/brake", Float64, queue_size=1 ) self.heading_publisher = rospy.Publisher( - "auton/debug/heading", Float32, queue_size=1 + buggy_name + "/auton/debug/heading", Float32, queue_size=1 ) self.distance_publisher = rospy.Publisher( - "auton/debug/distance", Float64, queue_size=1 + buggy_name + "/auton/debug/distance", Float64, queue_size=1 ) self.auton_rate = 100 @@ -154,6 +154,7 @@ def tick(self): arg_start_dist = sys.argv[2] arg_path = sys.argv[3] start_dist = float(arg_start_dist) + buggy_name = sys.argv[4] print("\n\nStarting Controller: " + str(arg_ctrl) + "\n\n") print("\n\nUsing path: /rb_ws/src/buggy/paths/" + str(arg_path) + "\n\n") @@ -178,7 +179,8 @@ def tick(self): auton_system = AutonSystem( trajectory, ctrller, - BrakeController() + BrakeController(), + buggy_name ) while not rospy.is_shutdown(): rospy.spin() From 6a2ef8e7540b681d0cdc420bd3354a9e314af7ee Mon Sep 17 00:00:00 2001 From: Jackack Date: Mon, 2 Oct 2023 16:26:21 -0400 Subject: [PATCH 3/6] added initial velocity argument to sim --- rb_ws/src/buggy/launch/sim_2d.launch | 31 ++++++++++++------- rb_ws/src/buggy/scripts/2d_sim/engine.py | 16 +++++----- rb_ws/src/buggy/scripts/auton/autonsystem.py | 6 ++-- rb_ws/src/buggy/scripts/auton/controller.py | 11 ++++--- .../auton/model_predictive_controller.py | 8 ++--- .../scripts/auton/pure_pursuit_controller.py | 10 +++--- .../buggy/scripts/auton/stanley_controller.py | 8 ++--- 7 files changed, 51 insertions(+), 39 deletions(-) diff --git a/rb_ws/src/buggy/launch/sim_2d.launch b/rb_ws/src/buggy/launch/sim_2d.launch index 8defa95f..f4fc9fcc 100644 --- a/rb_ws/src/buggy/launch/sim_2d.launch +++ b/rb_ws/src/buggy/launch/sim_2d.launch @@ -3,19 +3,28 @@ - - - - + + + + + - - - + + + + + - - - - + + + + + + diff --git a/rb_ws/src/buggy/scripts/2d_sim/engine.py b/rb_ws/src/buggy/scripts/2d_sim/engine.py index f48f80ed..59af37c8 100755 --- a/rb_ws/src/buggy/scripts/2d_sim/engine.py +++ b/rb_ws/src/buggy/scripts/2d_sim/engine.py @@ -22,7 +22,7 @@ class Simulator: START_LONG = -79.9409643423245 NOISE = True # Noisy outputs for nav/odom? - def __init__(self, starting_pose, buggy_name): + def __init__(self, starting_pose, velocity, buggy_name): """ Args: heading (float): degrees start heading of buggy @@ -37,23 +37,23 @@ def __init__(self, starting_pose, buggy_name): buggy_name + "/input/steering", Float64, self.update_steering_angle ) self.velocity_subscriber = rospy.Subscriber( - buggy_name + "velocity", Float64, self.update_velocity + buggy_name + "/velocity", Float64, self.update_velocity ) # to plot on Foxglove (no noise) self.navsatfix_publisher = rospy.Publisher( - "state/pose_navsat", NavSatFix, queue_size=1 + buggy_name + "state/pose_navsat", NavSatFix, queue_size=1 ) # to plot on Foxglove (with noise) self.navsatfix_noisy_publisher = rospy.Publisher( - "state/pose_navsat_noisy", NavSatFix, queue_size=1 + buggy_name + "state/pose_navsat_noisy", NavSatFix, queue_size=1 ) # (UTM east, UTM north, HEADING(degs)) self.starting_poses = { "Hill1_SC": (Simulator.UTM_EAST_ZERO + 60, Simulator.UTM_NORTH_ZERO + 150, -110), - "Hill1_NAND": (Simulator.UTM_EAST_ZERO + 55, Simulator.UTM_NORTH_ZERO + 155, -110), + "Hill1_NAND": (Simulator.UTM_EAST_ZERO + 55, Simulator.UTM_NORTH_ZERO + 165, -125), } # Start position for End of Hill 2 @@ -70,7 +70,7 @@ def __init__(self, starting_pose, buggy_name): # self.n_utm = utm_coords[1] self.e_utm, self.n_utm, self.heading = self.starting_poses[starting_pose] - self.velocity = 15 # m/s + self.velocity = velocity # m/s self.steering_angle = 0 # degrees @@ -297,5 +297,7 @@ def loop(self): print("sim 2d eng args:") print(sys.argv) starting_pose = sys.argv[1] - sim = Simulator(starting_pose) + velocity = float(sys.argv[2]) + buggy_name = sys.argv[3] + sim = Simulator(starting_pose, velocity, buggy_name) sim.loop() diff --git a/rb_ws/src/buggy/scripts/auton/autonsystem.py b/rb_ws/src/buggy/scripts/auton/autonsystem.py index cae804cd..92ed4651 100755 --- a/rb_ws/src/buggy/scripts/auton/autonsystem.py +++ b/rb_ws/src/buggy/scripts/auton/autonsystem.py @@ -168,11 +168,11 @@ def tick(self): # Add Controllers Here ctrller = None if (arg_ctrl == "stanley"): - ctrller = StanleyController(start_index) + ctrller = StanleyController(buggy_name, start_index) elif (arg_ctrl == "pure_pursuit"): - ctrller = PurePursuitController(start_index) + ctrller = PurePursuitController(buggy_name, start_index) elif (arg_ctrl == "mpc"): - ctrller = ModelPredictiveController(start_index) + ctrller = ModelPredictiveController(buggy_name, start_index) if (ctrller == None): raise Exception("Invalid Controller Argument") diff --git a/rb_ws/src/buggy/scripts/auton/controller.py b/rb_ws/src/buggy/scripts/auton/controller.py index 8fc865ca..81848574 100755 --- a/rb_ws/src/buggy/scripts/auton/controller.py +++ b/rb_ws/src/buggy/scripts/auton/controller.py @@ -26,18 +26,19 @@ class Controller(ABC): current_traj_index = 0 - def __init__(self, start_index) -> None: + def __init__(self, start_index, buggy_name) -> None: + self.buggy_name = buggy_name self.trajectory_forward_1 = rospy.Publisher( - "auton/debug/forward1_navsat", NavSatFix, queue_size=1 + buggy_name + "/auton/debug/forward1_navsat", NavSatFix, queue_size=1 ) self.trajectory_forward_2 = rospy.Publisher( - "auton/debug/forward2_navsat", NavSatFix, queue_size=1 + buggy_name + "/auton/debug/forward2_navsat", NavSatFix, queue_size=1 ) self.trajectory_forward_3 = rospy.Publisher( - "auton/debug/forward3_navsat", NavSatFix, queue_size=1 + buggy_name + "/auton/debug/forward3_navsat", NavSatFix, queue_size=1 ) self.trajectory_backward_1 = rospy.Publisher( - "auton/debug/backward1_navsat", NavSatFix, queue_size=1 + buggy_name + "/auton/debug/backward1_navsat", NavSatFix, queue_size=1 ) # Make lists of publishers for easy iteration self.forward_publishers = [self.trajectory_forward_1, self.trajectory_forward_2, self.trajectory_forward_3] diff --git a/rb_ws/src/buggy/scripts/auton/model_predictive_controller.py b/rb_ws/src/buggy/scripts/auton/model_predictive_controller.py index fa1ea314..e2af6ea5 100644 --- a/rb_ws/src/buggy/scripts/auton/model_predictive_controller.py +++ b/rb_ws/src/buggy/scripts/auton/model_predictive_controller.py @@ -87,9 +87,9 @@ class ModelPredictiveController(Controller): state_cost_diag = np.diag(state_cost) control_cost_diag = np.diag(control_cost) - def __init__(self, start_index=0, ref_trajectory=None, ROS=False) -> None: + def __init__(self, buggy_name, start_index=0, ref_trajectory=None, ROS=False) -> None: # instantiate parent - super(ModelPredictiveController, self).__init__(start_index) + super(ModelPredictiveController, self).__init__(start_index, buggy_name) # Internal variables self.current_traj_index = 0 # Where in the trajectory we are currently @@ -99,10 +99,10 @@ def __init__(self, start_index=0, ref_trajectory=None, ROS=False) -> None: self.solver: osqp.OSQP = None self.debug_reference_pos_publisher = rospy.Publisher( - "auton/debug/reference_navsat", NavSatFix, queue_size=1 + buggy_name + "/auton/debug/reference_navsat", NavSatFix, queue_size=1 ) self.debug_error_publisher = rospy.Publisher( - "auton/debug/error", ROSPose, queue_size=1 + buggy_name + "/auton/debug/error", ROSPose, queue_size=1 ) self.solver = osqp.OSQP() diff --git a/rb_ws/src/buggy/scripts/auton/pure_pursuit_controller.py b/rb_ws/src/buggy/scripts/auton/pure_pursuit_controller.py index ab935c96..81f48a2f 100755 --- a/rb_ws/src/buggy/scripts/auton/pure_pursuit_controller.py +++ b/rb_ws/src/buggy/scripts/auton/pure_pursuit_controller.py @@ -21,16 +21,16 @@ class PurePursuitController(Controller): MIN_LOOK_AHEAD_DIST = 0.5 MAX_LOOK_AHEAD_DIST = 10 - def __init__(self, start_index=0) -> None: - super(PurePursuitController, self).__init__(start_index) + def __init__(self, buggy_name, start_index=0) -> None: + super(PurePursuitController, self).__init__(start_index, buggy_name) self.debug_reference_pos_publisher = rospy.Publisher( - "auton/debug/reference_navsat", NavSatFix, queue_size=1 + buggy_name + "/auton/debug/reference_navsat", NavSatFix, queue_size=1 ) self.debug_track_pos_publisher = rospy.Publisher( - "auton/debug/track_navsat", NavSatFix, queue_size=1 + buggy_name + "/auton/debug/track_navsat", NavSatFix, queue_size=1 ) self.debug_error_publisher = rospy.Publisher( - "auton/debug/error", ROSPose, queue_size=1 + buggy_name + "/auton/debug/error", ROSPose, queue_size=1 ) def compute_control( diff --git a/rb_ws/src/buggy/scripts/auton/stanley_controller.py b/rb_ws/src/buggy/scripts/auton/stanley_controller.py index 24a946d0..f2138a33 100644 --- a/rb_ws/src/buggy/scripts/auton/stanley_controller.py +++ b/rb_ws/src/buggy/scripts/auton/stanley_controller.py @@ -25,13 +25,13 @@ class StanleyController(Controller): CROSS_TRACK_GAIN = 1 HEADING_GAIN = 0.75 - def __init__(self, start_index=0) -> None: - super(StanleyController, self).__init__(start_index) + def __init__(self, buggy_name, start_index=0) -> None: + super(StanleyController, self).__init__(start_index, buggy_name) self.debug_reference_pos_publisher = rospy.Publisher( - "auton/debug/reference_navsat", NavSatFix, queue_size=1 + buggy_name + "/auton/debug/reference_navsat", NavSatFix, queue_size=1 ) self.debug_error_publisher = rospy.Publisher( - "auton/debug/error", ROSPose, queue_size=1 + buggy_name + "/auton/debug/error", ROSPose, queue_size=1 ) def compute_control( From bf224dac81ea404bea4a2d00c5c70acd97a4d439 Mon Sep 17 00:00:00 2001 From: Jackack Date: Mon, 23 Oct 2023 13:31:25 -0400 Subject: [PATCH 4/6] updated launch files --- rb_ws/src/buggy/launch/main.launch | 12 ++---------- rb_ws/src/buggy/launch/sim_2d.launch | 11 ++++++----- rb_ws/src/buggy/scripts/2d_sim/engine.py | 9 ++++----- rb_ws/src/buggy/scripts/auton/autonsystem.py | 15 ++++++++------- rb_ws/src/buggy/scripts/auton/controller.py | 8 +++----- 5 files changed, 23 insertions(+), 32 deletions(-) diff --git a/rb_ws/src/buggy/launch/main.launch b/rb_ws/src/buggy/launch/main.launch index 685afeb4..14f4cc7f 100644 --- a/rb_ws/src/buggy/launch/main.launch +++ b/rb_ws/src/buggy/launch/main.launch @@ -7,15 +7,6 @@ - @@ -28,5 +19,6 @@ - + + \ No newline at end of file diff --git a/rb_ws/src/buggy/launch/sim_2d.launch b/rb_ws/src/buggy/launch/sim_2d.launch index f4fc9fcc..2c04c0d2 100644 --- a/rb_ws/src/buggy/launch/sim_2d.launch +++ b/rb_ws/src/buggy/launch/sim_2d.launch @@ -13,18 +13,19 @@ - + + args="$(arg sc_starting_pose) $(arg sc_velocity) SC"/> + + args="$(arg sc_controller) $(arg sc_start_dist) $(arg sc_path) SC True"/> + args="$(arg nand_starting_pose) $(arg nand_velocity) Nand"/> + args="$(arg nand_controller) $(arg nand_start_dist) $(arg nand_path) Nand True"/> diff --git a/rb_ws/src/buggy/scripts/2d_sim/engine.py b/rb_ws/src/buggy/scripts/2d_sim/engine.py index 59af37c8..8bb34526 100755 --- a/rb_ws/src/buggy/scripts/2d_sim/engine.py +++ b/rb_ws/src/buggy/scripts/2d_sim/engine.py @@ -28,10 +28,10 @@ def __init__(self, starting_pose, velocity, buggy_name): heading (float): degrees start heading of buggy """ # for X11 matplotlib (direction included) - self.plot_publisher = rospy.Publisher("sim_2d/utm", Pose, queue_size=1) + self.plot_publisher = rospy.Publisher(buggy_name + "/sim_2d/utm", Pose, queue_size=1) # simulate the INS's outputs (noise included) - self.pose_publisher = rospy.Publisher("nav/odom", Odometry, queue_size=1) + self.pose_publisher = rospy.Publisher(buggy_name + "/nav/odom", Odometry, queue_size=1) self.steering_subscriber = rospy.Subscriber( buggy_name + "/input/steering", Float64, self.update_steering_angle @@ -42,12 +42,12 @@ def __init__(self, starting_pose, velocity, buggy_name): # to plot on Foxglove (no noise) self.navsatfix_publisher = rospy.Publisher( - buggy_name + "state/pose_navsat", NavSatFix, queue_size=1 + buggy_name + "/state/pose_navsat", NavSatFix, queue_size=1 ) # to plot on Foxglove (with noise) self.navsatfix_noisy_publisher = rospy.Publisher( - buggy_name + "state/pose_navsat_noisy", NavSatFix, queue_size=1 + buggy_name + "/state/pose_navsat_noisy", NavSatFix, queue_size=1 ) # (UTM east, UTM north, HEADING(degs)) @@ -73,7 +73,6 @@ def __init__(self, starting_pose, velocity, buggy_name): self.velocity = velocity # m/s self.steering_angle = 0 # degrees - self.rate = 100 # Hz self.pub_skip = 10 # publish every pub_skip ticks diff --git a/rb_ws/src/buggy/scripts/auton/autonsystem.py b/rb_ws/src/buggy/scripts/auton/autonsystem.py index 92ed4651..ba43285c 100755 --- a/rb_ws/src/buggy/scripts/auton/autonsystem.py +++ b/rb_ws/src/buggy/scripts/auton/autonsystem.py @@ -40,18 +40,19 @@ class AutonSystem: ticks = 0 - def __init__(self, trajectory, controller, brake_controller, buggy_name) -> None: + def __init__(self, trajectory, controller, brake_controller, buggy_name, is_sim) -> None: self.trajectory = trajectory self.controller = controller self.brake_controller = brake_controller self.lock = Lock() self.ticks = 0 - - self.msg = None + + if (is_sim): + rospy.Subscriber(buggy_name + "/nav/odom", Odometry, self.update_msg) + - rospy.Subscriber("nav/odom", Odometry, self.update_msg) self.covariance_warning_publisher = rospy.Publisher( buggy_name + "/debug/is_high_covariance", Bool, queue_size=1 ) @@ -67,7 +68,6 @@ def __init__(self, trajectory, controller, brake_controller, buggy_name) -> None self.heading_publisher = rospy.Publisher( buggy_name + "/auton/debug/heading", Float32, queue_size=1 ) - self.distance_publisher = rospy.Publisher( buggy_name + "/auton/debug/distance", Float64, queue_size=1 ) @@ -155,6 +155,7 @@ def tick(self): arg_path = sys.argv[3] start_dist = float(arg_start_dist) buggy_name = sys.argv[4] + is_sim = sys.argv[5] == "True" print("\n\nStarting Controller: " + str(arg_ctrl) + "\n\n") print("\n\nUsing path: /rb_ws/src/buggy/paths/" + str(arg_path) + "\n\n") @@ -164,7 +165,6 @@ def tick(self): # calculate starting index start_index = trajectory.get_index_from_distance(start_dist) - # Add Controllers Here ctrller = None if (arg_ctrl == "stanley"): @@ -180,7 +180,8 @@ def tick(self): trajectory, ctrller, BrakeController(), - buggy_name + buggy_name, + is_sim ) while not rospy.is_shutdown(): rospy.spin() diff --git a/rb_ws/src/buggy/scripts/auton/controller.py b/rb_ws/src/buggy/scripts/auton/controller.py index 81848574..5cd3aaf5 100755 --- a/rb_ws/src/buggy/scripts/auton/controller.py +++ b/rb_ws/src/buggy/scripts/auton/controller.py @@ -19,11 +19,10 @@ class Controller(ABC): Example schemes include Pure Pursuit, Stanley, and LQR. """ + # TODO: add case for buggy intrinsics NAND_WHEELBASE = 1.3 - SS_WHEELBASE = 1.3 - - WHEELBASE = NAND_WHEELBASE - + SC_WHEELBASE = 1.104 + WHEELBASE = SC_WHEELBASE current_traj_index = 0 def __init__(self, start_index, buggy_name) -> None: @@ -43,7 +42,6 @@ def __init__(self, start_index, buggy_name) -> None: # Make lists of publishers for easy iteration self.forward_publishers = [self.trajectory_forward_1, self.trajectory_forward_2, self.trajectory_forward_3] self.backward_publishers = [self.trajectory_backward_1] - self.current_traj_index = start_index @abstractmethod From f267acbcce53901efe6b40b786c0eeca6940e20b Mon Sep 17 00:00:00 2001 From: Jack He Date: Mon, 13 Nov 2023 15:17:33 -0500 Subject: [PATCH 5/6] renamed launch files and added single buggy launch file --- rb_ws/src/buggy/launch/sim_2d.launch | 31 ---------------------------- 1 file changed, 31 deletions(-) delete mode 100644 rb_ws/src/buggy/launch/sim_2d.launch diff --git a/rb_ws/src/buggy/launch/sim_2d.launch b/rb_ws/src/buggy/launch/sim_2d.launch deleted file mode 100644 index 2c04c0d2..00000000 --- a/rb_ws/src/buggy/launch/sim_2d.launch +++ /dev/null @@ -1,31 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - From 0d57d88399129e689dd9ce6c7d7be0bb14f1e492 Mon Sep 17 00:00:00 2001 From: Jack He Date: Fri, 17 Nov 2023 13:36:12 -0500 Subject: [PATCH 6/6] added new launch files --- rb_ws/src/buggy/launch/sim_2d_2buggies.launch | 31 +++++++++++++++++++ rb_ws/src/buggy/launch/sim_2d_single.launch | 21 +++++++++++++ 2 files changed, 52 insertions(+) create mode 100644 rb_ws/src/buggy/launch/sim_2d_2buggies.launch create mode 100644 rb_ws/src/buggy/launch/sim_2d_single.launch diff --git a/rb_ws/src/buggy/launch/sim_2d_2buggies.launch b/rb_ws/src/buggy/launch/sim_2d_2buggies.launch new file mode 100644 index 00000000..94b99140 --- /dev/null +++ b/rb_ws/src/buggy/launch/sim_2d_2buggies.launch @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rb_ws/src/buggy/launch/sim_2d_single.launch b/rb_ws/src/buggy/launch/sim_2d_single.launch new file mode 100644 index 00000000..02731404 --- /dev/null +++ b/rb_ws/src/buggy/launch/sim_2d_single.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + +