added launch options and path planner support for auton system

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

@@ -15,12 +15,14 @@
 from controller import Controller
 from pure_pursuit_controller import PurePursuitController
 from stanley_controller import StanleyController
-from model_predictive_controller import ModelPredictiveController
 from brake_controller import BrakeController
-# from model_predictive_controller import ModelPredictiveController
-from model_predictive_interpolation import ModelPredictiveController
+from model_predictive_controller import ModelPredictiveController
+# from model_predictive_interpolation import ModelPredictiveController
+from path_planner import PathPlanner
 from pose import Pose
 
+import argparse
+import cProfile
 
 class AutonSystem:
     """
@@ -29,159 +31,252 @@ class AutonSystem:
     On every tick, this class will read the current state of the buggy,
     compare it to the reference trajectory, and use a given controller
     to compute the desired control output.
+
+    On every 10th tick, this class will generate a new reference trajectory
+    based on the updated position of the other buggy.
     """
 
-    trajectory: Trajectory = None
-    controller: Controller = None
+    global_trajectory: Trajectory = None
+    local_controller: Controller = None
     brake_controller: BrakeController = None
     lock = None
 
     steer_publisher = None
-
     ticks = 0
 
-    def __init__(self, trajectory, controller, brake_controller, buggy_name, is_sim) -> None:
-        self.trajectory = trajectory
-        self.controller = controller
+    def __init__(self,
+            global_trajectory, 
+            local_controller,
+            brake_controller, 
+            self_name, 
+            other_name,
+            profile) -> None:
+
+
+        self.global_trajectory = global_trajectory
+
+        # local trajectory is initialized as global trajectory. If there is no other buggy,
+        # the local trajectory is never updated.
+
+        self.has_other_buggy = not other_name is None
+        self.cur_traj = global_trajectory
+        self.local_controller = local_controller
         self.brake_controller = brake_controller
 
+        self.path_planner = PathPlanner(global_trajectory)
+        self.other_steering = 0
+
         self.lock = Lock()
         self.ticks = 0
-        self.msg = None
+        self.self_odom_msg = None
+        self.other_odom_msg = None
 
-        if (is_sim):
-            rospy.Subscriber(buggy_name + "/nav/odom", Odometry, self.update_msg)
+        rospy.Subscriber(self_name + "/nav/odom", Odometry, self.update_self_odom)
+        if self.has_other_buggy:
+            rospy.Subscriber(other_name + "/nav/odom", Odometry, self.update_other_odom)
+            self.other_steer_subscriber = rospy.Subscriber(
+                other_name + "/input/steering", Float64, self.update_other_steering_angle
+            )
 
 
         self.covariance_warning_publisher = rospy.Publisher(
-            buggy_name + "/debug/is_high_covariance", Bool, queue_size=1
+            self_name + "/debug/is_high_covariance", Bool, queue_size=1
         )
         self.steer_publisher = rospy.Publisher(
-            buggy_name + "/input/steering", Float64, queue_size=1
+            self_name + "/input/steering", Float64, queue_size=1
         )
         self.brake_publisher = rospy.Publisher(
-            buggy_name + "/input/brake", Float64, queue_size=1
+            self_name + "/input/brake", Float64, queue_size=1
         )
         self.brake_debug_publisher = rospy.Publisher(
-            buggy_name + "/auton/debug/brake", Float64, queue_size=1
+            self_name + "/auton/debug/brake", Float64, queue_size=1
         )
         self.heading_publisher = rospy.Publisher(
-             buggy_name + "/auton/debug/heading", Float32, queue_size=1
+            self_name + "/auton/debug/heading", Float32, queue_size=1
         )
         self.distance_publisher = rospy.Publisher(
-             buggy_name + "/auton/debug/distance", Float64, queue_size=1
+            self_name + "/auton/debug/distance", Float64, queue_size=1
         )
 
+
         self.auton_rate = 100
         self.rosrate = rospy.Rate(self.auton_rate)
 
+        self.profile = profile
         self.tick_caller()
 
-    def update_speed(self, msg):
+    def update_self_odom(self, msg):
         with self.lock:
-            self.speed = msg.data
+            self.self_odom_msg = msg
 
-    def update_msg(self, msg):
+    def update_other_odom(self, msg):
         with self.lock:
-            self.msg = msg
-
+            self.other_odom_msg = msg
+
+    def update_other_steering_angle(self, msg):
+        with self.lock:
+            self.other_steering = msg.data
+
     def tick_caller(self):
-        while ((not rospy.is_shutdown()) and (self.msg == None)): # with no message, we wait
+        while ((not rospy.is_shutdown()) and
+            (self.self_odom_msg == None or 
+            (self.has_other_buggy and self.other_odom_msg == None))): # with no message, we wait
             rospy.sleep(0.001)
 
         # wait for covariance matrix to be better
         while ((not rospy.is_shutdown()) and
-               (self.msg.pose.covariance[0] ** 2 + self.msg.pose.covariance[7] ** 2 > 1**2)):
+               (self.self_odom_msg.pose.covariance[0] ** 2 + self.self_odom_msg.pose.covariance[7] ** 2 > 1**2)):
             # Covariance larger than one meter. We definitely can't trust the pose
             rospy.sleep(0.001)
             print("Waiting for Covariance to be better: ",  rospy.get_rostime())
         print("done checking covariance")
-        while (not rospy.is_shutdown()): # start the actual control loop
-            self.tick()
-            self.ticks += 1
-            self.rosrate.sleep()
 
-    def tick(self):
-        # Received an updated pose from the state estimator
-        # Compute the new control output and publish it to the buggy
+        # initialize global trajectory index
+
         with self.lock:
-            msg = self.msg
-        current_rospose = msg.pose.pose
+            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
+            # thr main cycle runs at 100hz, the planner runs at 10hz, but looks 1 second ahead
+
+            if not self.other_odom_msg is None and self.ticks % 10 == 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
+            self.rosrate.sleep() 
+
+
+    def get_world_pose_and_speed(self, msg):
+        current_rospose = msg.pose.pose
         # Check if the pose covariance is a sane value. Publish a warning if insane
         if msg.pose.covariance[0] ** 2 + msg.pose.covariance[7] ** 2 > 1**2:
             # Covariance larger than one meter. We definitely can't trust the pose
             self.covariance_warning_publisher.publish(Bool(True))
         else:
             self.covariance_warning_publisher.publish(Bool(False))
-
         current_speed = np.sqrt(
             msg.twist.twist.linear.x**2 + msg.twist.twist.linear.y**2
         )
 
         # Get data from message
         pose_gps = Pose.rospose_to_pose(current_rospose)
-        pose = World.gps_to_world_pose(pose_gps)
+        return World.gps_to_world_pose(pose_gps), current_speed
+
+    def local_controller_tick(self):
+        with self.lock:
+            self_pose, self_speed = self.get_world_pose_and_speed(self.self_odom_msg)
 
         # Compute control output
-        steering_angle = self.controller.compute_control(
-            pose, self.trajectory, current_speed
-        )
-
-        # Publish control output
+        steering_angle = self.local_controller.compute_control(
+            self_pose, self.cur_traj, self_speed)
         steering_angle_deg = np.rad2deg(steering_angle)
         self.steer_publisher.publish(Float64(steering_angle_deg))
-        brake_cmd = self.brake_controller.compute_braking(current_speed, steering_angle_deg)
-        self.brake_debug_publisher.publish(Float64(brake_cmd))
-        self.brake_publisher.publish(Float64(0)) # No braking for now, just look at debug data
-
-        # Publish debug data
-        self.heading_publisher.publish(Float32(pose.theta))
-
-        # Plot projected forward/back positions
-        if (self.ticks % 50 == 0):
-            self.controller.plot_trajectory(
-                pose, self.trajectory, current_speed
-            )
-            distance_msg = Float64(self.trajectory.get_distance_from_index(
-                self.controller.current_traj_index))
-            self.distance_publisher.publish(distance_msg)
 
+    def planner_tick(self):
+        with self.lock:
+            other_pose, other_speed = self.get_world_pose_and_speed(self.other_odom_msg)
+        # update local trajectory via path planner
+        self.cur_traj = self.path_planner.compute_traj(
+                                            other_pose,
+                                            other_speed)
 if __name__ == "__main__":
     rospy.init_node("auton_system")
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--controller", 
+        type=str,
+        help="set controller type",
+        required=True)
+
+    parser.add_argument(
+        "--dist", 
+        type=float,
+        help="start buggy at meters distance along the path",
+        required=True)
+
+    parser.add_argument(
+        "--traj", 
+        type=str, 
+        help="path to the trajectory file, relative to /rb_ws/src/buggy/paths/", 
+        required=True)
+
+    parser.add_argument(
+        "--self_name",
+        type=str,
+        help="name of ego-buggy", 
+        required=True)
+
+    parser.add_argument(
+        "--other_name", 
+        type=str,
+        help="name of other buggy, if left unspecified, the autonsystem assumes it is the only buggy on the course",         
+        required=False)
+
+    parser.add_argument(
+        "--profile",
+        action='store_true',
+        help="turn on profiling for the path planner")
+
+    args, _ = parser.parse_known_args()
+    ctrl = args.controller
+    start_dist = args.dist
+    traj = args.traj
+    self_name = args.self_name
+    other_name = args.other_name
+    profile = args.profile
 
-    arg_ctrl = sys.argv[1]
-    arg_start_dist = sys.argv[2]
-    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(ctrl) + "\n\n")
+    print("\n\nUsing path: /rb_ws/src/buggy/paths/" + str(traj) + "\n\n")
+    print("\n\nStarting at distance: " + str(start_dist) + "\n\n")
 
-    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(json_filepath="/rb_ws/src/buggy/paths/" + traj)
 
-    trajectory = Trajectory("/rb_ws/src/buggy/paths/" + arg_path)
     # calculate starting index
     start_index = trajectory.get_index_from_distance(start_dist)
 
     # Add Controllers Here
-    ctrller = None
-    if (arg_ctrl == "stanley"):
-        ctrller = StanleyController(buggy_name, start_index)
-    elif (arg_ctrl == "pure_pursuit"):
-        ctrller = PurePursuitController(buggy_name, start_index)
-    elif (arg_ctrl == "mpc"):
-        ctrller = ModelPredictiveController(buggy_name, start_index)
-    if (ctrller == None):
+    local_ctrller = None
+    if (ctrl == "stanley"):
+        local_ctrller = StanleyController(
+            self_name, 
+            start_index=start_index)
+
+    elif (ctrl == "pure_pursuit"):
+        local_ctrller = PurePursuitController(
+            self_name, 
+            start_index=start_index)
+
+    elif (ctrl == "mpc"):
+        local_ctrller = ModelPredictiveController(
+            self_name, 
+            start_index=start_index)
+
+    if (local_ctrller == None):
         raise Exception("Invalid Controller Argument")
 
     auton_system = AutonSystem(
         trajectory,
-        ctrller,
+        local_ctrller,
         BrakeController(),
-        buggy_name,
-        is_sim
+        self_name,
+        other_name,
+        profile
     )
+
     while not rospy.is_shutdown():
         rospy.spin()