Transfered the trajectory passing in autonsystem to be handled by ros…

… messages (#100)

* Added the use of a custom trajectory msg

* Finished trajectory class

* Added typings

* pylint fixes round 1

* pylint

* Added cur_idx to the trajectory message

* pylint :(

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

@@ -1,13 +1,13 @@
 <launch>
     <arg name="sc_controller"     default="stanley" />
     <arg name="sc_start_pos"    default="Hill1_SC" />
-    <arg name="sc_velocity"         default="12.0" />
+    <arg name="sc_velocity"         default="15.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" />
+    <arg name="nand_velocity"       default="7.0" />
 
     <arg name="manual_vel"          default="false" />
     <arg name="auto_vel"            default="false" />
@@ -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" />
     <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" />
     <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 buggycourse_safe.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,4 @@
+float64 time
+float64 cur_idx
+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)
@@ -123,6 +125,11 @@ def update_other_steering_angle(self, msg):
         with self.lock:
             self.other_steering = msg.data
 
+    def update_traj(self, msg):
+        with self.lock:
+            self.cur_traj, self.local_controller.current_traj_index = Trajectory.unpack(msg)
+
+
 
     def init_check(self):
         """
@@ -252,10 +259,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(
-                                            self_pose,
-                                            other_pose)
-        self.local_controller.current_traj_index = cur_idx
+        self.path_planner.compute_traj(self_pose, other_pose)
 
 def init_parser ():
     """

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,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_pose.x, self_pose.y))
         return local_traj, \
                 local_traj.get_closest_index_on_path(
                 self_pose.x,

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

@@ -1,8 +1,11 @@
 import json
+import time
 import uuid
 import matplotlib.pyplot as plt
-import numpy as np
 
+from buggy.msg import TrajectoryMsg
+
+import numpy as np
 from scipy.interpolate import Akima1DInterpolator, CubicSpline
 
 from world import World
@@ -347,6 +350,19 @@ def get_closest_index_on_path(
             + start_index
         )
 
+    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)
+        cur_idx = trajMsg.cur_idx
+        return Trajectory(positions=pos), cur_idx
+
 
 if __name__ == "__main__":
     # Example usage