Added cur_idx to the trajectory message

CMU-Robotics-Club · Jun 27, 2024 · 3cd63fb · 3cd63fb
1 parent 6465ea1
commit 3cd63fb
Show file tree

Hide file tree

Showing 4 changed files with 9 additions and 11 deletions.
diff --git a/rb_ws/src/buggy/msg/TrajectoryMsg.msg b/rb_ws/src/buggy/msg/TrajectoryMsg.msg
@@ -1,3 +1,4 @@
 float64 time
+float64 cur_idx
 float64[] easting
 float64[] northing
diff --git a/rb_ws/src/buggy/scripts/auton/autonsystem.py b/rb_ws/src/buggy/scripts/auton/autonsystem.py
@@ -127,10 +127,8 @@ def update_other_steering_angle(self, msg):
 
     def update_traj(self, msg):
         with self.lock:
-            # print("New Trajectory:")
-            # print(msg.easting)
-            # print(msg.northing)
-            self.cur_traj = Trajectory.unpack(msg)
+            self.cur_traj, self.local_controller.current_traj_index = Trajectory.unpack(msg)
+
 
 
     def init_check(self):
@@ -261,10 +259,7 @@ def planner_tick(self):
             other_pose = self.get_world_pose(self.other_odom_msg)
 
         # update local trajectory via path planner
-        _, cur_idx = self.path_planner.compute_traj(
-                                            self_pose,
-                                            other_pose)
-        self.local_controller.current_traj_index = cur_idx
+        self.path_planner.compute_traj(self_pose, other_pose)
 
 def init_parser ():
     """

diff --git a/rb_ws/src/buggy/scripts/auton/path_planner.py b/rb_ws/src/buggy/scripts/auton/path_planner.py
@@ -205,7 +205,7 @@ def compute_traj(
             self.debug_passing_traj_publisher.publish(reference_navsat)
 
         local_traj = Trajectory(json_filepath=None, positions=positions)
-        self.traj_publisher.publish(local_traj.pack())
+        self.traj_publisher.publish(local_traj.pack(self_pose.x, self_pose.y))
         return local_traj, \
                 local_traj.get_closest_index_on_path(
                 self_pose.x,

diff --git a/rb_ws/src/buggy/scripts/auton/trajectory.py b/rb_ws/src/buggy/scripts/auton/trajectory.py
@@ -350,16 +350,18 @@ def get_closest_index_on_path(
             + start_index
         )
 
-    def pack(self) -> TrajectoryMsg:
+    def pack(self, x, y) -> TrajectoryMsg:
         traj = TrajectoryMsg()
         traj.easting = self.positions[:, 0]
         traj.northing = self.positions[:, 1]
         traj.time = time.time()
+        traj.cur_idx = self.get_closest_index_on_path(x,y)
         return traj
 
     def unpack(trajMsg : TrajectoryMsg):
         pos = np.array([trajMsg.easting, trajMsg.northing]).transpose(1, 0)
-        return Trajectory(positions=pos)
+        cur_idx = trajMsg.cur_idx
+        return Trajectory(positions=pos), cur_idx
 
 
 if __name__ == "__main__":