Added the use of a custom trajectory msg

rb_ws/src/buggy/CMakeLists.txt

@@ -54,6 +54,7 @@ add_message_files(
   FILES
   EncoderStatus.msg
   LoRaEvent.msg
+  TrajectoryMsg.msg
 )
 
 ## Generate services in the 'srv' folder

rb_ws/src/buggy/launch/sim_2d_2buggies.launch

@@ -4,8 +4,8 @@
     <arg name="sc_velocity"         default="12.0" />
 
     <arg name="nand_controller"     default="stanley" />
-    <arg name="nand_path"           default="buggycourse_safe_1.json" />
-    <arg name="sc_path"           default="buggycourse_safe_1.json" />
+    <arg name="nand_path"           default="buggycourse_safe.json" />
+    <arg name="sc_path"           default="buggycourse_safe.json" />
     <arg name="nand_start_pos"     default="Hill1_NAND" />
     <arg name="nand_velocity"       default="10.0" />
 
@@ -35,11 +35,11 @@
         args="$(arg nand_start_pos) $(arg nand_velocity) NAND"/>
 
     <!-- NAND is not aware of SC -->
-    <arg name="nand_autonsystem_args" default="--controller $(arg nand_controller) --dist 0.0 --traj $(arg nand_path) --self_name NAND --left_curb curb.json" />
+    <arg name="nand_autonsystem_args" default="--controller $(arg nand_controller) --dist 0.0 --traj $(arg nand_path) --self_name NAND --left_curb buggycourse_curb.json" />
     <node name="nand_auton_system" pkg="buggy" type="autonsystem.py" output="screen"
         args="$(arg nand_autonsystem_args)"/>
 
-    <arg name="sc_autonsystem_args" default="--controller $(arg sc_controller) --dist 0.0 --traj $(arg sc_path) --self_name SC --other_name NAND --left_curb curb.json" />
+    <arg name="sc_autonsystem_args" default="--controller $(arg sc_controller) --dist 0.0 --traj $(arg sc_path) --self_name SC --other_name NAND --left_curb buggycourse_curb.json" />
     <node name="sc_auton_system" pkg="buggy" type="autonsystem.py" output="screen"
         args="$(arg sc_autonsystem_args)"/>
 

rb_ws/src/buggy/launch/sim_2d_single.launch

@@ -1,6 +1,6 @@
 <launch>
     <arg name="start_pos"        default="Hill1_SC" />
-    <arg name="autonsystem_args" default="--controller mpc --dist 0.0 --traj frew_test.json --self_name SC" />
+    <arg name="autonsystem_args" default="--controller stanley --dist 0.0 --traj frew_test.json --self_name SC" />
     <arg name="velocity"         default="15.0" />
     <arg name="buggy_name"       default="SC" />
 

rb_ws/src/buggy/msg/TrajectoryMsg.msg

@@ -0,0 +1,2 @@
+float64[] easting
+float64[] northing

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

@@ -9,6 +9,7 @@
 # ROS Message Imports
 from std_msgs.msg import Float32, Float64, Bool
 from nav_msgs.msg import Odometry
+from buggy.msg import TrajectoryMsg
 
 import numpy as np
 
@@ -69,6 +70,7 @@ def __init__(self,
 
         rospy.Subscriber(self_name + "/nav/odom", Odometry, self.update_self_odom)
         rospy.Subscriber(self_name + "/gnss1/odom", Odometry, self.update_self_odom_backup)
+        rospy.Subscriber(self_name + "/nav/traj", TrajectoryMsg, self.update_traj)
 
         # to report if the filter position has separated (so we need to use the antenna position)
         rospy.Subscriber(self_name + "/debug/filter_gps_seperation_status", Bool, self.update_use_gps)
@@ -122,6 +124,13 @@ def update_other_odom(self, msg):
     def update_other_steering_angle(self, msg):
         with self.lock:
             self.other_steering = msg.data
+
+    def update_traj(self, msg):
+        with self.lock:
+            # print("New Trajectory:")
+            # print(msg.easting)
+            # print(msg.northing)
+            self.cur_traj = Trajectory.unpack(msg)
 
 
     def init_check(self):
@@ -252,7 +261,7 @@ def planner_tick(self):
             other_pose = self.get_world_pose(self.other_odom_msg)
 
         # update local trajectory via path planner
-        self.cur_traj, cur_idx = self.path_planner.compute_traj(
+        _, cur_idx = self.path_planner.compute_traj(
                                             self_pose,
                                             other_pose)
         self.local_controller.current_traj_index = cur_idx

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

@@ -3,6 +3,7 @@
 import rospy
 from sensor_msgs.msg import NavSatFix
 from std_msgs.msg import Float64
+from buggy.msg import TrajectoryMsg
 from pose import Pose
 
 from trajectory import Trajectory
@@ -44,6 +45,8 @@ def __init__(self, nominal_traj:Trajectory, left_curb:Trajectory) -> None:
             "/auton/debug/other_buggy_xtrack", Float64, queue_size=1
         )
 
+        self.traj_publisher = rospy.Publisher("SC/nav/traj", TrajectoryMsg, queue_size=1)
+
         self.nominal_traj = nominal_traj
         self.left_curb = left_curb
 
@@ -202,7 +205,8 @@ 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())
+        print("published???")
         return local_traj, \
                 local_traj.get_closest_index_on_path(
                 self_pose.x,

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

@@ -4,6 +4,7 @@
 import numpy as np
 
 from scipy.interpolate import Akima1DInterpolator, CubicSpline
+from buggy.msg import TrajectoryMsg
 
 from world import World
 
@@ -347,6 +348,16 @@ def get_closest_index_on_path(
             + start_index
         )
 
+    def pack(self):
+        traj = TrajectoryMsg()
+        traj.easting = self.positions[:, 0]
+        traj.northing = self.positions[:, 1]
+        return traj
+
+    def unpack(trajMsg : TrajectoryMsg):
+        pos = np.array([trajMsg.easting, trajMsg.northing])
+        return Trajectory(positions=pos)
+
 
 if __name__ == "__main__":
     # Example usage