Update course publisher

scripts/debug_course_publisher.py

@@ -11,24 +11,26 @@
 """
 The purpose of this file is for testing courses in the sim 
 without needing the auton GUI. You can add waypoints with
-and without fiducials and these will get published to nav
+and without tags and these will get published to nav
 """
 
 
 if __name__ == "__main__":
     rospy.init_node("debug_course_publisher")
 
     waypoints = [
-        # ("course1", -2, -6, 0),
-        # ("course1", -3, -3, -1),
-        # ("course2", -5, -5, 0)
         (
-            Waypoint(fiducial_id=0, tf_id="course0", type=WaypointType(val=WaypointType.NO_SEARCH)),
-            # SE3(position=np.array([-844.37, 10351.56, 0])),
-            SE3(position=np.array([-8, -8, 0])),
+            Waypoint(tag_id=2, tf_id="course0", type=WaypointType(val=WaypointType.NO_SEARCH)),
+            SE3(position=np.array([4, 4, 0])),
         ),
-        # (Waypoint(fiducial_id=0, tf_id="course1"), SE3(position=np.array([-3, -3, -1]))),
-        # (Waypoint(fiducial_id=0, tf_id="course2"), SE3(position=np.array([-5, -5, 0]))),
+        (
+            Waypoint(tag_id=0, tf_id="course1", type=WaypointType(val=WaypointType.POST)),
+            SE3(position=np.array([-2, -2, 0])),
+        ),
+        (
+            Waypoint(tag_id=1, tf_id="course2", type=WaypointType(val=WaypointType.POST)),
+            SE3(position=np.array([11, -10, 0])),
+        )
     ]
 
     publish_waypoints([convert_waypoint_to_gps(waypoint) for waypoint in waypoints])