46-feat-autonomous-sys-status-node

.devcontainer/devcontainer.json

@@ -28,7 +28,8 @@
 				"redhat.vscode-xml",
 				"twxs.cmake",
 				"VisualStudioExptTeam.intellicode-api-usage-examples",
-				"VisualStudioExptTeam.vscodeintellicode"
+				"VisualStudioExptTeam.vscodeintellicode",
+				"redhat.vscode-yaml"
 			]
 		}
 	}

.gitignore

@@ -6,3 +6,4 @@ log
 .vscode/settings.json
 .gitignore
 .vscode/c_cpp_properties.json
+**/__pycache__/

src/moa/moa_bringup/launch/base.py

@@ -4,13 +4,22 @@
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from ament_index_python import get_package_share_directory
 import os
+from launch.actions.declare_launch_argument import DeclareLaunchArgument
 
 def generate_launch_description():  
     return launch.LaunchDescription([
+        DeclareLaunchArgument(
+        'can_id',
+        default_value='300',
+        description='The frame ID for the CAN messages containing Ackermann commands that are sent to the car'
+        ),
+
         launch_ros.actions.Node(
             package='moa_controllers',
             executable='ack_to_can_node',
-            name='ack_to_can_node'),
+            name='ack_to_can_node',
+            parameters=[{'can_id': launch.substitutions.LaunchConfiguration('can_id')}],
+        ),
 
         # # uncomment when CAN interface is completed
         # launch_ros.actions.Node(
@@ -23,8 +32,15 @@ def generate_launch_description():
                 get_package_share_directory('moa_description'), 'launch'),
                             '/urdf_model.py'])),
 
+        launch_ros.actions.Node(
+            package='moa_controllers',
+            executable='as_status_node',
+            name='as_status_node',
+        ),
+
         launch_ros.actions.Node(
             package='foxglove_bridge',
             executable='foxglove_bridge',
-            name='foxglove_bridge'),
-  ])
+            name='foxglove_bridge'
+        ),
+    ])

src/moa/moa_controllers/moa_controllers/ack_to_can.py

@@ -4,30 +4,26 @@
 import rclpy
 from rclpy.node import Node
 import numpy as np
-# import zlib
 from math import pi
+from rcl_interfaces.msg import ParameterDescriptor
 
 # Ros Imports
 from ackermann_msgs.msg import AckermannDriveStamped
 from moa_msgs.msg import CANStamped
-from std_msgs.msg import Header
-from builtin_interfaces.msg import Time
-
-
-# def compress_floats(values:np.ndarray) -> bytes:    
-#     # Compute the differences between consecutive values
-#     differences = np.diff(values)
-
-#     # Compress the differences using zlib
-#     compressed = zlib.compress(differences, level=9)
-
-#     return compressed
 
 
 class ack_to_can(Node):
     def __init__(self):
         super().__init__('ackermann_to_can') # node name (NB: MoTec listens to this)
 
+        # init ros_arg parameters
+        self.declare_parameter('can_id', 
+                               300, 
+                               ParameterDescriptor(description= 'The frame ID for the CAN messages sent to the car'))
+
+        self.can_id = self.get_parameter('can_id').get_parameter_value().integer_value
+        self.get_logger().info(f'the value of can_id is {self.can_id}')
+
         # create subscriber for ackermann input
         self.subscription = self.create_subscription(
             AckermannDriveStamped,     # msg type
@@ -119,12 +115,6 @@ def ackermann_to_can_parser(self, ack_msg: AckermannDriveStamped) -> Optional[CA
             dtype=np.uint8
             )
 
-        # #compress Ackermann values
-        # compressed_ack_vals = compress_floats(ackermann_vals)
-
-        # #separate compressed ackermann values to list of bytes for CAN
-        # data_packets = [compressed_ack_vals[i:i+1] for i in range(0, len(compressed_ack_vals), 1)]
-
         return ackermann_vals
 
 
@@ -135,7 +125,7 @@ def ack_to_can_publish_callback(self, ack_msg: AckermannDriveStamped):
         can_msg.header.frame_id = 'ackermann_to_can'
 
         # set CAN header/data/id 
-        can_msg.can.id = 25 #TODO need to change to dynamic (between 0-20 accumulator/inverter/brake sensor)
+        can_msg.can.id = self.can_id
         can_msg.can.data = self.ackermann_to_can_parser(ack_msg)
 
         if can_msg.can.data is not None:

src/moa/moa_controllers/moa_controllers/sys_status.py

@@ -0,0 +1,187 @@
+#!/usr/bin/env python3
+# Python imports
+import rclpy
+from rclpy.node import Node
+from rclpy.executors import MultiThreadedExecutor
+from re import search
+from message_filters import ApproximateTimeSynchronizer, Subscriber
+
+# ROS imports
+from ackermann_msgs.msg import AckermannDriveStamped
+from std_msgs.msg import UInt8
+from moa_msgs.msg import HardwareStatesStamped, MissionStatesStamped
+
+
+class as_status(Node):
+    def __init__(self):
+        super().__init__('autonomous_sys_status')
+
+        # init publishers
+        self.status_pub = self.create_publisher(
+            UInt8,
+            'as_status',
+            10,
+        )
+
+        # init subscribers
+        self.ackermann_sub = Subscriber(
+            self,
+            AckermannDriveStamped,
+            'moa/curr_vel',
+        )
+
+        self.mission_status_sub = Subscriber(
+            self,
+            MissionStatesStamped,
+            'mission_status', # TODO change name
+        )
+
+        self.hardware_status_sub = Subscriber(
+            self,
+            HardwareStatesStamped,
+            'moa/hardware_state', # TODO change name
+        )
+
+        # init Time Synchroniser
+        self.time_sync = ApproximateTimeSynchronizer(
+            [self.ackermann_sub, self.mission_status_sub, self.hardware_status_sub],
+            queue_size = 5,
+            slop=0.05,
+        )
+
+        self.time_sync.registerCallback(self.update_state)
+
+        # from MoTec
+        # system states
+        self.hw_states = {
+            'ebs_active': False,
+            'ts_active': False,
+            'in_gear': False,
+            'master_switch_on': False,
+            'asb_ready': False,
+            'brakes_engaged': False,
+            }
+
+        # from moa/curr_vel
+        self.car_stopped = False      
+
+        # from event controller
+        self.mission_finished = False
+        self.mission_selected = False
+
+
+    def check_mission_status(self, msg: MissionStatesStamped):
+        """
+            Records the states of hardware components 
+
+            Args
+            ------------------------------
+            msg: A ROS message of type `MissionStateStamped`, containing the whether mission is selected or finished.
+        """
+
+        msg_data = msg.mission_states
+        self.mission_selected = bool(msg_data.mission_selected)
+        self.mission_finished = bool(msg_data.mission_finished)
+
+
+    def check_hardware(self, msg: HardwareStatesStamped):
+        """
+            Records the states of hardware components 
+
+            Args
+            ------------------------------
+            msg: A ROS message of type `HardwareStatesStamped`, containing the states of hardware components to be parsed.
+        """
+
+        msg_data = msg.hardware_states
+        msg_attrs = [attr for attr in dir(msg_data) 
+                    if not attr.startswith("_") and "serialize" not in attr]
+
+        for attr in msg_attrs:
+            if search("get_*", attr) is None and search("SLOT*", attr) is None:           
+                self.hw_states[attr] = getattr(msg_data, attr)
+
+
+    def check_car_stopped(self, msg: AckermannDriveStamped):
+        """
+            Checks whether car is stationary and records this information
+
+            Args
+            ------------------------------
+            msg: A ROS message of type `AckermannDriveStamped`, containing the Ackermann drive commands to be parsed.
+        """
+
+        if msg.drive.speed == 0.0 and msg.drive.acceleration == 0.0:
+            self.car_stopped = True
+        else:
+            self.car_stopped = False
+
+    def update_state(self, ack_msg: AckermannDriveStamped =None, mission_msg: MissionStatesStamped = None, 
+                     hw_msg: HardwareStatesStamped = None, prod: bool = True):
+        """
+            Updates the state of autonomous system using messages provided by time synchroniser. 
+
+            Args
+            ------------------------------
+            - ack_msg: A ROS message of type `AckermannDriveStamped`, containing the Ackermann drive commands to be parsed.
+            - mission_msg: A ROS message of type `MissionStateStamped`, containing the whether mission is selected or finished.
+            - hw_msg: A ROS message of type `HardwareStatesStamped`, containing the states of hardware components to be parsed.
+            - prod: boolean value signifying whether function is in production or testing mode
+
+            Returns
+            ------------------------------
+            - Integer signifying the state of the Autonomous System.
+
+            Possible AS states:
+            0 : AS finished
+            1 : AS emergency
+            2 : AS ready
+            3 : AS driving
+            4 : AS off
+        """
+        if prod:
+            self.check_car_stopped(ack_msg)
+            self.check_hardware(hw_msg)
+            self.check_mission_status(mission_msg)
+
+        status_code = 4
+        try:
+            if self.hw_states['ebs_active']:
+                if self.mission_finished and self.car_stopped:
+                    status_code = 0
+                else:
+                    raise SystemExit
+            else:
+                if (self.mission_selected and self.hw_states['master_switch_on'] and 
+                        self.hw_states['asb_ready'] and self.hw_states['ts_active']):
+                    if self.hw_states['ts_active'] and self.hw_states['in_gear']:
+                        status_code = 3
+                    else:
+                        if self.hw_states['brakes_engaged']:
+                            status_code = 2       
+        except SystemExit:
+            self.get_logger().error("ERROR: AS in emergency state")
+            status_code = 1
+        finally:
+            self.get_logger().info(f'status code: {status_code}')
+            msg = UInt8(data=status_code)
+            self.status_pub.publish(msg)
+            return status_code
+
+
+def main(args=None):
+    rclpy.init(args=args)    
+    node = as_status()
+    executor = MultiThreadedExecutor(num_threads=2)
+    executor.add_node(node)
+
+    try:
+        executor.spin()
+
+    except SystemExit:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

src/moa/moa_controllers/moa_controllers/sys_status_to_can.py

@@ -0,0 +1,81 @@
+import rclpy
+from rclpy.node import Node
+from rcl_interfaces.msg import ParameterDescriptor
+from typing import Optional
+
+from std_msgs.msg import UInt8
+from moa_msgs.msg import CANStamped
+
+import numpy as np
+
+class sys_status_to_can(Node):
+    def __init__(self):
+        super().__init__("system_status_to_can")
+
+        # init ros_arg parameters
+        self.declare_parameter('can_id', 
+                               300, 
+                               ParameterDescriptor(description= 'The frame ID for the CAN messages sent to the car'))
+
+        self.can_id = self.get_parameter('can_id').get_parameter_value().integer_value
+        self.get_logger().info(f'the value of can_id is {self.can_id}')
+
+        # subscriber for system status
+        self.create_subscription(
+            UInt8,
+            "sys_status",
+            self.callback,
+            10
+        )
+
+        # publisher for can
+        self.canstamped_pub = self.create_publisher(
+            CANStamped,
+            "pub_raw_can",
+            10
+        )
+
+    def uint8_to_can(self, msg: UInt8) -> Optional[CANStamped]:
+        # check validity of data
+        if msg.data < 0 or msg.data > 4:
+            self.get_logger().info(f"system status out of bounds {msg.data}")
+            return None
+
+        # convert data
+        system_status = np.array([
+            msg.data], 
+            dtype=np.uint8
+            )
+
+        return system_status
+
+
+    def callback(self, msg: UInt8):
+        can_msg = CANStamped()
+
+        # configure header
+        can_msg.header.frame_id = 'system_status_to_can'
+
+        # set CAN id 
+        can_msg.can.id = self.can_id
+        data = self.uint8_to_can(msg)
+
+        if data is not None:
+            can_msg.can.data = data
+            # publish CAN to topic
+            self.canstamped_pub.publish(can_msg)
+
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    node = sys_status_to_can()
+    rclpy.spin(node)
+
+    node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()