fixed busy loop bug in path planner

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

@@ -167,18 +167,23 @@ def tick_caller(self):
         with self.lock:
             _, _ = self.get_world_pose_and_speed(self.self_odom_msg)
 
-        p2 = threading.Thread(target=self.planner_thread)
-        p1 = threading.Thread(target=self.local_controller_thread)
+        t_planner = threading.Thread(target=self.planner_thread)
+        t_controller = threading.Thread(target=self.local_controller_thread)
 
         # 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()
+
+        t_controller.start() #Main Cycles runs at 100hz
+        if self.has_other_buggy:
+            t_planner.start() #Planner runs every 10 hz
+
+        t_controller.join()
+        if self.has_other_buggy:
+            t_planner.join()
+
 
     def get_world_pose_and_speed(self, msg):
         current_rospose = msg.pose.pose
@@ -209,6 +214,7 @@ def local_controller_tick(self):
 
     def planner_thread(self):
         while (not rospy.is_shutdown()):
+            self.rosrate_planner.sleep()
             if not self.other_odom_msg is None:
                 with self.lock:
                     self_pose, _ = self.get_world_pose_and_speed(self.self_odom_msg)
@@ -218,7 +224,6 @@ def planner_thread(self):
                     self.distance_publisher.publish(Float64(distance))
 
                 self.planner_tick()
-                self.rosrate_planner.sleep()
 
 
     def planner_tick(self):