diff --git a/rb_ws/src/buggy/scripts/auton/autonsystem.py b/rb_ws/src/buggy/scripts/auton/autonsystem.py index c1c91ecd..e476af87 100755 --- a/rb_ws/src/buggy/scripts/auton/autonsystem.py +++ b/rb_ws/src/buggy/scripts/auton/autonsystem.py @@ -4,6 +4,7 @@ import cProfile from threading import Lock +import threading import rospy # ROS Message Imports @@ -11,6 +12,7 @@ from nav_msgs.msg import Odometry import numpy as np +import time from trajectory import Trajectory from world import World @@ -98,8 +100,11 @@ def __init__(self, ) - self.auton_rate = 100 - self.rosrate = rospy.Rate(self.auton_rate) + self.controller_rate = 100 + self.rosrate_controller = rospy.Rate(self.controller_rate) + + self.planner_rate = 10 + self.rosrate_planner = rospy.Rate(self.planner_rate) self.profile = profile self.tick_caller() @@ -164,38 +169,18 @@ def tick_caller(self): with self.lock: e, _ = self.get_world_pose_and_speed(self.self_odom_msg) - while (not rospy.is_shutdown()): - # start the actual control loop - # run the planner every 10 ticks - # the main cycle runs at 100hz, the planner runs at 10hz. - # See LOOKAHEAD_TIME in path_planner.py for the horizon of the - # planner. Make sure it is significantly (at least 2x) longer - # than 1 period of the planner when you change the planner frequency. - - if not self.other_odom_msg is None and self.ticks == 0: - # for debugging, publish distance to other buggy - with self.lock: - self_pose, _ = self.get_world_pose_and_speed(self.self_odom_msg) - other_pose, _ = self.get_world_pose_and_speed(self.other_odom_msg) - distance = (self_pose.x - other_pose.x) ** 2 + (self_pose.y - other_pose.y) ** 2 - distance = np.sqrt(distance) - self.distance_publisher.publish(Float64(distance)) - - # profiling - if self.profile: - cProfile.runctx('self.planner_tick()', globals(), locals(), sort="cumtime") - else: - self.planner_tick() - - self.local_controller_tick() - - self.ticks += 1 - - if self.ticks >= 10: - self.ticks = 0 - - self.rosrate.sleep() - + p2 = threading.Thread(target=self.planner_process) + p1 = threading.Thread(target=self.local_controller_process) + + # starting processes + # See LOOKAHEAD_TIME in path_planner.py for the horizon of the + # planner. Make sure it is significantly (at least 2x) longer + # than 1 period of the planner when you change the planner frequency. + p2.start() #Planner runs every 10 hz + p1.start() #Main Cycles runs at 100hz + + p2.join() + p1.join() def get_world_pose_and_speed(self, msg): current_rospose = msg.pose.pose @@ -207,6 +192,11 @@ def get_world_pose_and_speed(self, msg): # Get data from message pose_gps = Pose.rospose_to_pose(current_rospose) return World.gps_to_world_pose(pose_gps), current_speed + + def local_controller_process(self): + while (not rospy.is_shutdown()): + self.local_controller_tick() + self.rosrate_controller.sleep() def local_controller_tick(self): with self.lock: @@ -217,6 +207,21 @@ def local_controller_tick(self): self_pose, self.cur_traj, self_speed) steering_angle_deg = np.rad2deg(steering_angle) self.steer_publisher.publish(Float64(steering_angle_deg)) + + + def planner_process(self): + while (not rospy.is_shutdown()): + if not self.other_odom_msg is None: + with self.lock: + self_pose, _ = self.get_world_pose_and_speed(self.self_odom_msg) + other_pose, _ = self.get_world_pose_and_speed(self.other_odom_msg) + distance = (self_pose.x - other_pose.x) ** 2 + (self_pose.y - other_pose.y) ** 2 + distance = np.sqrt(distance) + self.distance_publisher.publish(Float64(distance)) + + self.planner_tick() + self.rosrate_planner.sleep() + def planner_tick(self): with self.lock: