Introduced multithreading into system

rb_ws/src/buggy/scripts/auton/autonsystem.py

@@ -4,13 +4,15 @@
 import cProfile
 from threading import Lock
 
+import threading
 import rospy
 
 # ROS Message Imports
 from std_msgs.msg import Float32, Float64, Bool, Int8
 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: