added Trajectory Msg and finished off path planner

rb_ws/src/buggy/CMakeLists.txt

@@ -23,7 +23,7 @@ install(DIRECTORY
 )
 
 # Install Python modules
-ament_python_install_package(${PROJECT_NAME})
+# ament_python_install_package(${PROJECT_NAME})
 
 # Install Python executables
 install(PROGRAMS
@@ -37,4 +37,12 @@ install(PROGRAMS
   DESTINATION lib/${PROJECT_NAME}
 )
 
+
+find_package(rosidl_default_generators REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/TrajectoryMsg.msg"
+)
+ament_export_dependencies(rosidl_default_runtime)
+
 ament_package()

rb_ws/src/buggy/launch/sim_2d_double.xml

@@ -1,30 +1,31 @@
 <launch>
 
     <node pkg="foxglove_bridge" exec="foxglove_bridge" name="foxglove" namespace="SC"/>
-
-    <node pkg="buggy" exec="engine.py" name="SC_sim_single" namespace="SC">
-        <param name="velocity" value="12"/>
-        <param name="pose" value="Hill1_SC"/>
-    </node>
-    <node pkg="buggy" exec="buggy_state_converter.py" name="SC_state_converter" namespace="SC"/>
-    <node pkg="buggy" exec="controller_node.py" name="SC_controller" namespace="SC">
+
+    <node pkg="buggy" exec="buggy_state_converter.py" name="NAND_state_converter" namespace="NAND"/>
+    <node pkg="buggy" exec="controller_node.py" name="NAND_controller" namespace="NAND">
         <param name="dist" value="0.0"/>
         <param name="traj_name" value="buggycourse_safe.json"/>
         <param name="controller" value="stanley"/>
     </node>
-    <!-- <node pkg="buggy" exec="path_planner" name="SC_path_planner" namespace="SC">
-        <param name="traj_name" value="buggycourse_safe.json"/>
-    </node> -->
-
-    <node pkg="buggy" exec="buggy_state_converter.py" name="NAND_state_converter" namespace="NAND"/>
     <node pkg="buggy" exec="engine.py" name="NAND_sim_single" namespace="NAND">
-        <param name="velocity" value="10"/>
+        <param name="velocity" value="8"/>
         <param name="pose" value="Hill1_NAND"/>
     </node>
-    <node pkg="buggy" exec="controller_node.py" name="NAND_controller" namespace="NAND">
+
+    <node pkg="buggy" exec="buggy_state_converter.py" name="SC_state_converter" namespace="SC"/>
+    <node pkg="buggy" exec="controller_node.py" name="SC_controller" namespace="SC">
         <param name="dist" value="0.0"/>
         <param name="traj_name" value="buggycourse_safe.json"/>
         <param name="controller" value="stanley"/>
     </node>
+    <node pkg="buggy" exec="engine.py" name="SC_sim_single" namespace="SC">
+        <param name="velocity" value="12"/>
+        <param name="pose" value="Hill1_SC"/>
+    </node>
+    <node pkg="buggy" exec="path_planner.py" name="SC_path_planner" namespace="SC" output="screen">
+        <param name="traj_name" value="buggycourse_safe.json"/>
+        <remap from="other/state" to="/NAND/self/state"/>
+    </node>
 
 </launch>

rb_ws/src/buggy/msg/TrajectoryMsg.msg

@@ -0,0 +1,4 @@
+float64 time
+float64 cur_idx
+float64[] easting
+float64[] northing

rb_ws/src/buggy/package.xml

@@ -10,13 +10,20 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
   <buildtool_depend>ament_cmake_python</buildtool_depend>
 
+
   <depend>rclcpp</depend>
   <depend>rclpy</depend>
-
+
+
   <exec_depend>ros2launch</exec_depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
+
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
 
   <export>
     <build_type>ament_cmake</build_type>

rb_ws/src/buggy/scripts/controller/controller_node.py

@@ -10,6 +10,7 @@
 
 from std_msgs.msg import Float32, Float64, Bool
 from nav_msgs.msg import Odometry
+from buggy.msg import TrajectoryMsg
 
 sys.path.append("/rb_ws/src/buggy/scripts")
 from util.trajectory import Trajectory
@@ -62,7 +63,7 @@ def __init__(self):
 
         # Subscribers
         self.odom_subscriber = self.create_subscription(Odometry, 'self/state', self.odom_listener, 1)
-        self.traj_subscriber = self.create_subscription(Odometry, 'self/cur_traj', self.traj_listener, 1)
+        self.traj_subscriber = self.create_subscription(TrajectoryMsg, 'self/cur_traj', self.traj_listener, 1)
 
         self.lock = threading.Lock()
 

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

@@ -9,7 +9,7 @@
 
 from nav_msgs.msg import Odometry
 from std_msgs.msg import Float64
-from TrajectoryMsg.msg import TrajectoryMsg
+from buggy.msg import TrajectoryMsg
 
 sys.path.append("/rb_ws/src/buggy/scripts")
 from util.pose import Pose
@@ -50,17 +50,18 @@ def __init__(self) -> None:
         self.get_logger().info('INITIALIZED.')
 
 
-        left_curb_filepath = ""
-        left_curb_trajectory = Trajectory(left_curb_filepath)
-
         #Parameters
         self.declare_parameter("traj_name", "buggycourse_safe.json")
         traj_name = self.get_parameter("traj_name").value
         self.nominal_traj = Trajectory(json_filepath="/rb_ws/src/buggy/paths/" + traj_name) #TODO: Fixed filepath, not good
 
-        self.declare_parameter("curb_name", None)
+        self.declare_parameter("curb_name", "")
         curb_name = self.get_parameter("curb_name").value
-        self.left_curb = Trajectory(json_filepath="/rb_ws/src/buggy/paths/" + curb_name) #TODO: Fixed filepath, not good
+        curb_name = None if curb_name == "" else curb_name
+        if curb_name is None:
+            self.left_curb = None
+        else:
+            self.left_curb = Trajectory(json_filepath="/rb_ws/src/buggy/paths/" + curb_name) #TODO: Fixed filepath, not good
 
         #Publishers
         self.other_buggy_xtrack_publisher = self.create_publisher(Float64, "self/debug/other_buggy_xtrack", 10)
@@ -86,6 +87,7 @@ def self_pose_callback(self, msg):
     def other_pose_callback(self, msg):
         with self.other_pose_lock:
             self.other_pose = Pose.rospose_to_pose(msg.pose.pose)
+            self.get_logger().debug("RECEIVED FROM NAND")
 
     def timer_callback(self):
         with self.self_pose_lock and self.other_pose_lock:
@@ -187,7 +189,7 @@ def compute_traj(
         other_cross_track_dist = np.sum(
             nominal_to_other * other_normal, axis=1)
 
-        self.other_buggy_xtrack_publisher.publish(Float64(other_cross_track_dist))
+        self.other_buggy_xtrack_publisher.publish(Float64(data=float(other_cross_track_dist)))
 
         # here, use passing offsets to weight NAND's cross track signed distance:
         # if the sample point is far from SC, the cross track distance doesn't matter
@@ -227,7 +229,7 @@ def compute_traj(
         positions = nominal_slice + (passing_offsets[:, None] * nominal_normals)
 
         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 ))
 
 
 def main(args=None):

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

@@ -4,9 +4,10 @@
 
 import numpy as np
 from scipy.interpolate import Akima1DInterpolator, CubicSpline
+from buggy.msg import TrajectoryMsg
 
 import utm
-
+import time
 
 class Trajectory:
     """A wrapper around a trajectory JSON file that does some under-the-hood math. Will
@@ -347,16 +348,16 @@ def get_closest_index_on_path(
             + start_index
         )
 
-    """ def pack(self, x, y) -> TrajectoryMsg:
+    def pack(self, x, y) -> TrajectoryMsg:
         traj = TrajectoryMsg()
-        traj.easting = self.positions[:, 0]
-        traj.northing = self.positions[:, 1]
+        traj.easting = list(self.positions[:, 0])
+        traj.northing = list(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 """
+        return Trajectory(positions=pos), cur_idx