Merge branch 'sc' of https://github.com/CMU-Robotics-Club/robobuggy-software into sc

Author: cmuroboclub

rb_ws/src/buggy/CMakeLists.txt

@@ -33,8 +33,6 @@ install(PROGRAMS
   scripts/simulator/engine.py
   scripts/watchdog/watchdog_node.py
   scripts/buggy_state_converter.py
-  scripts/serial/ros_to_bnyahaj_reader.py
-  scripts/serial/ros_to_bnyahaj_writer.py
   scripts/serial/ros_to_bnyahaj.py
   scripts/visualization/telematics.py
   scripts/debug/debug_steer.py

rb_ws/src/buggy/config/sc-mockroll.yaml

@@ -0,0 +1,19 @@
+/**: # Global Params
+  ros__parameters:
+      traj_name: "cut_square.json"
+
+SC:
+  bnyahaj:
+    ros__parameters:
+      teensy_name: "ttyUSB0"
+
+  SC_controller:
+    ros__parameters:
+      dist: 0.0
+      # traj_name: "buggycourse_safe.json"
+      controller: "stanley"
+
+  SC_path_planner:
+    ros__parameters:
+      # traj_name: "buggycourse_safe.json"
+      curb_name: ""

rb_ws/src/buggy/config/sc.yaml

@@ -30,5 +30,5 @@ SC:
 SC:
   SC_path_planner:
     ros__parameters:
-      traj_name: "buggycourse_safe.json"
+      # traj_name: "buggycourse_safe.json"
       curb_name: "buggycourse_curb.json"

rb_ws/src/buggy/config/sim_double.yaml

@@ -0,0 +1,7 @@
+<arg name="config_file" default="src/buggy/config/sc-mockroll.yaml"/>
+<node pkg="buggy" exec="controller_node.py" name="SC_controller" output = "screen" namespace="SC">
+        <param from="$(var config_file)"/>
+</node>
+<node pkg="buggy" exec="path_planner.py" name="SC_path_planner" namespace="SC" output = "screen">
+    <param from="$(var config_file)"/>
+</node>

rb_ws/src/buggy/launch/sc-main.xml

@@ -2,9 +2,15 @@
     <include file="$(find-pkg-share microstrain_inertial_driver)/launch/microstrain_launch.py">
         <arg name="params_file" value="$(env RBROOT)/src/buggy/INS_params.yml"/>
     </include>
-    
-    <node name="bnyahaj" pkg="buggy" exec="ros_to_bnyahaj.py" respawn="true" output="screen" args="--self_name SC --other_name NAND --teensy_name ttyUSB0"/>
+    <arg name="config_file" default="src/buggy/config/sc-mockroll.yaml"/>
 
     <node pkg="foxglove_bridge" exec="foxglove_bridge" name="foxglove" output = "screen" namespace="SC"/>
-    <node name="telematics" pkg="buggy" exec="telematics.py" namespace="SC"/>
+    
+    <node name="bnyahaj" pkg="buggy" exec="ros_to_bnyahaj.py" respawn="true" output="screen", namespace="SC">
+        <param from="$(var config_file)"/>
+    </node>
+    <node name="stateconverter" pkg = "buggy" exec = "buggy_state_converter.py" output="screen", namespace="SC"/>
+    
+    <node name="telematics" pkg="buggy" exec="telematics.py" namespace="SC" output="screen"/>
+    <node name="watchdog" pkg="buggy" exec="watchdog_node.py" namespace="SC" output="screen"/>
 </launch>

rb_ws/src/buggy/launch/sc-system.xml

@@ -17,14 +17,19 @@ class Translator(Node):
     be careful of multithreading synchronizaiton issues.
     """
 
-    def __init__(self, teensy_name):
+    def __init__(self):
         """
         teensy_name: required for communication, different for SC and NAND
 
         Initializes the subscribers, rates, and ros topics (including debug topics)
         """
 
         super().__init__('ROS_serial_translator')
+        self.get_logger().info('INITIALIZED.')
+
+        #Parameters
+        self.declare_parameter("teensy_name", "ttyUSB0") #Default is SC's port
+        teensy_name = self.get_parameter("teensy_name").value
 
         self.comms = Comms("/dev/" + teensy_name)
         namespace = self.get_namespace()
@@ -190,39 +195,11 @@ def loop(self):
             self.comms.send_timestamp(time.time_ns() * 1e-6)
 
 
-# Initializes ros nodes, using self and other name
-# other name is not requires, and if not submitted, use NONE
-if __name__ == "__main__":
-    parser = argparse.ArgumentParser()
-    parser.add_argument(
-        "--self_name", type=str, help="name of ego-buggy", required=True
-    )
-    parser.add_argument(
-        "--other_name",
-        type=str,
-        help="name of other buggy",
-        required=False,
-        default=None,
-    )
-    parser.add_argument(
-        "--teensy_name", type=str, help="name of teensy port", required=True
-    )
-    args, _ = parser.parse_known_args()
-    self_name = args.self_name
-    other_name = args.other_name
-    teensy_name = args.teensy_name
-
-    rclpy.init()
-
-    translate = Translator(teensy_name)
-
-    if self_name == "SC" and other_name is None:
-        translate.get_logger().warn(
-            "Not reading NAND Odometry messages, double check roslaunch files for ros_to_bnyahaj"
-        )
-    elif other_name is None:
-        translate.get_logger().info("No other name passed in, presuming that this is NAND ")
+def main(args=None):
+    rclpy.init(args=args)
 
-    rclpy.spin(translate)
+    translator = Translator()
+    rclpy.spin(translator)
 
-    rclpy.shutdown()
+    translator.destroy_node()
+    rclpy.shutdown()