Debugging with Jack and discussion of new plans

CMU-Robotics-Club · Mar 1, 2024 · 715d1c0 · 715d1c0
1 parent ce885aa
commit 715d1c0
Show file tree

Hide file tree

Showing 5 changed files with 26 additions and 4 deletions.
diff --git a/rb_ws/src/buggy/scripts/auton/autonsystem.py b/rb_ws/src/buggy/scripts/auton/autonsystem.py
@@ -196,9 +196,10 @@ def planner_tick(self, curr_pose):
         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.local_controller.current_traj_index = self.path_planner.compute_traj(
+        self.cur_traj, spliceIndex = self.path_planner.compute_traj(
                                             other_pose,
                                             other_speed, self.local_controller.current_glob_index, curr_pose)
+        self.local_controller.setCurrIndex(spliceIndex)
 if __name__ == "__main__":
     rospy.init_node("auton_system")
     parser = argparse.ArgumentParser()
@@ -251,23 +252,27 @@ def planner_tick(self, curr_pose):
     trajectory = Trajectory(json_filepath="/rb_ws/src/buggy/paths/" + traj)
 
     # calculate starting index
+    #FIXME: Don't ratchet from the start index, ratchet from using get closest index on path
     start_index = trajectory.get_index_from_distance(start_dist)
 
     # Add Controllers Here
     local_ctrller = None
     if (ctrl == "stanley"):
         local_ctrller = StanleyController(
             self_name,
+            global_traj=trajectory,
             start_index=start_index)
 
     elif (ctrl == "pure_pursuit"):
         local_ctrller = PurePursuitController(
             self_name,
+            global_traj=trajectory,
             start_index=start_index)
 
     elif (ctrl == "mpc"):
         local_ctrller = ModelPredictiveController(
             self_name,
+            global_traj=trajectory,
             start_index=start_index)
 
     if (local_ctrller == None):

diff --git a/rb_ws/src/buggy/scripts/auton/controller.py b/rb_ws/src/buggy/scripts/auton/controller.py
@@ -108,10 +108,23 @@ def updateTrajectoryIndexes(self, current_pose : Pose, local_trajectory : Trajec
             current_pose.x,
             current_pose.y,
             start_index=self.current_glob_index,
-            start_index=self.current_glob_index + 2, #FIXME: Should not use arbitrary indexes of search environment
+            end_index=self.current_glob_index + 5, #FIXME: Should not use arbitrary indexes of search environment
             subsample_resolution=1000,
         )
+
+        debug_index = new_index = local_trajectory.get_closest_index_on_path(
+                current_pose.x, #X Position
+                current_pose.y, #Y Position
+                start_index=0
+        )
+        if (self.current_traj_index < debug_index): print(self.current_traj_index, debug_index)
+
         self.current_glob_index = max(glob_index, self.current_glob_index)
 
+    def setCurrIndex(self, newCurrIndex):
+        self.current_traj_index = newCurrIndex
+
+
+
 
 
diff --git a/rb_ws/src/buggy/scripts/auton/model_predictive_controller.py b/rb_ws/src/buggy/scripts/auton/model_predictive_controller.py
@@ -378,7 +378,7 @@ def compute_trajectory(self, current_pose: Pose, trajectory: Trajectory, current
 
         #Finding index on local path
         #Do the same on global trajectory
-        super.updateTrajectoryIndexes(current_pose, trajectory)
+        self.updateTrajectoryIndexes(current_pose, trajectory)
 
 
         traj_distance = trajectory.get_distance_from_index(self.current_traj_index)

diff --git a/rb_ws/src/buggy/scripts/auton/path_planner.py b/rb_ws/src/buggy/scripts/auton/path_planner.py
@@ -145,6 +145,10 @@ def compute_traj(
         self.debug_splice_pt_publisher.publish(reference_navsat)
 
         new_traj = Trajectory(json_filepath=None, positions=new_path)
+
+        #TODO: RESAMPLE NEW TRAJECTORY at sliced indexes start one behind and go one ahead
+        #Assumption: smallish resample that does not mess up indexing so much that search range in controller can't compensate
+
         #Modifying index
         new_index = curr_index
         new_traj_end_index = len(passing_targets) + new_segment_start_idx

diff --git a/rb_ws/src/buggy/scripts/auton/stanley_controller.py b/rb_ws/src/buggy/scripts/auton/stanley_controller.py
@@ -58,7 +58,7 @@ def compute_control(
         front_x = x + StanleyController.WHEELBASE * np.cos(heading)
         front_y = y + StanleyController.WHEELBASE * np.sin(heading)
 
-        super.updateTrajectoryIndexes(Pose(front_x, front_y, heading), trajectory)
+        self.updateTrajectoryIndexes(Pose(front_x, front_y, heading), trajectory)
 
         # Calculate heading error