deprecation of std_msg

README_ROBOBUGGY2.md

@@ -137,8 +137,8 @@ Ask Software Lead (WIP)
 - UTM coordinates (assume we're in Zone 17N): `/sim_2d/utm` (geometry_msgs/Pose - position.x = Easting meters , position.y = Northing meters, position.z = heading in degrees from East axis + is CCW)
 - INS Simulation: `/nav/odom` (nsg_msgs/Odometry) (**Noise** is implemented to vary ~1cm)
 Commands:
-- Steering angle: `/buggy/steering` in degrees (std_msgs/Float64)
-- Velocity: `/buggy/velocity` in m/s (std_msgs/Float64)
+- Steering angle: `/buggy/steering` in degrees (example_interfaces/Float64)
+- Velocity: `/buggy/velocity` in m/s (example_interfaces/Float64)
 
 ### Auton Logic
 Ask someone with experience (WIP)

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

@@ -6,7 +6,7 @@
 import rclpy
 from rclpy.node import Node
 
-from std_msgs.msg import Float32, Float64, Bool
+from example_interfaces.msg import Float32, Float64, Bool
 from nav_msgs.msg import Odometry
 
 sys.path.append("/rb_ws/src/buggy/buggy")

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

@@ -6,7 +6,7 @@
 from rclpy.node import Node
 
 from nav_msgs.msg import Odometry
-from std_msgs.msg import Float64
+from example_interfaces.msg import Float64
 from TrajectoryMsg.msg import TrajectoryMsg
 
 sys.path.append("/rb_ws/src/buggy/buggy")
@@ -43,15 +43,31 @@ class PathPlanner(Node):
     # frequency to run in hz
     FREQUENCY = 10
 
-    def __init__(self, nominal_traj:Trajectory, left_curb:Trajectory) -> None:
+    def __init__(self) -> None:
         super().__init__('path_planner')
+        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)
+        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
+
+        #Publishers
         self.other_buggy_xtrack_publisher = self.create_publisher(Float64, "self/debug/other_buggy_xtrack", 10)
         self.traj_publisher = self.create_publisher(TrajectoryMsg, "self/cur_traj", 10)
+
+        #Subscribers
         self.self_pose_subscriber = self.create_subscription(Odometry, 'self/state', self.self_pose_callback, 1)
         self.other_pose_subscriber = self.create_subscription(Odometry, 'other/state', self.other_pose_callback, 1)
 
-        self.nominal_traj = nominal_traj
-        self.left_curb = left_curb
         self.timer = self.create_timer(1/self.FREQUENCY, self.timer_callback)
 
         # set up subscriber and callback for pose variables
@@ -212,18 +228,15 @@ def compute_traj(
         self.traj_publisher.publish(local_traj.pack())
 
 
-def main():
-    rclpy.init(args=None)
+def main(args=None):
+    rclpy.init(args=args)
     # TODO: set file paths here
     # At the time of writing the below snippet, config management is TBD
 
-    traj_filepath = ""
-    left_curb_filepath = ""
-    ref_trajectory = Trajectory(traj_filepath)
-    left_curb_trajectory = Trajectory(left_curb_filepath)
-
-    path_planner = PathPlanner(ref_trajectory, left_curb_trajectory)
+    path_planner = PathPlanner()
     rclpy.spin(path_planner)
+
+    path_planner.destroy_node()
     rclpy.shutdown()
 
 if __name__ == '__main__':

rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py

@@ -7,7 +7,7 @@
 from scipy.spatial.transform import Rotation
 import utm
 
-from std_msgs.msg import Float64, Int8, UInt8, Bool
+from example_interfaces.msg import Float64, Int8, UInt8, Bool
 
 from host_comm import *
 

rb_ws/src/buggy/buggy/simulator/engine.py

@@ -4,7 +4,7 @@
 import rclpy
 from rclpy.node import Node
 from geometry_msgs.msg import Pose, Twist, PoseWithCovariance, TwistWithCovariance
-from std_msgs.msg import Float64
+from example_interfaces.msg import Float64
 from sensor_msgs.msg import NavSatFix
 from nav_msgs.msg import Odometry
 import numpy as np

rb_ws/src/buggy/buggy/watchdog/watchdog_node.py

@@ -1,7 +1,7 @@
 import rclpy
 from rclpy.node import Node
 
-from std_msgs.msg import Bool
+from example_interfaces.msg import Bool
 
 class Watchdog(Node):
 

rb_ws/src/buggy/launch/sim_2d_double.xml

@@ -0,0 +1,25 @@
+<launch>
+
+    <node pkg="foxglove_bridge" exec="foxglove_bridge" name="foxglove" namespace="SC"/>
+
+    <node pkg="buggy" exec="sim_single" 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" name="SC_state_converter" namespace="SC"/>
+    <node pkg="buggy" exec="controller" 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="path_planner" name="SC_path_planner" namespace="SC">
+        <param name="traj_name" value="buggycourse_safe.json"/>
+    </node>
+
+    <node pkg="buggy" exec="buggy_state_converter" name="NAND_state_converter" namespace="NAND"/>
+    <node pkg="buggy" exec="sim_single" name="NAND_sim_single" namespace="NAND">
+        <param name="velocity" value="12"/>
+        <param name="pose" value="Hill1_NAND"/>
+    </node>
+
+</launch>

rb_ws/src/buggy/setup.py

@@ -27,6 +27,7 @@
             'hello_world = buggy.hello_world:main',
             'sim_single = buggy.simulator.engine:main',
             'controller = buggy.controller.controller_node:main',
+            'path_planner = buggy.path_planner.path_planner:main',
             'buggy_state_converter = buggy.buggy_state_converter:main',
             'watchdog = buggy.watchdog.watchdog_node:main',
         ],