Short Circuit - Files Changed (#21)

Co-authored-by: Evan <evan@brucemcrooster.dev>
Co-authored-by: monkeymango55 <90772792+sophizhao@users.noreply.github.com>
Co-authored-by: TiaSinghania <samanvita.singhania@gmail.com>
Co-authored-by: Saransh Agrawal <saransh323@gmail.com>
Co-authored-by: ShortCircuit <officers@roboclub.org>
Co-authored-by: TiaSinghania <TiaSinghania@users.noreply.github.com>
Co-authored-by: Gyanepsaa Singh <GyanepsaaS@users.noreply.github.com>
Co-authored-by: Tucker Shea <tucker@tuckershea.com>
Co-authored-by: Alden-G878 <Alden-G878@users.noreply.github.com>
Co-authored-by: garrison2 <garrison2@users.noreply.github.com>
Co-authored-by: delaynie <thetaspirit@users.noreply.github.com>

.gitignore

@@ -8,8 +8,11 @@ docker-compose.yml~
 rb_ws/bags
 rb_ws/src/buggy/bags/                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                   
 *.bag
-rb_ws/rosbag2/
+rb_ws/rosbag2*/
 
 # VISION
 .vision/
 vision/data/
+
+#VENV
+.sc/

python-requirements.txt

@@ -1,7 +1,7 @@
 matplotlib
 NavPy
 numba
-numpy
+numpy<2
 osqp
 pandas
 pymap3d

rb_ws/environments/sc_env.bash

@@ -0,0 +1,4 @@
+#!/bin/sh
+export RBROOT=/home/nuc/robobuggy-software/rb_ws
+export PYTHONPATH=$PYTHONPATH:$RBROOT/src/buggy/scripts
+export TRAJPATH=$RBROOT/src/buggy/paths/

rb_ws/src/buggy/CMakeLists.txt

@@ -34,6 +34,8 @@ install(PROGRAMS
   scripts/watchdog/watchdog_node.py
   scripts/buggy_state_converter.py
   scripts/serial/ros_to_bnyahaj.py
+  scripts/telemetry/telematics.py
+  scripts/debug/debug_steer.py
   DESTINATION lib/${PROJECT_NAME}
 )
 
@@ -42,6 +44,10 @@ find_package(rosidl_default_generators REQUIRED)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/TrajectoryMsg.msg"
+  "msg/SCDebugInfoMsg.msg"
+  "msg/SCSensorMsg.msg"
+  "msg/NANDRawGPSMsg.msg"
+  "msg/NANDDebugInfoMsg.msg"
 )
 ament_export_dependencies(rosidl_default_runtime)
 

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

@@ -0,0 +1,19 @@
+/**: # Global Params
+  ros__parameters:
+      traj_name: "UC_to_purnell.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/sim_double.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/launch/debug_steer.xml

@@ -0,0 +1,5 @@
+<launch>
+    <node pkg="foxglove_bridge" exec="foxglove_bridge" name="foxglove" namespace="SC"/>
+    <node name="debug_steer" pkg="buggy" exec="debug_steer.py" output="screen"/>
+    <node name="bnyahaj" pkg="buggy" exec="ros_to_bnyahaj.py" output="screen" respawn="true"  args="--self_name SC --other_name NAND --teensy_name ttyUSB0" namespace="SC"/>
+</launch>

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

@@ -0,0 +1,9 @@
+<launch>
+    <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>
+</launch>

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

@@ -0,0 +1,16 @@
+<launch>
+    <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>
+    <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="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/msg/NANDDebugInfoMsg.msg

@@ -0,0 +1,12 @@
+float64 heading_rate
+float64 encoder_angle
+float64 rc_steering_angle
+float64 software_steering_angle
+float64 true_steering_angle
+int32 rfm69_timeout_num
+bool operator_ready
+bool brake_status
+bool auton_steer
+bool tx12_state
+uint8 stepper_alarm
+uint8 rc_uplink_quality

rb_ws/src/buggy/msg/NANDRawGPSMsg.msg

@@ -0,0 +1,6 @@
+float64 easting
+float64 northing
+float64 accuracy
+uint64 gps_time
+int32 gps_seqnum
+uint8 gps_fix

rb_ws/src/buggy/msg/SCDebugInfoMsg.msg

@@ -0,0 +1,10 @@
+float64 encoder_angle
+float64 rc_steering_angle
+float64 software_steering_angle
+float64 true_steering_angle
+bool operator_ready
+bool brake_status
+bool use_auton_steer
+bool tx12_state
+uint8 stepper_alarm
+uint8 rc_uplink_qual

rb_ws/src/buggy/msg/SCSensorMsg.msg

@@ -0,0 +1,2 @@
+float64 velocity
+float32 steering_angle

rb_ws/src/buggy/scripts/buggy_state_converter.py

@@ -15,24 +15,24 @@ def __init__(self):
         namespace = self.get_namespace()
         if namespace == "/SC":
             self.SC_raw_state_subscriber = self.create_subscription(
-                Odometry, "/raw_state", self.convert_SC_state_callback, 10
+                Odometry, "/ekf/odometry_earth", self.convert_SC_state_callback, 10
             )
 
             self.NAND_other_raw_state_subscriber = self.create_subscription(
-                Odometry, "/NAND_raw_state", self.convert_NAND_other_state_callback, 10
+                Odometry, "NAND_raw_state", self.convert_NAND_other_state_callback, 10
             )
 
-            self.other_state_publisher = self.create_publisher(Odometry, "/other/state", 10)
+            self.other_state_publisher = self.create_publisher(Odometry, "other/state", 10)
 
         elif namespace == "/NAND":
             self.NAND_raw_state_subscriber = self.create_subscription(
-                Odometry, "/raw_state", self.convert_NAND_state_callback, 10
+                Odometry, "raw_state", self.convert_NAND_state_callback, 10
             )
 
         else:
             self.get_logger().warn(f"Namespace not recognized for buggy state conversion: {namespace}")
 
-        self.self_state_publisher = self.create_publisher(Odometry, "/state", 10)
+        self.self_state_publisher = self.create_publisher(Odometry, "self/state", 10)
 
         # Initialize pyproj Transformer for ECEF -> UTM conversion for /SC
         self.ecef_to_utm_transformer = pyproj.Transformer.from_crs(
@@ -49,7 +49,6 @@ def convert_NAND_state_callback(self, msg):
         converted_msg = self.convert_NAND_state(msg)
         self.self_state_publisher.publish(converted_msg)
 
-
     def convert_NAND_other_state_callback(self, msg):
         """ Callback for processing SC/NAND_raw_state messages and publishing to other/state """
         converted_msg = self.convert_NAND_other_state(msg)
@@ -95,15 +94,9 @@ def convert_SC_state(self, msg):
         converted_msg.pose.covariance = msg.pose.covariance
         converted_msg.twist.covariance = msg.twist.covariance
 
-        # ---- 4. Copy Linear Velocities ----
-        converted_msg.twist.twist.linear.x = msg.twist.twist.linear.x   # m/s in x-direction
-        converted_msg.twist.twist.linear.y = msg.twist.twist.linear.y   # m/s in x-direction
-        converted_msg.twist.twist.linear.z = msg.twist.twist.linear.z   # keep original Z velocity
 
-        # ---- 5. Copy Angular Velocities ----
-        converted_msg.twist.twist.angular.x = msg.twist.twist.angular.x   # copying over
-        converted_msg.twist.twist.angular.y = msg.twist.twist.angular.y   # copying over
-        converted_msg.twist.twist.angular.z = msg.twist.twist.angular.z   # rad/s, heading change rate
+        # ---- 4. Copy Linear/Angular Velocities (Unchanged) ----
+        converted_msg.twist.twist = msg.twist.twist
 
         return converted_msg
 
@@ -151,37 +144,9 @@ def convert_NAND_state(self, msg):
     def convert_NAND_other_state(self, msg):
         """ Converts other/raw_state in SC namespace (NAND data) to clean state units and structure """
         converted_msg = Odometry()
-        converted_msg.header = msg.header
-
-        # ---- 1. Directly use UTM Coordinates ----
-        converted_msg.pose.pose.position.x = msg.x    # UTM Easting
-        converted_msg.pose.pose.position.y = msg.y    # UTM Northing
-        converted_msg.pose.pose.position.z = msg.z    # UTM Altitude (not provided in other/raw_state, defaults to 0.0)
-
-        # ---- 2. Orientation in Radians ----
-        converted_msg.pose.pose.orientation.x = msg.roll      # (roll not provided in other/raw_state, defaults to 0.0)
-        converted_msg.pose.pose.orientation.y = msg.pitch     # (pitch not provided in other/raw_state, defaults to 0.0)
-        converted_msg.pose.pose.orientation.z = msg.heading   # heading in radians
-        converted_msg.pose.pose.orientation.w = 0.0           # fourth quaternion term irrelevant for euler angles
-
-        # ---- 3. Copy Covariances (Unchanged) ----
-        converted_msg.pose.covariance = msg.pose_covariance     # (not provided in other/raw_state)
-        converted_msg.twist.covariance = msg.twist_covariance   # (not provided in other/raw_state)
-
-        # ---- 4. Linear Velocities in m/s ----
-        # Convert scalar speed to velocity x/y components using heading (msg.heading)
-        speed = msg.speed       # m/s scalar velocity
-        heading = msg.heading   # heading in radians
-
-        # Calculate velocity components
-        converted_msg.twist.twist.linear.x = speed * np.cos(heading)    # m/s in x-direction
-        converted_msg.twist.twist.linear.y = speed * np.sin(heading)    # m/s in y-direction
-        converted_msg.twist.twist.linear.z = 0.0
 
-        # ---- 5. Angular Velocities ----
-        converted_msg.twist.twist.angular.x = msg.roll_rate      # (roll rate not provided in other/raw_state, defaults to 0.0)
-        converted_msg.twist.twist.angular.y = msg.pitch_rate     # (pitch rate not provided in other/raw_state, defaults to 0.0)
-        converted_msg.twist.twist.angular.z = msg.heading_rate   # rad/s, heading change rate
+        #No actual changes as the other state is just easting northing, everything else is zeroed
+        converted_msg = msg
 
         return converted_msg
 

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

@@ -100,8 +100,8 @@ def init_check(self):
             self.get_logger().warn("WARNING: no available position estimate")
             return False
 
-        elif (self.odom.pose.covariance[0] ** 2 + self.odom.pose.covariance[7] ** 2 > 1**2):
-            self.get_logger().warn("checking position estimate certainty")
+        elif (self.odom.pose.covariance[0] ** 2 + self.odom.pose.covariance[7] ** 2 > 1):
+            self.get_logger().warn("checking position estimate certainty | current covariance: " + str(self.odom.pose.covariance[0] ** 2 + self.odom.pose.covariance[7] ** 2 ))
             return False
 
         #Originally under a lock, doesn't seem necessary?

rb_ws/src/buggy/scripts/debug/debug_steer.py

@@ -0,0 +1,66 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import Float64
+import numpy as np
+
+
+"""
+Debug Controller
+Sends oscillating steering command for firmware and system level debug
+"""
+class DebugController(Node):
+
+    """
+    @input: self_name, for namespace for current buggy
+    Initializes steer publisher to publish steering angles
+    Tick = 1ms
+    """
+    def __init__(self) -> None:
+        super().__init__("debug_steer")
+        self.steer_publisher = self.create_publisher(
+            Float64, "/input/steering", 10)
+        self.rate = 1000  # Hz
+        self.tick_count = 0
+        self.steer_cmd = 0.0
+
+        # Create a timer to call the loop function
+        self.timer = self.create_timer(1.0 / self.rate, self.loop)
+
+    # Outputs a continuous sine wave ranging from -50 to 50, with a period of 500 ticks
+    def sin_steer(self, tick_count):
+        return 50 * np.sin((2 * np.pi) * tick_count/500)
+
+    #returns a constant steering angle of 42 degrees
+    def constant_steer(self, _):
+        return 42.0
+
+    #Creates a loop based on tick counter
+    def loop(self):
+        self.steer_cmd = self.sin_steer(self.tick_count)
+        msg = Float64()
+        msg.data = self.steer_cmd
+        # if self.tick_count % 10 == 0:
+            # self.get_logger().info(f"SIN STEER: {self.steer_cmd}")
+        self.steer_publisher.publish(msg)
+
+        self.tick_count += 1
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    debug_steer = DebugController()
+
+    rclpy.spin(debug_steer)
+
+    # Destroy the node explicitly
+    # (optional - otherwise it will be done automatically
+    # when the garbage collector destroys the node object)
+    debug_steer.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

rb_ws/src/buggy/scripts/serial/host_comm.py

@@ -165,7 +165,7 @@ class ChecksumMismatch(Exception):
 
 class Comms:
     def __init__(self, path_to_port):
-        self.port = Serial(path_to_port, 1_000_000)
+        self.port = Serial(path_to_port, 1_000_000, exclusive=True)
         self.rx_buffer = b''
 
     def send_packet_raw(self, msg_type: bytes, payload: bytes):
@@ -263,23 +263,23 @@ def read_packet(self):
 
         msg_type, payload = packet
         if msg_type == MSG_TYPE_NAND_DEBUG:
-            data = struct.unpack('<ddIfffI????BB', payload)
+            data = struct.unpack('<ddIfffI????BBxxxx', payload)
             return NANDDebugInfo(*data)
 
         elif msg_type == MSG_TYPE_NAND_UKF:
-            data = struct.unpack('<dddddI', payload)
+            data = struct.unpack('<dddddIxxxx', payload)
             return NANDUKF(*data)
 
         elif msg_type == MSG_TYPE_NAND_GPS:
-            data = struct.unpack('<dddQIIB', payload)
+            data = struct.unpack('<dddQIIBxxxxxxx', payload)
             return NANDRawGPS(*data)
 
         elif msg_type == MSG_TYPE_RADIO:
-            data = struct.unpack('<ffIB', payload)
+            data = struct.unpack('<ffIBxxx', payload)
             return Radio(*data)
 
         elif msg_type == MSG_TYPE_SC_DEBUG:
-            data = struct.unpack('<dfffII??B??B', payload)
+            data = struct.unpack('<dfffII??B??Bxxxxxx', payload)
             return SCDebugInfo(*data)
 
         elif msg_type == MSG_TYPE_SC_SENSORS:
@@ -288,7 +288,7 @@ def read_packet(self):
 
         elif msg_type == MSG_TYPE_ROUNDTRIP_TIMESTAMP:
             time = struct.unpack('<d', payload)
-            return RoundtripTimestamp(time)
+            return RoundtripTimestamp(*time)
         else:
             print(f'Unknown packet type {msg_type}')
             return None