From 289314809baba930881350d84b3d14220fd0a65d Mon Sep 17 00:00:00 2001 From: Adam Keating Date: Wed, 24 May 2023 14:23:54 +1200 Subject: [PATCH 01/25] branch init --- src/moa/moa_controllers/moa_controllers/sys_status.py | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 src/moa/moa_controllers/moa_controllers/sys_status.py diff --git a/src/moa/moa_controllers/moa_controllers/sys_status.py b/src/moa/moa_controllers/moa_controllers/sys_status.py new file mode 100644 index 00000000..e69de29b From 4b47eea6fd1f2383597d52b60b7f82af83faa1a0 Mon Sep 17 00:00:00 2001 From: Adam Keating Date: Fri, 16 Jun 2023 08:19:30 +0000 Subject: [PATCH 02/25] cleanup previous code --- .../moa_controllers/ack_to_can.py | 18 --------- .../scrutineering/inspection_mission.py | 6 ++- .../scrutineering/scrutineering/test_node.py | 40 ------------------- 3 files changed, 5 insertions(+), 59 deletions(-) delete mode 100644 src/plan_act/scrutineering/scrutineering/test_node.py diff --git a/src/moa/moa_controllers/moa_controllers/ack_to_can.py b/src/moa/moa_controllers/moa_controllers/ack_to_can.py index 47a69a14..d624e2df 100644 --- a/src/moa/moa_controllers/moa_controllers/ack_to_can.py +++ b/src/moa/moa_controllers/moa_controllers/ack_to_can.py @@ -4,7 +4,6 @@ import rclpy from rclpy.node import Node import numpy as np -# import zlib from math import pi # Ros Imports @@ -13,17 +12,6 @@ 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) @@ -119,12 +107,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 diff --git a/src/plan_act/scrutineering/scrutineering/inspection_mission.py b/src/plan_act/scrutineering/scrutineering/inspection_mission.py index ed143c55..a4aff732 100644 --- a/src/plan_act/scrutineering/scrutineering/inspection_mission.py +++ b/src/plan_act/scrutineering/scrutineering/inspection_mission.py @@ -1,12 +1,16 @@ +#!/usr/bin/env python3 +# Python imports import rclpy from rclpy.node import Node from rclpy.duration import Duration from rclpy.executors import SingleThreadedExecutor -from ackermann_msgs.msg import AckermannDriveStamped, AckermannDrive from math import radians as to_rads from math import sin import time +# Ros Imports +from ackermann_msgs.msg import AckermannDriveStamped, AckermannDrive + class inspection_mission_node(Node): def __init__(self): super().__init__('inspection_mission_node') diff --git a/src/plan_act/scrutineering/scrutineering/test_node.py b/src/plan_act/scrutineering/scrutineering/test_node.py deleted file mode 100644 index 0c20d722..00000000 --- a/src/plan_act/scrutineering/scrutineering/test_node.py +++ /dev/null @@ -1,40 +0,0 @@ -import rclpy -from rclpy.node import Node -from time import sleep -from ackermann_msgs.msg import AckermannDriveStamped, AckermannDrive - -class testNode(Node): - def __init__(self): - super().__init__('testNode') - self.sub = self.create_subscription(AckermannDriveStamped, 'cmd_val', self.msg_callback,10) - self.pub = self.create_publisher(AckermannDriveStamped, 'moa/curr_vel',10) - self.received = [] - self.real = False - def msg_callback(self, msg): - self.received.append(msg) - sleep(1) - if self.real: - for i in range(1,5): - rand = random.random() - noise_msg = AckermannDrive(speed = rand, accel = rand) - self.pub.publish(AckermannDriveStamped(drive = noise_msg)) - sleep(0.5) - - self.pub.publish(AckermannDriveStamped(drive = msg.drive)) - self.get_logger().info(f'received msg {msg}') - - -def main(args=None): - rclpy.init(args=args) - - test_node = testNode() - - rclpy.spin(test_node) - - test_node.destroy_node() - - rclpy.shutdown() - - -if __name__ == '__main__': - main() \ No newline at end of file From 9994ce46cd88ea81eb95275068a4274c6307a8a0 Mon Sep 17 00:00:00 2001 From: Adam Keating Date: Fri, 16 Jun 2023 08:53:15 +0000 Subject: [PATCH 03/25] Added hardware state message for receiving data from motec --- src/moa/moa_msgs/CMakeLists.txt | 2 ++ src/moa/moa_msgs/msg/HardwareStates.msg | 5 +++++ src/moa/moa_msgs/msg/HardwareStatesStamped.msg | 3 +++ 3 files changed, 10 insertions(+) create mode 100644 src/moa/moa_msgs/msg/HardwareStates.msg create mode 100644 src/moa/moa_msgs/msg/HardwareStatesStamped.msg diff --git a/src/moa/moa_msgs/CMakeLists.txt b/src/moa/moa_msgs/CMakeLists.txt index ab667214..619a1470 100644 --- a/src/moa/moa_msgs/CMakeLists.txt +++ b/src/moa/moa_msgs/CMakeLists.txt @@ -20,6 +20,8 @@ set(msg_files "msg/ConeStamped.msg" "msg/ConeMap.msg" "msg/ConeMapStamped.msg" + "msg/HardwareStates.msg" + "msg/HardwareStatesStamped.msg" ) set(srv_files "srv/CANSendReq.srv" diff --git a/src/moa/moa_msgs/msg/HardwareStates.msg b/src/moa/moa_msgs/msg/HardwareStates.msg new file mode 100644 index 00000000..67a1d235 --- /dev/null +++ b/src/moa/moa_msgs/msg/HardwareStates.msg @@ -0,0 +1,5 @@ +uint8 ebs_active +uint8 ts_active +uint8 in_gear +uint8 master_switch_on +uint8 brakes_engaged \ No newline at end of file diff --git a/src/moa/moa_msgs/msg/HardwareStatesStamped.msg b/src/moa/moa_msgs/msg/HardwareStatesStamped.msg new file mode 100644 index 00000000..9737ceb4 --- /dev/null +++ b/src/moa/moa_msgs/msg/HardwareStatesStamped.msg @@ -0,0 +1,3 @@ +std_msgs/Header header + +HardwareStates hardware_states \ No newline at end of file From 41d0c0d654d01ede11f083d6ae9633427e615527 Mon Sep 17 00:00:00 2001 From: Adam Keating Date: Fri, 16 Jun 2023 08:54:03 +0000 Subject: [PATCH 04/25] Added status node to moa_bringup launchfiles --- src/moa/moa_bringup/launch/base.py | 5 +++++ src/moa/moa_controllers/setup.py | 3 ++- 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/src/moa/moa_bringup/launch/base.py b/src/moa/moa_bringup/launch/base.py index eff2a426..9a5b099c 100644 --- a/src/moa/moa_bringup/launch/base.py +++ b/src/moa/moa_bringup/launch/base.py @@ -14,6 +14,11 @@ def generate_launch_description(): # executable='can_interface_jnano', # name='can_interface_jnano'), + 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', diff --git a/src/moa/moa_controllers/setup.py b/src/moa/moa_controllers/setup.py index 5820683e..dea0cbec 100644 --- a/src/moa/moa_controllers/setup.py +++ b/src/moa/moa_controllers/setup.py @@ -20,7 +20,8 @@ tests_require=['pytest'], entry_points={ 'console_scripts': [ - 'ack_to_can_node = moa_controllers.ack_to_can:main' + 'ack_to_can_node = moa_controllers.ack_to_can:main', + 'as_status_node = moa_controllers.sys_status:main', ], }, ) From ce4653d9677285e68603f2fd7b0b053e9bab0e9e Mon Sep 17 00:00:00 2001 From: Adam Keating Date: Fri, 16 Jun 2023 09:38:33 +0000 Subject: [PATCH 05/25] - cleanup code - created & setup test file - created as_status node - finished v1 of state updater - setup subscribers/publishers - setup car stopped check --- .../moa_controllers/ack_to_can.py | 3 +- .../moa_controllers/sys_status.py | 140 ++++++++++++++++++ .../moa_controllers/test/test_ackermann.py | 3 +- .../moa_controllers/test/test_sys_status.py | 16 ++ 4 files changed, 158 insertions(+), 4 deletions(-) create mode 100644 src/moa/moa_controllers/test/test_sys_status.py diff --git a/src/moa/moa_controllers/moa_controllers/ack_to_can.py b/src/moa/moa_controllers/moa_controllers/ack_to_can.py index d624e2df..ad1b4cdf 100644 --- a/src/moa/moa_controllers/moa_controllers/ack_to_can.py +++ b/src/moa/moa_controllers/moa_controllers/ack_to_can.py @@ -9,8 +9,7 @@ # 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 + class ack_to_can(Node): def __init__(self): diff --git a/src/moa/moa_controllers/moa_controllers/sys_status.py b/src/moa/moa_controllers/moa_controllers/sys_status.py index e69de29b..d1bfed37 100644 --- a/src/moa/moa_controllers/moa_controllers/sys_status.py +++ b/src/moa/moa_controllers/moa_controllers/sys_status.py @@ -0,0 +1,140 @@ +#!/usr/bin/env python3 +# Python imports +import rclpy +from rclpy.node import Node +from rclpy.callback_groups import ReentrantCallbackGroup + +# ROS imports +from ackermann_msgs.msg import AckermannDriveStamped +from std_msgs.msg import UInt8 +from moa_msgs.msg import HardwareStatesStamped + +class as_status(Node): + def __init__(self): + super().__init__('autonomous_sys_status') + + self.sub_cbgroup = ReentrantCallbackGroup() + + # publishers + self.status_pub = self.create_publisher( + UInt8, + 'as_status', + 10, + ) + + # subscribers + self.ackermann_sub = self.create_subscription( + AckermannDriveStamped, + 'moa/curr_vel', + self.check_car_stopped, + 10, + callback_group = self.sub_cbgroup, + ) + + self.mission_status_sub = self.create_subscription( + UInt8, + '', + self.check_mission_status, + 10, + callback_group = self.sub_cbgroup, + ) + + self.hardware_status_sub = self.create_subscription( + HardwareStatesStamped, + '', + self.check_hardware, + 10, + callback_group = self.sub_cbgroup, + ) + + #timer + self.update_timer = self.create_timer( + 1/50, #time in seconds (currently equiv of 50Hz) + self.update_state, + ) + + # system states + # from motec + self.ebs_state = False + self.ts_active = False + self.in_gear = False + self.master_switch_on = False + self.asb_ready = False + self.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: UInt8): # TODO needs more planning work done + if msg.data == 0: + self.mission_selected = False + self.mission_finished = False + + if msg.data == 1: + self.mission_finished == True + elif msg.data == 2: + self.mission_selected = True + + self.get_logger().info('checked mission status') + + + def check_hardware(self, msg: HardwareStatesStamped): + self.get_logger().info('checked hardware') + + + def check_car_stopped(self, msg: AckermannDriveStamped): + if msg.drive.speed == 0.0 and msg.drive.acceleration == 0.0: + self.car_stopped = True + else: + self.car_stopped = False + + self.get_logger().info('checked car motion') + + + def update_state(self): + ''' + AS states: + 0 : AS finished + 1 : AS emergency + 2 : AS ready + 3 : AS driving + 4 : AS off + ''' + status_code = 4 + try: + if self.ebs_state: + if self.mission_status and self.car_stopped: + status_code = 0 + else: + raise Exception + else: + if self.mission_selected and self.master_switch_on and self.asb_ready and self.ts_active: + if self.ts_active and self.in_gear: + status_code = 3 + else: + if self.brakes_engaged: + status_code = 2 + except: + status_code = 1 + + msg = UInt8(data=status_code) + self.status_pub.publish(msg) + + +def main(args=None): + rclpy.init(args=args) + + node = as_status() + rclpy.spin(node) + + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/moa/moa_controllers/test/test_ackermann.py b/src/moa/moa_controllers/test/test_ackermann.py index af6a9636..0bba7c41 100644 --- a/src/moa/moa_controllers/test/test_ackermann.py +++ b/src/moa/moa_controllers/test/test_ackermann.py @@ -1,7 +1,6 @@ import pytest import numpy as np -from ackermann_msgs.msg import AckermannDrive, AckermannDriveStamped -from moa_msgs.msg import CANStamped +from ackermann_msgs.msg import AckermannDriveStamped from moa_controllers.ack_to_can import ack_to_can from unittest.mock import Mock import rclpy diff --git a/src/moa/moa_controllers/test/test_sys_status.py b/src/moa/moa_controllers/test/test_sys_status.py new file mode 100644 index 00000000..22aaac5f --- /dev/null +++ b/src/moa/moa_controllers/test/test_sys_status.py @@ -0,0 +1,16 @@ +import pytest +import rclpy +from moa_controllers.sys_status import as_status + + +@pytest.fixture(autouse=True, scope='session') +def node(): + rclpy.init(args=None) + test_status_node = as_status() + + yield test_status_node + test_status_node.destroy_node() + rclpy.shutdown() + +def test_status(node: as_status): + pass \ No newline at end of file From ae8b9f523943f2e52244b2128fe67f996829d9c3 Mon Sep 17 00:00:00 2001 From: Adam Keating Date: Wed, 21 Jun 2023 00:32:11 +0000 Subject: [PATCH 06/25] cleanup/add yaml package to devcontainer.json --- .devcontainer/devcontainer.json | 3 ++- src/moa/moa_controllers/package.xml | 3 +-- src/moa/moa_controllers/test/test_ackermann.py | 8 +------- 3 files changed, 4 insertions(+), 10 deletions(-) diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index c9c42c19..c08d898a 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -28,7 +28,8 @@ "redhat.vscode-xml", "twxs.cmake", "VisualStudioExptTeam.intellicode-api-usage-examples", - "VisualStudioExptTeam.vscodeintellicode" + "VisualStudioExptTeam.vscodeintellicode", + "redhat.vscode-yaml" ] } } diff --git a/src/moa/moa_controllers/package.xml b/src/moa/moa_controllers/package.xml index 56a8082c..af40df55 100644 --- a/src/moa/moa_controllers/package.xml +++ b/src/moa/moa_controllers/package.xml @@ -15,8 +15,7 @@ python3-numpy - - + ackermann_msgs moa_msgs diff --git a/src/moa/moa_controllers/test/test_ackermann.py b/src/moa/moa_controllers/test/test_ackermann.py index 0bba7c41..99296e58 100644 --- a/src/moa/moa_controllers/test/test_ackermann.py +++ b/src/moa/moa_controllers/test/test_ackermann.py @@ -7,7 +7,7 @@ -@pytest.fixture(autouse=True, scope='session') +@pytest.fixture(autouse=True, scope='module') def node(): rclpy.init(args=None) test_ack = ack_to_can() @@ -16,12 +16,6 @@ def node(): test_ack.destroy_node() rclpy.shutdown() - -# def test_compress_floats(): -# values = np.array([1.0, 1.5, 2.0, 2.5]) -# expected_result = b'x\x9c+\x01\x00\xf6\xff,\x02\x00\xfa\xff' -# assert compress_floats(values) == expected_result - def test_ackermann_to_can_parser_out_of_bounds(node: ack_to_can): ack_msg = AckermannDriveStamped() From 091157d53da110d096abaa7753adf8e0afd06390 Mon Sep 17 00:00:00 2001 From: Adam Keating Date: Wed, 21 Jun 2023 01:17:16 +0000 Subject: [PATCH 07/25] added attribute to HardwareStates message and updated gitignore --- .gitignore | 1 + src/moa/moa_msgs/msg/HardwareStates.msg | 1 + 2 files changed, 2 insertions(+) diff --git a/.gitignore b/.gitignore index ec12b3c9..5e3e5972 100644 --- a/.gitignore +++ b/.gitignore @@ -6,3 +6,4 @@ log .vscode/settings.json .gitignore .vscode/c_cpp_properties.json +**/__pycache__/ \ No newline at end of file diff --git a/src/moa/moa_msgs/msg/HardwareStates.msg b/src/moa/moa_msgs/msg/HardwareStates.msg index 67a1d235..412b20ff 100644 --- a/src/moa/moa_msgs/msg/HardwareStates.msg +++ b/src/moa/moa_msgs/msg/HardwareStates.msg @@ -2,4 +2,5 @@ uint8 ebs_active uint8 ts_active uint8 in_gear uint8 master_switch_on +uint8 asb_ready uint8 brakes_engaged \ No newline at end of file From 5a6b0aad408eebf29c6c91ec806edad370a766aa Mon Sep 17 00:00:00 2001 From: Adam Keating Date: Wed, 21 Jun 2023 01:24:27 +0000 Subject: [PATCH 08/25] WIP: changing devices --- .../moa_controllers/sys_status.py | 66 +++++++---- .../moa_controllers/test/test_sys_status.py | 107 +++++++++++++++++- 2 files changed, 146 insertions(+), 27 deletions(-) diff --git a/src/moa/moa_controllers/moa_controllers/sys_status.py b/src/moa/moa_controllers/moa_controllers/sys_status.py index d1bfed37..4c5a1d33 100644 --- a/src/moa/moa_controllers/moa_controllers/sys_status.py +++ b/src/moa/moa_controllers/moa_controllers/sys_status.py @@ -3,6 +3,8 @@ import rclpy from rclpy.node import Node from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.executors import MultiThreadedExecutor +from re import search # ROS imports from ackermann_msgs.msg import AckermannDriveStamped @@ -33,7 +35,7 @@ def __init__(self): self.mission_status_sub = self.create_subscription( UInt8, - '', + 'as_status', # TODO change name self.check_mission_status, 10, callback_group = self.sub_cbgroup, @@ -41,7 +43,7 @@ def __init__(self): self.hardware_status_sub = self.create_subscription( HardwareStatesStamped, - '', + 'moa/hardware_state', # TODO change name self.check_hardware, 10, callback_group = self.sub_cbgroup, @@ -49,18 +51,19 @@ def __init__(self): #timer self.update_timer = self.create_timer( - 1/50, #time in seconds (currently equiv of 50Hz) + 1/50, #time in seconds (currently equiv of 50 times per second) self.update_state, ) # system states - # from motec - self.ebs_state = False - self.ts_active = False - self.in_gear = False - self.master_switch_on = False - self.asb_ready = False - self.brakes_engaged = False + 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 @@ -72,18 +75,27 @@ def __init__(self): def check_mission_status(self, msg: UInt8): # TODO needs more planning work done if msg.data == 0: + self.mission_finished = True + + elif msg.data == 1: self.mission_selected = False self.mission_finished = False - - if msg.data == 1: - self.mission_finished == True + elif msg.data == 2: self.mission_selected = True self.get_logger().info('checked mission status') - def check_hardware(self, msg: HardwareStatesStamped): + def check_hardware(self, msg: HardwareStatesStamped): #TODO + 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) + self.get_logger().info('checked hardware') @@ -107,30 +119,36 @@ def update_state(self): ''' status_code = 4 try: - if self.ebs_state: - if self.mission_status and self.car_stopped: + if self.hw_states['ebs_active']: + if self.mission_finished and self.car_stopped: status_code = 0 else: raise Exception else: - if self.mission_selected and self.master_switch_on and self.asb_ready and self.ts_active: - if self.ts_active and self.in_gear: + 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.brakes_engaged: + if self.hw_states['brakes_engaged']: status_code = 2 - except: + except Exception: + self.get_logger().error("ERROR: AS in emergency state") status_code = 1 - - msg = UInt8(data=status_code) - self.status_pub.publish(msg) + finally: + msg = UInt8(data=status_code) + self.status_pub.publish(msg) + return status_code def main(args=None): rclpy.init(args=args) node = as_status() - rclpy.spin(node) + executor = MultiThreadedExecutor(num_threads=2) + executor.add_node(node) + + executor.spin() node.destroy_node() rclpy.shutdown() diff --git a/src/moa/moa_controllers/test/test_sys_status.py b/src/moa/moa_controllers/test/test_sys_status.py index 22aaac5f..1253fe46 100644 --- a/src/moa/moa_controllers/test/test_sys_status.py +++ b/src/moa/moa_controllers/test/test_sys_status.py @@ -1,16 +1,117 @@ import pytest import rclpy from moa_controllers.sys_status import as_status +from moa_msgs.msg import HardwareStatesStamped +from ackermann_msgs.msg import AckermannDriveStamped +from std_msgs.msg import UInt8 -@pytest.fixture(autouse=True, scope='session') +@pytest.fixture(autouse=True, scope='function') def node(): rclpy.init(args=None) + test_status_node = as_status() + test_status_node.update_timer = None yield test_status_node + test_status_node.destroy_node() rclpy.shutdown() -def test_status(node: as_status): - pass \ No newline at end of file +def test_state_0(node: as_status): + node.hw_states['ebs_active'] = True + node.mission_finished = True + node.car_stopped = True + + assert node.update_state() == 0 + + +def test_state_1(node: as_status): + node.hw_states['ebs_active'] = True + test_cases = [[0,0],[0,1],[1,0]] + + for test in test_cases: + node.mission_finished = test[0] + node.car_stopped = test[1] + + assert node.update_state() == 1 + + +def test_state_2(node: as_status): + node.mission_selected = True + node.hw_states['master_switch_on'] = True + node.hw_states['asb_ready'] = True + node.hw_states['ts_active'] = True + + node.hw_states['brakes_engaged'] = True + + assert node.update_state() == 2 + + +def test_state_3(node: as_status): + node.mission_selected = True + node.hw_states['master_switch_on'] = True + node.hw_states['asb_ready'] = True + node.hw_states['ts_active'] = True + + node.hw_states['in_gear'] = True + + assert node.update_state() == 3 + + +def test_state_4(node: as_status): # TODO to complete + node.mission_selected = True + node.hw_states['master_switch_on'] = True + node.hw_states['asb_ready'] = True + node.hw_states['ts_active'] = True + + assert node.update_state() == 4 + + +def test_check_mission_status(node: as_status): # TODO need to sort out planning/complete + msg = UInt8(data = 0) + node.check_mission_status(msg) + assert node.mission_selected == False and node.mission_finished == True + + msg = UInt8(data = 1) + node.check_mission_status(msg) + assert node.mission_selected == False and node.mission_finished == False + + msg = UInt8(data = 2) + node.check_mission_status(msg) + assert node.mission_selected == True and node.mission_finished == True + + +def test_check_car_stopped(node: as_status): + msg = AckermannDriveStamped() + + msg.drive.speed = 0.0 + msg.drive.acceleration = 0.0 + node.check_car_stopped(msg) + assert node.car_stopped + + msg.drive.speed = 1.0 + msg.drive.acceleration = 1.0 + node.check_car_stopped(msg) + assert not node.car_stopped + + +def test_check_hardware(node: as_status): # TODO complete + msg = HardwareStatesStamped() + msg.hardware_states.ebs_active = 1 + msg.hardware_states.ts_active = 0 + msg.hardware_states.in_gear = 1 + msg.hardware_states.master_switch_on = 1 + msg.hardware_states.brakes_engaged = 1 + + test_case = { + 'ebs_active': 1, + 'ts_active': 0, + 'in_gear': 1, + 'master_switch_on': 1, + 'asb_ready': 0, + 'brakes_engaged': 1, + } + + node.check_hardware(msg) + assert node.hw_states == test_case \ No newline at end of file From 972eea2257123257275a8cbd454a40fd7b9ccb69 Mon Sep 17 00:00:00 2001 From: Adam Keating Date: Wed, 21 Jun 2023 09:54:28 +0000 Subject: [PATCH 09/25] Added mission state custom message/fixed node testing/modified node for time synchronisation of topics --- .../moa_controllers/sys_status.py | 86 +++++++++---------- .../moa_controllers/test/test_sys_status.py | 23 +++-- src/moa/moa_msgs/CMakeLists.txt | 1 + src/moa/moa_msgs/msg/MissionStatesStamped.msg | 5 ++ 4 files changed, 58 insertions(+), 57 deletions(-) create mode 100644 src/moa/moa_msgs/msg/MissionStatesStamped.msg diff --git a/src/moa/moa_controllers/moa_controllers/sys_status.py b/src/moa/moa_controllers/moa_controllers/sys_status.py index 4c5a1d33..d84923ab 100644 --- a/src/moa/moa_controllers/moa_controllers/sys_status.py +++ b/src/moa/moa_controllers/moa_controllers/sys_status.py @@ -2,21 +2,20 @@ # Python imports import rclpy from rclpy.node import Node -from rclpy.callback_groups import ReentrantCallbackGroup 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 +from moa_msgs.msg import HardwareStatesStamped, MissionStatesStamped + class as_status(Node): def __init__(self): super().__init__('autonomous_sys_status') - self.sub_cbgroup = ReentrantCallbackGroup() - # publishers self.status_pub = self.create_publisher( UInt8, @@ -25,36 +24,32 @@ def __init__(self): ) # subscribers - self.ackermann_sub = self.create_subscription( + self.ackermann_sub = Subscriber( + self, AckermannDriveStamped, 'moa/curr_vel', - self.check_car_stopped, - 10, - callback_group = self.sub_cbgroup, ) - self.mission_status_sub = self.create_subscription( - UInt8, - 'as_status', # TODO change name - self.check_mission_status, - 10, - callback_group = self.sub_cbgroup, + self.mission_status_sub = Subscriber( + self, + MissionStatesStamped, + 'mission_status', # TODO change name ) - self.hardware_status_sub = self.create_subscription( + self.hardware_status_sub = Subscriber( + self, HardwareStatesStamped, 'moa/hardware_state', # TODO change name - self.check_hardware, - 10, - callback_group = self.sub_cbgroup, ) - #timer - self.update_timer = self.create_timer( - 1/50, #time in seconds (currently equiv of 50 times per second) - self.update_state, + 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) + # system states self.hw_states = { 'ebs_active': False, @@ -73,19 +68,18 @@ def __init__(self): self.mission_selected = False - def check_mission_status(self, msg: UInt8): # TODO needs more planning work done - if msg.data == 0: - self.mission_finished = True + def check_mission_status(self, msg: MissionStatesStamped): # TODO needs more planning work done + if msg.mission_state == 0: + self.mission_selected = False + self.mission_finished = True - elif msg.data == 1: + elif msg.mission_state == 1: self.mission_selected = False self.mission_finished = False - elif msg.data == 2: + elif msg.mission_state == 2: self.mission_selected = True - - self.get_logger().info('checked mission status') - + self.mission_finished = False def check_hardware(self, msg: HardwareStatesStamped): #TODO msg_data = msg.hardware_states @@ -96,19 +90,14 @@ def check_hardware(self, msg: HardwareStatesStamped): #TODO if search("get_*", attr) is None and search("SLOT*", attr) is None: self.hw_states[attr] = getattr(msg_data, attr) - self.get_logger().info('checked hardware') - def check_car_stopped(self, msg: AckermannDriveStamped): if msg.drive.speed == 0.0 and msg.drive.acceleration == 0.0: self.car_stopped = True else: self.car_stopped = False - - self.get_logger().info('checked car motion') - - def update_state(self): + def update_state(self, ack_msg=None, mission_msg=None, hw_msg=None, prod=True): ''' AS states: 0 : AS finished @@ -117,13 +106,18 @@ def update_state(self): 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 Exception + 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']): @@ -132,26 +126,28 @@ def update_state(self): else: if self.hw_states['brakes_engaged']: status_code = 2 - except Exception: + 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) - + rclpy.init(args=args) node = as_status() executor = MultiThreadedExecutor(num_threads=2) executor.add_node(node) - - executor.spin() - - node.destroy_node() - rclpy.shutdown() + + try: + executor.spin() + + except SystemExit: + node.destroy_node() + rclpy.shutdown() if __name__ == '__main__': diff --git a/src/moa/moa_controllers/test/test_sys_status.py b/src/moa/moa_controllers/test/test_sys_status.py index 1253fe46..9b0248ab 100644 --- a/src/moa/moa_controllers/test/test_sys_status.py +++ b/src/moa/moa_controllers/test/test_sys_status.py @@ -1,9 +1,8 @@ import pytest import rclpy from moa_controllers.sys_status import as_status -from moa_msgs.msg import HardwareStatesStamped +from moa_msgs.msg import HardwareStatesStamped, MissionStatesStamped from ackermann_msgs.msg import AckermannDriveStamped -from std_msgs.msg import UInt8 @pytest.fixture(autouse=True, scope='function') @@ -23,7 +22,7 @@ def test_state_0(node: as_status): node.mission_finished = True node.car_stopped = True - assert node.update_state() == 0 + assert node.update_state(prod=0) == 0 def test_state_1(node: as_status): @@ -34,7 +33,7 @@ def test_state_1(node: as_status): node.mission_finished = test[0] node.car_stopped = test[1] - assert node.update_state() == 1 + assert node.update_state(prod=0) == 1 def test_state_2(node: as_status): @@ -45,7 +44,7 @@ def test_state_2(node: as_status): node.hw_states['brakes_engaged'] = True - assert node.update_state() == 2 + assert node.update_state(prod=0) == 2 def test_state_3(node: as_status): @@ -56,7 +55,7 @@ def test_state_3(node: as_status): node.hw_states['in_gear'] = True - assert node.update_state() == 3 + assert node.update_state(prod=0) == 3 def test_state_4(node: as_status): # TODO to complete @@ -65,21 +64,21 @@ def test_state_4(node: as_status): # TODO to complete node.hw_states['asb_ready'] = True node.hw_states['ts_active'] = True - assert node.update_state() == 4 + assert node.update_state(prod=0) == 4 def test_check_mission_status(node: as_status): # TODO need to sort out planning/complete - msg = UInt8(data = 0) + msg = MissionStatesStamped(mission_state = 0) node.check_mission_status(msg) assert node.mission_selected == False and node.mission_finished == True - msg = UInt8(data = 1) + msg = MissionStatesStamped(mission_state = 1) node.check_mission_status(msg) assert node.mission_selected == False and node.mission_finished == False - msg = UInt8(data = 2) + msg = MissionStatesStamped(mission_state = 2) node.check_mission_status(msg) - assert node.mission_selected == True and node.mission_finished == True + assert node.mission_selected == True and node.mission_finished == False def test_check_car_stopped(node: as_status): @@ -114,4 +113,4 @@ def test_check_hardware(node: as_status): # TODO complete } node.check_hardware(msg) - assert node.hw_states == test_case \ No newline at end of file + assert node.hw_states == test_case \ No newline at end of file diff --git a/src/moa/moa_msgs/CMakeLists.txt b/src/moa/moa_msgs/CMakeLists.txt index 619a1470..32787247 100644 --- a/src/moa/moa_msgs/CMakeLists.txt +++ b/src/moa/moa_msgs/CMakeLists.txt @@ -22,6 +22,7 @@ set(msg_files "msg/ConeMapStamped.msg" "msg/HardwareStates.msg" "msg/HardwareStatesStamped.msg" + "msg/MissionStatesStamped.msg" ) set(srv_files "srv/CANSendReq.srv" diff --git a/src/moa/moa_msgs/msg/MissionStatesStamped.msg b/src/moa/moa_msgs/msg/MissionStatesStamped.msg new file mode 100644 index 00000000..17cb761e --- /dev/null +++ b/src/moa/moa_msgs/msg/MissionStatesStamped.msg @@ -0,0 +1,5 @@ +std_msgs/Header header + +uint8 mission_state + + From 8a1c448f3e60729323f81e7eb1589c521708fc8d Mon Sep 17 00:00:00 2001 From: Adam Keating Date: Wed, 21 Jun 2023 23:03:50 +0000 Subject: [PATCH 10/25] Added documentation --- .../moa_controllers/sys_status.py | 54 ++++++++++++++++--- 1 file changed, 47 insertions(+), 7 deletions(-) diff --git a/src/moa/moa_controllers/moa_controllers/sys_status.py b/src/moa/moa_controllers/moa_controllers/sys_status.py index d84923ab..91268929 100644 --- a/src/moa/moa_controllers/moa_controllers/sys_status.py +++ b/src/moa/moa_controllers/moa_controllers/sys_status.py @@ -16,14 +16,14 @@ class as_status(Node): def __init__(self): super().__init__('autonomous_sys_status') - # publishers + # init publishers self.status_pub = self.create_publisher( UInt8, 'as_status', 10, ) - # subscribers + # init subscribers self.ackermann_sub = Subscriber( self, AckermannDriveStamped, @@ -42,6 +42,7 @@ def __init__(self): '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, @@ -50,6 +51,7 @@ def __init__(self): self.time_sync.registerCallback(self.update_state) + # from MoTec # system states self.hw_states = { 'ebs_active': False, @@ -68,7 +70,15 @@ def __init__(self): self.mission_selected = False - def check_mission_status(self, msg: MissionStatesStamped): # TODO needs more planning work done + def check_mission_status(self, msg: MissionStatesStamped): # TODO needs more planning work done (maybe change message to separate selected and finished) + """ + Records the states of hardware components + + Args + ------------------------------ + msg: A ROS message of type `MissionStateStamped`, containing the whether mission is selected or finished. + """ + if msg.mission_state == 0: self.mission_selected = False self.mission_finished = True @@ -82,6 +92,14 @@ def check_mission_status(self, msg: MissionStatesStamped): # TODO needs more pla self.mission_finished = False def check_hardware(self, msg: HardwareStatesStamped): #TODO + """ + 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] @@ -92,20 +110,42 @@ def check_hardware(self, msg: HardwareStatesStamped): #TODO 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=None, mission_msg=None, hw_msg=None, prod=True): - ''' - AS states: + 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) From 2b27583e3187d0a5a1ab07af4a7b39f349578209 Mon Sep 17 00:00:00 2001 From: Adam Keating Date: Wed, 21 Jun 2023 23:51:40 +0000 Subject: [PATCH 11/25] updated MissionStates msg to separate mission selected and mission finished --- .../moa_controllers/sys_status.py | 17 +++++------------ src/moa/moa_controllers/test/test_sys_status.py | 17 +++++++++++++---- src/moa/moa_msgs/CMakeLists.txt | 1 + src/moa/moa_msgs/msg/MissionStates.msg | 2 ++ src/moa/moa_msgs/msg/MissionStatesStamped.msg | 2 +- 5 files changed, 22 insertions(+), 17 deletions(-) create mode 100644 src/moa/moa_msgs/msg/MissionStates.msg diff --git a/src/moa/moa_controllers/moa_controllers/sys_status.py b/src/moa/moa_controllers/moa_controllers/sys_status.py index 91268929..0609f4ac 100644 --- a/src/moa/moa_controllers/moa_controllers/sys_status.py +++ b/src/moa/moa_controllers/moa_controllers/sys_status.py @@ -70,7 +70,7 @@ def __init__(self): self.mission_selected = False - def check_mission_status(self, msg: MissionStatesStamped): # TODO needs more planning work done (maybe change message to separate selected and finished) + def check_mission_status(self, msg: MissionStatesStamped): """ Records the states of hardware components @@ -79,19 +79,12 @@ def check_mission_status(self, msg: MissionStatesStamped): # TODO needs more pla msg: A ROS message of type `MissionStateStamped`, containing the whether mission is selected or finished. """ - if msg.mission_state == 0: - self.mission_selected = False - self.mission_finished = True - - elif msg.mission_state == 1: - self.mission_selected = False - self.mission_finished = False + msg_data = msg.mission_states + self.mission_selected = bool(msg_data.mission_selected) + self.mission_finished = bool(msg_data.mission_finished) - elif msg.mission_state == 2: - self.mission_selected = True - self.mission_finished = False - def check_hardware(self, msg: HardwareStatesStamped): #TODO + def check_hardware(self, msg: HardwareStatesStamped): """ Records the states of hardware components diff --git a/src/moa/moa_controllers/test/test_sys_status.py b/src/moa/moa_controllers/test/test_sys_status.py index 9b0248ab..fb5f40e0 100644 --- a/src/moa/moa_controllers/test/test_sys_status.py +++ b/src/moa/moa_controllers/test/test_sys_status.py @@ -67,19 +67,28 @@ def test_state_4(node: as_status): # TODO to complete assert node.update_state(prod=0) == 4 -def test_check_mission_status(node: as_status): # TODO need to sort out planning/complete - msg = MissionStatesStamped(mission_state = 0) +def test_check_mission_status(node: as_status): # TODO need to sort out planning/complete + msg = MissionStatesStamped() + msg.mission_states.mission_selected = 0 + msg.mission_states.mission_finished = 1 node.check_mission_status(msg) assert node.mission_selected == False and node.mission_finished == True - msg = MissionStatesStamped(mission_state = 1) + msg.mission_states.mission_selected = 0 + msg.mission_states.mission_finished = 0 node.check_mission_status(msg) assert node.mission_selected == False and node.mission_finished == False - msg = MissionStatesStamped(mission_state = 2) + msg.mission_states.mission_selected = 1 + msg.mission_states.mission_finished = 0 node.check_mission_status(msg) assert node.mission_selected == True and node.mission_finished == False + msg.mission_states.mission_selected = 1 + msg.mission_states.mission_finished = 1 + node.check_mission_status(msg) + assert node.mission_selected == True and node.mission_finished == True + def test_check_car_stopped(node: as_status): msg = AckermannDriveStamped() diff --git a/src/moa/moa_msgs/CMakeLists.txt b/src/moa/moa_msgs/CMakeLists.txt index 32787247..9d4a5e74 100644 --- a/src/moa/moa_msgs/CMakeLists.txt +++ b/src/moa/moa_msgs/CMakeLists.txt @@ -22,6 +22,7 @@ set(msg_files "msg/ConeMapStamped.msg" "msg/HardwareStates.msg" "msg/HardwareStatesStamped.msg" + "msg/MissionStates.msg" "msg/MissionStatesStamped.msg" ) set(srv_files diff --git a/src/moa/moa_msgs/msg/MissionStates.msg b/src/moa/moa_msgs/msg/MissionStates.msg new file mode 100644 index 00000000..1237fc97 --- /dev/null +++ b/src/moa/moa_msgs/msg/MissionStates.msg @@ -0,0 +1,2 @@ +uint8 mission_selected +uint8 mission_finished \ No newline at end of file diff --git a/src/moa/moa_msgs/msg/MissionStatesStamped.msg b/src/moa/moa_msgs/msg/MissionStatesStamped.msg index 17cb761e..11f709e2 100644 --- a/src/moa/moa_msgs/msg/MissionStatesStamped.msg +++ b/src/moa/moa_msgs/msg/MissionStatesStamped.msg @@ -1,5 +1,5 @@ std_msgs/Header header -uint8 mission_state +MissionStates mission_states From f52424287722bc8cdeeb9299fe8463c50bc00d91 Mon Sep 17 00:00:00 2001 From: Adam Keating Date: Sun, 25 Jun 2023 23:43:08 +0000 Subject: [PATCH 12/25] Added can_id ros argument for the ack_to_can node --- src/moa/moa_bringup/launch/base.py | 19 +++++++++++++++---- .../moa_controllers/ack_to_can.py | 11 ++++++++++- 2 files changed, 25 insertions(+), 5 deletions(-) diff --git a/src/moa/moa_bringup/launch/base.py b/src/moa/moa_bringup/launch/base.py index 9a5b099c..e15ea17e 100644 --- a/src/moa/moa_bringup/launch/base.py +++ b/src/moa/moa_bringup/launch/base.py @@ -1,12 +1,21 @@ import launch import launch_ros.actions +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( @@ -17,10 +26,12 @@ def generate_launch_description(): launch_ros.actions.Node( package='moa_controllers', executable='as_status_node', - name='as_status_node'), + name='as_status_node', + ), launch_ros.actions.Node( package='foxglove_bridge', executable='foxglove_bridge', - name='foxglove_bridge'), - ]) \ No newline at end of file + name='foxglove_bridge' + ), + ]) \ No newline at end of file diff --git a/src/moa/moa_controllers/moa_controllers/ack_to_can.py b/src/moa/moa_controllers/moa_controllers/ack_to_can.py index ad1b4cdf..ad5a35db 100644 --- a/src/moa/moa_controllers/moa_controllers/ack_to_can.py +++ b/src/moa/moa_controllers/moa_controllers/ack_to_can.py @@ -5,6 +5,7 @@ from rclpy.node import Node import numpy as np from math import pi +from rcl_interfaces.msg import ParameterDescriptor # Ros Imports from ackermann_msgs.msg import AckermannDriveStamped @@ -15,6 +16,14 @@ 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 @@ -116,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: From ca6de50879758c3e9f416bfc6d2205e6bbefcf64 Mon Sep 17 00:00:00 2001 From: Adam Keating Date: Wed, 24 May 2023 14:23:54 +1200 Subject: [PATCH 13/25] branch init --- src/moa/moa_controllers/moa_controllers/sys_status.py | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 src/moa/moa_controllers/moa_controllers/sys_status.py diff --git a/src/moa/moa_controllers/moa_controllers/sys_status.py b/src/moa/moa_controllers/moa_controllers/sys_status.py new file mode 100644 index 00000000..e69de29b From e5279cf68a119ded91042a5257dc6c1453b5c9b5 Mon Sep 17 00:00:00 2001 From: Adam Keating Date: Fri, 16 Jun 2023 08:19:30 +0000 Subject: [PATCH 14/25] cleanup previous code --- .../moa_controllers/ack_to_can.py | 18 --------- .../scrutineering/inspection_mission.py | 6 ++- .../scrutineering/scrutineering/test_node.py | 40 ------------------- 3 files changed, 5 insertions(+), 59 deletions(-) delete mode 100644 src/plan_act/scrutineering/scrutineering/test_node.py diff --git a/src/moa/moa_controllers/moa_controllers/ack_to_can.py b/src/moa/moa_controllers/moa_controllers/ack_to_can.py index 47a69a14..d624e2df 100644 --- a/src/moa/moa_controllers/moa_controllers/ack_to_can.py +++ b/src/moa/moa_controllers/moa_controllers/ack_to_can.py @@ -4,7 +4,6 @@ import rclpy from rclpy.node import Node import numpy as np -# import zlib from math import pi # Ros Imports @@ -13,17 +12,6 @@ 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) @@ -119,12 +107,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 diff --git a/src/plan_act/scrutineering/scrutineering/inspection_mission.py b/src/plan_act/scrutineering/scrutineering/inspection_mission.py index ed143c55..a4aff732 100644 --- a/src/plan_act/scrutineering/scrutineering/inspection_mission.py +++ b/src/plan_act/scrutineering/scrutineering/inspection_mission.py @@ -1,12 +1,16 @@ +#!/usr/bin/env python3 +# Python imports import rclpy from rclpy.node import Node from rclpy.duration import Duration from rclpy.executors import SingleThreadedExecutor -from ackermann_msgs.msg import AckermannDriveStamped, AckermannDrive from math import radians as to_rads from math import sin import time +# Ros Imports +from ackermann_msgs.msg import AckermannDriveStamped, AckermannDrive + class inspection_mission_node(Node): def __init__(self): super().__init__('inspection_mission_node') diff --git a/src/plan_act/scrutineering/scrutineering/test_node.py b/src/plan_act/scrutineering/scrutineering/test_node.py deleted file mode 100644 index 0c20d722..00000000 --- a/src/plan_act/scrutineering/scrutineering/test_node.py +++ /dev/null @@ -1,40 +0,0 @@ -import rclpy -from rclpy.node import Node -from time import sleep -from ackermann_msgs.msg import AckermannDriveStamped, AckermannDrive - -class testNode(Node): - def __init__(self): - super().__init__('testNode') - self.sub = self.create_subscription(AckermannDriveStamped, 'cmd_val', self.msg_callback,10) - self.pub = self.create_publisher(AckermannDriveStamped, 'moa/curr_vel',10) - self.received = [] - self.real = False - def msg_callback(self, msg): - self.received.append(msg) - sleep(1) - if self.real: - for i in range(1,5): - rand = random.random() - noise_msg = AckermannDrive(speed = rand, accel = rand) - self.pub.publish(AckermannDriveStamped(drive = noise_msg)) - sleep(0.5) - - self.pub.publish(AckermannDriveStamped(drive = msg.drive)) - self.get_logger().info(f'received msg {msg}') - - -def main(args=None): - rclpy.init(args=args) - - test_node = testNode() - - rclpy.spin(test_node) - - test_node.destroy_node() - - rclpy.shutdown() - - -if __name__ == '__main__': - main() \ No newline at end of file From 7768e1dfba30a76268b81870cbbafadf9ed3f599 Mon Sep 17 00:00:00 2001 From: Adam Keating Date: Fri, 16 Jun 2023 08:53:15 +0000 Subject: [PATCH 15/25] Added hardware state message for receiving data from motec --- src/moa/moa_msgs/CMakeLists.txt | 2 ++ src/moa/moa_msgs/msg/HardwareStates.msg | 5 +++++ src/moa/moa_msgs/msg/HardwareStatesStamped.msg | 3 +++ 3 files changed, 10 insertions(+) create mode 100644 src/moa/moa_msgs/msg/HardwareStates.msg create mode 100644 src/moa/moa_msgs/msg/HardwareStatesStamped.msg diff --git a/src/moa/moa_msgs/CMakeLists.txt b/src/moa/moa_msgs/CMakeLists.txt index ab667214..619a1470 100644 --- a/src/moa/moa_msgs/CMakeLists.txt +++ b/src/moa/moa_msgs/CMakeLists.txt @@ -20,6 +20,8 @@ set(msg_files "msg/ConeStamped.msg" "msg/ConeMap.msg" "msg/ConeMapStamped.msg" + "msg/HardwareStates.msg" + "msg/HardwareStatesStamped.msg" ) set(srv_files "srv/CANSendReq.srv" diff --git a/src/moa/moa_msgs/msg/HardwareStates.msg b/src/moa/moa_msgs/msg/HardwareStates.msg new file mode 100644 index 00000000..67a1d235 --- /dev/null +++ b/src/moa/moa_msgs/msg/HardwareStates.msg @@ -0,0 +1,5 @@ +uint8 ebs_active +uint8 ts_active +uint8 in_gear +uint8 master_switch_on +uint8 brakes_engaged \ No newline at end of file diff --git a/src/moa/moa_msgs/msg/HardwareStatesStamped.msg b/src/moa/moa_msgs/msg/HardwareStatesStamped.msg new file mode 100644 index 00000000..9737ceb4 --- /dev/null +++ b/src/moa/moa_msgs/msg/HardwareStatesStamped.msg @@ -0,0 +1,3 @@ +std_msgs/Header header + +HardwareStates hardware_states \ No newline at end of file From 5841d545ddd1a5e02a6b23ad75cea19c7d41da2b Mon Sep 17 00:00:00 2001 From: Adam Keating Date: Fri, 16 Jun 2023 08:54:03 +0000 Subject: [PATCH 16/25] Added status node to moa_bringup launchfiles --- src/moa/moa_bringup/launch/base.py | 5 +++++ src/moa/moa_controllers/setup.py | 3 ++- 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/src/moa/moa_bringup/launch/base.py b/src/moa/moa_bringup/launch/base.py index d9d11005..de80f321 100644 --- a/src/moa/moa_bringup/launch/base.py +++ b/src/moa/moa_bringup/launch/base.py @@ -23,6 +23,11 @@ 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', diff --git a/src/moa/moa_controllers/setup.py b/src/moa/moa_controllers/setup.py index 5820683e..dea0cbec 100644 --- a/src/moa/moa_controllers/setup.py +++ b/src/moa/moa_controllers/setup.py @@ -20,7 +20,8 @@ tests_require=['pytest'], entry_points={ 'console_scripts': [ - 'ack_to_can_node = moa_controllers.ack_to_can:main' + 'ack_to_can_node = moa_controllers.ack_to_can:main', + 'as_status_node = moa_controllers.sys_status:main', ], }, ) From 1aa8a3d8a148110a94b6b9691670cae27554df8e Mon Sep 17 00:00:00 2001 From: Adam Keating Date: Fri, 16 Jun 2023 09:38:33 +0000 Subject: [PATCH 17/25] - cleanup code - created & setup test file - created as_status node - finished v1 of state updater - setup subscribers/publishers - setup car stopped check --- .../moa_controllers/ack_to_can.py | 3 +- .../moa_controllers/sys_status.py | 140 ++++++++++++++++++ .../moa_controllers/test/test_ackermann.py | 3 +- .../moa_controllers/test/test_sys_status.py | 16 ++ 4 files changed, 158 insertions(+), 4 deletions(-) create mode 100644 src/moa/moa_controllers/test/test_sys_status.py diff --git a/src/moa/moa_controllers/moa_controllers/ack_to_can.py b/src/moa/moa_controllers/moa_controllers/ack_to_can.py index d624e2df..ad1b4cdf 100644 --- a/src/moa/moa_controllers/moa_controllers/ack_to_can.py +++ b/src/moa/moa_controllers/moa_controllers/ack_to_can.py @@ -9,8 +9,7 @@ # 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 + class ack_to_can(Node): def __init__(self): diff --git a/src/moa/moa_controllers/moa_controllers/sys_status.py b/src/moa/moa_controllers/moa_controllers/sys_status.py index e69de29b..d1bfed37 100644 --- a/src/moa/moa_controllers/moa_controllers/sys_status.py +++ b/src/moa/moa_controllers/moa_controllers/sys_status.py @@ -0,0 +1,140 @@ +#!/usr/bin/env python3 +# Python imports +import rclpy +from rclpy.node import Node +from rclpy.callback_groups import ReentrantCallbackGroup + +# ROS imports +from ackermann_msgs.msg import AckermannDriveStamped +from std_msgs.msg import UInt8 +from moa_msgs.msg import HardwareStatesStamped + +class as_status(Node): + def __init__(self): + super().__init__('autonomous_sys_status') + + self.sub_cbgroup = ReentrantCallbackGroup() + + # publishers + self.status_pub = self.create_publisher( + UInt8, + 'as_status', + 10, + ) + + # subscribers + self.ackermann_sub = self.create_subscription( + AckermannDriveStamped, + 'moa/curr_vel', + self.check_car_stopped, + 10, + callback_group = self.sub_cbgroup, + ) + + self.mission_status_sub = self.create_subscription( + UInt8, + '', + self.check_mission_status, + 10, + callback_group = self.sub_cbgroup, + ) + + self.hardware_status_sub = self.create_subscription( + HardwareStatesStamped, + '', + self.check_hardware, + 10, + callback_group = self.sub_cbgroup, + ) + + #timer + self.update_timer = self.create_timer( + 1/50, #time in seconds (currently equiv of 50Hz) + self.update_state, + ) + + # system states + # from motec + self.ebs_state = False + self.ts_active = False + self.in_gear = False + self.master_switch_on = False + self.asb_ready = False + self.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: UInt8): # TODO needs more planning work done + if msg.data == 0: + self.mission_selected = False + self.mission_finished = False + + if msg.data == 1: + self.mission_finished == True + elif msg.data == 2: + self.mission_selected = True + + self.get_logger().info('checked mission status') + + + def check_hardware(self, msg: HardwareStatesStamped): + self.get_logger().info('checked hardware') + + + def check_car_stopped(self, msg: AckermannDriveStamped): + if msg.drive.speed == 0.0 and msg.drive.acceleration == 0.0: + self.car_stopped = True + else: + self.car_stopped = False + + self.get_logger().info('checked car motion') + + + def update_state(self): + ''' + AS states: + 0 : AS finished + 1 : AS emergency + 2 : AS ready + 3 : AS driving + 4 : AS off + ''' + status_code = 4 + try: + if self.ebs_state: + if self.mission_status and self.car_stopped: + status_code = 0 + else: + raise Exception + else: + if self.mission_selected and self.master_switch_on and self.asb_ready and self.ts_active: + if self.ts_active and self.in_gear: + status_code = 3 + else: + if self.brakes_engaged: + status_code = 2 + except: + status_code = 1 + + msg = UInt8(data=status_code) + self.status_pub.publish(msg) + + +def main(args=None): + rclpy.init(args=args) + + node = as_status() + rclpy.spin(node) + + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/moa/moa_controllers/test/test_ackermann.py b/src/moa/moa_controllers/test/test_ackermann.py index af6a9636..0bba7c41 100644 --- a/src/moa/moa_controllers/test/test_ackermann.py +++ b/src/moa/moa_controllers/test/test_ackermann.py @@ -1,7 +1,6 @@ import pytest import numpy as np -from ackermann_msgs.msg import AckermannDrive, AckermannDriveStamped -from moa_msgs.msg import CANStamped +from ackermann_msgs.msg import AckermannDriveStamped from moa_controllers.ack_to_can import ack_to_can from unittest.mock import Mock import rclpy diff --git a/src/moa/moa_controllers/test/test_sys_status.py b/src/moa/moa_controllers/test/test_sys_status.py new file mode 100644 index 00000000..22aaac5f --- /dev/null +++ b/src/moa/moa_controllers/test/test_sys_status.py @@ -0,0 +1,16 @@ +import pytest +import rclpy +from moa_controllers.sys_status import as_status + + +@pytest.fixture(autouse=True, scope='session') +def node(): + rclpy.init(args=None) + test_status_node = as_status() + + yield test_status_node + test_status_node.destroy_node() + rclpy.shutdown() + +def test_status(node: as_status): + pass \ No newline at end of file From 9fd9af8fcc4311497b898c51ca48304b453e91f8 Mon Sep 17 00:00:00 2001 From: Adam Keating Date: Wed, 21 Jun 2023 00:32:11 +0000 Subject: [PATCH 18/25] cleanup/add yaml package to devcontainer.json --- .devcontainer/devcontainer.json | 3 ++- src/moa/moa_controllers/package.xml | 3 +-- src/moa/moa_controllers/test/test_ackermann.py | 8 +------- 3 files changed, 4 insertions(+), 10 deletions(-) diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 58496c3f..42a88709 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -28,7 +28,8 @@ "redhat.vscode-xml", "twxs.cmake", "VisualStudioExptTeam.intellicode-api-usage-examples", - "VisualStudioExptTeam.vscodeintellicode" + "VisualStudioExptTeam.vscodeintellicode", + "redhat.vscode-yaml" ] } } diff --git a/src/moa/moa_controllers/package.xml b/src/moa/moa_controllers/package.xml index 56a8082c..af40df55 100644 --- a/src/moa/moa_controllers/package.xml +++ b/src/moa/moa_controllers/package.xml @@ -15,8 +15,7 @@ python3-numpy - - + ackermann_msgs moa_msgs diff --git a/src/moa/moa_controllers/test/test_ackermann.py b/src/moa/moa_controllers/test/test_ackermann.py index 0bba7c41..99296e58 100644 --- a/src/moa/moa_controllers/test/test_ackermann.py +++ b/src/moa/moa_controllers/test/test_ackermann.py @@ -7,7 +7,7 @@ -@pytest.fixture(autouse=True, scope='session') +@pytest.fixture(autouse=True, scope='module') def node(): rclpy.init(args=None) test_ack = ack_to_can() @@ -16,12 +16,6 @@ def node(): test_ack.destroy_node() rclpy.shutdown() - -# def test_compress_floats(): -# values = np.array([1.0, 1.5, 2.0, 2.5]) -# expected_result = b'x\x9c+\x01\x00\xf6\xff,\x02\x00\xfa\xff' -# assert compress_floats(values) == expected_result - def test_ackermann_to_can_parser_out_of_bounds(node: ack_to_can): ack_msg = AckermannDriveStamped() From 53d0f90eee1a14b39011ad8946ef3a76b5842dab Mon Sep 17 00:00:00 2001 From: Adam Keating Date: Wed, 21 Jun 2023 01:17:16 +0000 Subject: [PATCH 19/25] added attribute to HardwareStates message and updated gitignore --- .gitignore | 1 + src/moa/moa_msgs/msg/HardwareStates.msg | 1 + 2 files changed, 2 insertions(+) diff --git a/.gitignore b/.gitignore index ec12b3c9..5e3e5972 100644 --- a/.gitignore +++ b/.gitignore @@ -6,3 +6,4 @@ log .vscode/settings.json .gitignore .vscode/c_cpp_properties.json +**/__pycache__/ \ No newline at end of file diff --git a/src/moa/moa_msgs/msg/HardwareStates.msg b/src/moa/moa_msgs/msg/HardwareStates.msg index 67a1d235..412b20ff 100644 --- a/src/moa/moa_msgs/msg/HardwareStates.msg +++ b/src/moa/moa_msgs/msg/HardwareStates.msg @@ -2,4 +2,5 @@ uint8 ebs_active uint8 ts_active uint8 in_gear uint8 master_switch_on +uint8 asb_ready uint8 brakes_engaged \ No newline at end of file From 9bc895c0708ebe9d056cee6db1c8cae68ccf4360 Mon Sep 17 00:00:00 2001 From: Adam Keating Date: Wed, 21 Jun 2023 01:24:27 +0000 Subject: [PATCH 20/25] WIP: changing devices --- .../moa_controllers/sys_status.py | 66 +++++++---- .../moa_controllers/test/test_sys_status.py | 107 +++++++++++++++++- 2 files changed, 146 insertions(+), 27 deletions(-) diff --git a/src/moa/moa_controllers/moa_controllers/sys_status.py b/src/moa/moa_controllers/moa_controllers/sys_status.py index d1bfed37..4c5a1d33 100644 --- a/src/moa/moa_controllers/moa_controllers/sys_status.py +++ b/src/moa/moa_controllers/moa_controllers/sys_status.py @@ -3,6 +3,8 @@ import rclpy from rclpy.node import Node from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.executors import MultiThreadedExecutor +from re import search # ROS imports from ackermann_msgs.msg import AckermannDriveStamped @@ -33,7 +35,7 @@ def __init__(self): self.mission_status_sub = self.create_subscription( UInt8, - '', + 'as_status', # TODO change name self.check_mission_status, 10, callback_group = self.sub_cbgroup, @@ -41,7 +43,7 @@ def __init__(self): self.hardware_status_sub = self.create_subscription( HardwareStatesStamped, - '', + 'moa/hardware_state', # TODO change name self.check_hardware, 10, callback_group = self.sub_cbgroup, @@ -49,18 +51,19 @@ def __init__(self): #timer self.update_timer = self.create_timer( - 1/50, #time in seconds (currently equiv of 50Hz) + 1/50, #time in seconds (currently equiv of 50 times per second) self.update_state, ) # system states - # from motec - self.ebs_state = False - self.ts_active = False - self.in_gear = False - self.master_switch_on = False - self.asb_ready = False - self.brakes_engaged = False + 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 @@ -72,18 +75,27 @@ def __init__(self): def check_mission_status(self, msg: UInt8): # TODO needs more planning work done if msg.data == 0: + self.mission_finished = True + + elif msg.data == 1: self.mission_selected = False self.mission_finished = False - - if msg.data == 1: - self.mission_finished == True + elif msg.data == 2: self.mission_selected = True self.get_logger().info('checked mission status') - def check_hardware(self, msg: HardwareStatesStamped): + def check_hardware(self, msg: HardwareStatesStamped): #TODO + 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) + self.get_logger().info('checked hardware') @@ -107,30 +119,36 @@ def update_state(self): ''' status_code = 4 try: - if self.ebs_state: - if self.mission_status and self.car_stopped: + if self.hw_states['ebs_active']: + if self.mission_finished and self.car_stopped: status_code = 0 else: raise Exception else: - if self.mission_selected and self.master_switch_on and self.asb_ready and self.ts_active: - if self.ts_active and self.in_gear: + 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.brakes_engaged: + if self.hw_states['brakes_engaged']: status_code = 2 - except: + except Exception: + self.get_logger().error("ERROR: AS in emergency state") status_code = 1 - - msg = UInt8(data=status_code) - self.status_pub.publish(msg) + finally: + msg = UInt8(data=status_code) + self.status_pub.publish(msg) + return status_code def main(args=None): rclpy.init(args=args) node = as_status() - rclpy.spin(node) + executor = MultiThreadedExecutor(num_threads=2) + executor.add_node(node) + + executor.spin() node.destroy_node() rclpy.shutdown() diff --git a/src/moa/moa_controllers/test/test_sys_status.py b/src/moa/moa_controllers/test/test_sys_status.py index 22aaac5f..1253fe46 100644 --- a/src/moa/moa_controllers/test/test_sys_status.py +++ b/src/moa/moa_controllers/test/test_sys_status.py @@ -1,16 +1,117 @@ import pytest import rclpy from moa_controllers.sys_status import as_status +from moa_msgs.msg import HardwareStatesStamped +from ackermann_msgs.msg import AckermannDriveStamped +from std_msgs.msg import UInt8 -@pytest.fixture(autouse=True, scope='session') +@pytest.fixture(autouse=True, scope='function') def node(): rclpy.init(args=None) + test_status_node = as_status() + test_status_node.update_timer = None yield test_status_node + test_status_node.destroy_node() rclpy.shutdown() -def test_status(node: as_status): - pass \ No newline at end of file +def test_state_0(node: as_status): + node.hw_states['ebs_active'] = True + node.mission_finished = True + node.car_stopped = True + + assert node.update_state() == 0 + + +def test_state_1(node: as_status): + node.hw_states['ebs_active'] = True + test_cases = [[0,0],[0,1],[1,0]] + + for test in test_cases: + node.mission_finished = test[0] + node.car_stopped = test[1] + + assert node.update_state() == 1 + + +def test_state_2(node: as_status): + node.mission_selected = True + node.hw_states['master_switch_on'] = True + node.hw_states['asb_ready'] = True + node.hw_states['ts_active'] = True + + node.hw_states['brakes_engaged'] = True + + assert node.update_state() == 2 + + +def test_state_3(node: as_status): + node.mission_selected = True + node.hw_states['master_switch_on'] = True + node.hw_states['asb_ready'] = True + node.hw_states['ts_active'] = True + + node.hw_states['in_gear'] = True + + assert node.update_state() == 3 + + +def test_state_4(node: as_status): # TODO to complete + node.mission_selected = True + node.hw_states['master_switch_on'] = True + node.hw_states['asb_ready'] = True + node.hw_states['ts_active'] = True + + assert node.update_state() == 4 + + +def test_check_mission_status(node: as_status): # TODO need to sort out planning/complete + msg = UInt8(data = 0) + node.check_mission_status(msg) + assert node.mission_selected == False and node.mission_finished == True + + msg = UInt8(data = 1) + node.check_mission_status(msg) + assert node.mission_selected == False and node.mission_finished == False + + msg = UInt8(data = 2) + node.check_mission_status(msg) + assert node.mission_selected == True and node.mission_finished == True + + +def test_check_car_stopped(node: as_status): + msg = AckermannDriveStamped() + + msg.drive.speed = 0.0 + msg.drive.acceleration = 0.0 + node.check_car_stopped(msg) + assert node.car_stopped + + msg.drive.speed = 1.0 + msg.drive.acceleration = 1.0 + node.check_car_stopped(msg) + assert not node.car_stopped + + +def test_check_hardware(node: as_status): # TODO complete + msg = HardwareStatesStamped() + msg.hardware_states.ebs_active = 1 + msg.hardware_states.ts_active = 0 + msg.hardware_states.in_gear = 1 + msg.hardware_states.master_switch_on = 1 + msg.hardware_states.brakes_engaged = 1 + + test_case = { + 'ebs_active': 1, + 'ts_active': 0, + 'in_gear': 1, + 'master_switch_on': 1, + 'asb_ready': 0, + 'brakes_engaged': 1, + } + + node.check_hardware(msg) + assert node.hw_states == test_case \ No newline at end of file From d67de6d29b5fed5a89d82a897ce0b1fef042545f Mon Sep 17 00:00:00 2001 From: Adam Keating Date: Wed, 21 Jun 2023 09:54:28 +0000 Subject: [PATCH 21/25] Added mission state custom message/fixed node testing/modified node for time synchronisation of topics --- .../moa_controllers/sys_status.py | 86 +++++++++---------- .../moa_controllers/test/test_sys_status.py | 23 +++-- src/moa/moa_msgs/CMakeLists.txt | 1 + src/moa/moa_msgs/msg/MissionStatesStamped.msg | 5 ++ 4 files changed, 58 insertions(+), 57 deletions(-) create mode 100644 src/moa/moa_msgs/msg/MissionStatesStamped.msg diff --git a/src/moa/moa_controllers/moa_controllers/sys_status.py b/src/moa/moa_controllers/moa_controllers/sys_status.py index 4c5a1d33..d84923ab 100644 --- a/src/moa/moa_controllers/moa_controllers/sys_status.py +++ b/src/moa/moa_controllers/moa_controllers/sys_status.py @@ -2,21 +2,20 @@ # Python imports import rclpy from rclpy.node import Node -from rclpy.callback_groups import ReentrantCallbackGroup 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 +from moa_msgs.msg import HardwareStatesStamped, MissionStatesStamped + class as_status(Node): def __init__(self): super().__init__('autonomous_sys_status') - self.sub_cbgroup = ReentrantCallbackGroup() - # publishers self.status_pub = self.create_publisher( UInt8, @@ -25,36 +24,32 @@ def __init__(self): ) # subscribers - self.ackermann_sub = self.create_subscription( + self.ackermann_sub = Subscriber( + self, AckermannDriveStamped, 'moa/curr_vel', - self.check_car_stopped, - 10, - callback_group = self.sub_cbgroup, ) - self.mission_status_sub = self.create_subscription( - UInt8, - 'as_status', # TODO change name - self.check_mission_status, - 10, - callback_group = self.sub_cbgroup, + self.mission_status_sub = Subscriber( + self, + MissionStatesStamped, + 'mission_status', # TODO change name ) - self.hardware_status_sub = self.create_subscription( + self.hardware_status_sub = Subscriber( + self, HardwareStatesStamped, 'moa/hardware_state', # TODO change name - self.check_hardware, - 10, - callback_group = self.sub_cbgroup, ) - #timer - self.update_timer = self.create_timer( - 1/50, #time in seconds (currently equiv of 50 times per second) - self.update_state, + 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) + # system states self.hw_states = { 'ebs_active': False, @@ -73,19 +68,18 @@ def __init__(self): self.mission_selected = False - def check_mission_status(self, msg: UInt8): # TODO needs more planning work done - if msg.data == 0: - self.mission_finished = True + def check_mission_status(self, msg: MissionStatesStamped): # TODO needs more planning work done + if msg.mission_state == 0: + self.mission_selected = False + self.mission_finished = True - elif msg.data == 1: + elif msg.mission_state == 1: self.mission_selected = False self.mission_finished = False - elif msg.data == 2: + elif msg.mission_state == 2: self.mission_selected = True - - self.get_logger().info('checked mission status') - + self.mission_finished = False def check_hardware(self, msg: HardwareStatesStamped): #TODO msg_data = msg.hardware_states @@ -96,19 +90,14 @@ def check_hardware(self, msg: HardwareStatesStamped): #TODO if search("get_*", attr) is None and search("SLOT*", attr) is None: self.hw_states[attr] = getattr(msg_data, attr) - self.get_logger().info('checked hardware') - def check_car_stopped(self, msg: AckermannDriveStamped): if msg.drive.speed == 0.0 and msg.drive.acceleration == 0.0: self.car_stopped = True else: self.car_stopped = False - - self.get_logger().info('checked car motion') - - def update_state(self): + def update_state(self, ack_msg=None, mission_msg=None, hw_msg=None, prod=True): ''' AS states: 0 : AS finished @@ -117,13 +106,18 @@ def update_state(self): 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 Exception + 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']): @@ -132,26 +126,28 @@ def update_state(self): else: if self.hw_states['brakes_engaged']: status_code = 2 - except Exception: + 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) - + rclpy.init(args=args) node = as_status() executor = MultiThreadedExecutor(num_threads=2) executor.add_node(node) - - executor.spin() - - node.destroy_node() - rclpy.shutdown() + + try: + executor.spin() + + except SystemExit: + node.destroy_node() + rclpy.shutdown() if __name__ == '__main__': diff --git a/src/moa/moa_controllers/test/test_sys_status.py b/src/moa/moa_controllers/test/test_sys_status.py index 1253fe46..9b0248ab 100644 --- a/src/moa/moa_controllers/test/test_sys_status.py +++ b/src/moa/moa_controllers/test/test_sys_status.py @@ -1,9 +1,8 @@ import pytest import rclpy from moa_controllers.sys_status import as_status -from moa_msgs.msg import HardwareStatesStamped +from moa_msgs.msg import HardwareStatesStamped, MissionStatesStamped from ackermann_msgs.msg import AckermannDriveStamped -from std_msgs.msg import UInt8 @pytest.fixture(autouse=True, scope='function') @@ -23,7 +22,7 @@ def test_state_0(node: as_status): node.mission_finished = True node.car_stopped = True - assert node.update_state() == 0 + assert node.update_state(prod=0) == 0 def test_state_1(node: as_status): @@ -34,7 +33,7 @@ def test_state_1(node: as_status): node.mission_finished = test[0] node.car_stopped = test[1] - assert node.update_state() == 1 + assert node.update_state(prod=0) == 1 def test_state_2(node: as_status): @@ -45,7 +44,7 @@ def test_state_2(node: as_status): node.hw_states['brakes_engaged'] = True - assert node.update_state() == 2 + assert node.update_state(prod=0) == 2 def test_state_3(node: as_status): @@ -56,7 +55,7 @@ def test_state_3(node: as_status): node.hw_states['in_gear'] = True - assert node.update_state() == 3 + assert node.update_state(prod=0) == 3 def test_state_4(node: as_status): # TODO to complete @@ -65,21 +64,21 @@ def test_state_4(node: as_status): # TODO to complete node.hw_states['asb_ready'] = True node.hw_states['ts_active'] = True - assert node.update_state() == 4 + assert node.update_state(prod=0) == 4 def test_check_mission_status(node: as_status): # TODO need to sort out planning/complete - msg = UInt8(data = 0) + msg = MissionStatesStamped(mission_state = 0) node.check_mission_status(msg) assert node.mission_selected == False and node.mission_finished == True - msg = UInt8(data = 1) + msg = MissionStatesStamped(mission_state = 1) node.check_mission_status(msg) assert node.mission_selected == False and node.mission_finished == False - msg = UInt8(data = 2) + msg = MissionStatesStamped(mission_state = 2) node.check_mission_status(msg) - assert node.mission_selected == True and node.mission_finished == True + assert node.mission_selected == True and node.mission_finished == False def test_check_car_stopped(node: as_status): @@ -114,4 +113,4 @@ def test_check_hardware(node: as_status): # TODO complete } node.check_hardware(msg) - assert node.hw_states == test_case \ No newline at end of file + assert node.hw_states == test_case \ No newline at end of file diff --git a/src/moa/moa_msgs/CMakeLists.txt b/src/moa/moa_msgs/CMakeLists.txt index 619a1470..32787247 100644 --- a/src/moa/moa_msgs/CMakeLists.txt +++ b/src/moa/moa_msgs/CMakeLists.txt @@ -22,6 +22,7 @@ set(msg_files "msg/ConeMapStamped.msg" "msg/HardwareStates.msg" "msg/HardwareStatesStamped.msg" + "msg/MissionStatesStamped.msg" ) set(srv_files "srv/CANSendReq.srv" diff --git a/src/moa/moa_msgs/msg/MissionStatesStamped.msg b/src/moa/moa_msgs/msg/MissionStatesStamped.msg new file mode 100644 index 00000000..17cb761e --- /dev/null +++ b/src/moa/moa_msgs/msg/MissionStatesStamped.msg @@ -0,0 +1,5 @@ +std_msgs/Header header + +uint8 mission_state + + From 0c9e9259c1f06666b574b5b13197654c1f715db7 Mon Sep 17 00:00:00 2001 From: Adam Keating Date: Wed, 21 Jun 2023 23:03:50 +0000 Subject: [PATCH 22/25] Added documentation --- .../moa_controllers/sys_status.py | 54 ++++++++++++++++--- 1 file changed, 47 insertions(+), 7 deletions(-) diff --git a/src/moa/moa_controllers/moa_controllers/sys_status.py b/src/moa/moa_controllers/moa_controllers/sys_status.py index d84923ab..91268929 100644 --- a/src/moa/moa_controllers/moa_controllers/sys_status.py +++ b/src/moa/moa_controllers/moa_controllers/sys_status.py @@ -16,14 +16,14 @@ class as_status(Node): def __init__(self): super().__init__('autonomous_sys_status') - # publishers + # init publishers self.status_pub = self.create_publisher( UInt8, 'as_status', 10, ) - # subscribers + # init subscribers self.ackermann_sub = Subscriber( self, AckermannDriveStamped, @@ -42,6 +42,7 @@ def __init__(self): '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, @@ -50,6 +51,7 @@ def __init__(self): self.time_sync.registerCallback(self.update_state) + # from MoTec # system states self.hw_states = { 'ebs_active': False, @@ -68,7 +70,15 @@ def __init__(self): self.mission_selected = False - def check_mission_status(self, msg: MissionStatesStamped): # TODO needs more planning work done + def check_mission_status(self, msg: MissionStatesStamped): # TODO needs more planning work done (maybe change message to separate selected and finished) + """ + Records the states of hardware components + + Args + ------------------------------ + msg: A ROS message of type `MissionStateStamped`, containing the whether mission is selected or finished. + """ + if msg.mission_state == 0: self.mission_selected = False self.mission_finished = True @@ -82,6 +92,14 @@ def check_mission_status(self, msg: MissionStatesStamped): # TODO needs more pla self.mission_finished = False def check_hardware(self, msg: HardwareStatesStamped): #TODO + """ + 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] @@ -92,20 +110,42 @@ def check_hardware(self, msg: HardwareStatesStamped): #TODO 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=None, mission_msg=None, hw_msg=None, prod=True): - ''' - AS states: + 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) From b8dccac7bc13b905fee83ad2802675277d0b3ae1 Mon Sep 17 00:00:00 2001 From: Adam Keating Date: Wed, 21 Jun 2023 23:51:40 +0000 Subject: [PATCH 23/25] updated MissionStates msg to separate mission selected and mission finished --- .../moa_controllers/sys_status.py | 17 +++++------------ src/moa/moa_controllers/test/test_sys_status.py | 17 +++++++++++++---- src/moa/moa_msgs/CMakeLists.txt | 1 + src/moa/moa_msgs/msg/MissionStates.msg | 2 ++ src/moa/moa_msgs/msg/MissionStatesStamped.msg | 2 +- 5 files changed, 22 insertions(+), 17 deletions(-) create mode 100644 src/moa/moa_msgs/msg/MissionStates.msg diff --git a/src/moa/moa_controllers/moa_controllers/sys_status.py b/src/moa/moa_controllers/moa_controllers/sys_status.py index 91268929..0609f4ac 100644 --- a/src/moa/moa_controllers/moa_controllers/sys_status.py +++ b/src/moa/moa_controllers/moa_controllers/sys_status.py @@ -70,7 +70,7 @@ def __init__(self): self.mission_selected = False - def check_mission_status(self, msg: MissionStatesStamped): # TODO needs more planning work done (maybe change message to separate selected and finished) + def check_mission_status(self, msg: MissionStatesStamped): """ Records the states of hardware components @@ -79,19 +79,12 @@ def check_mission_status(self, msg: MissionStatesStamped): # TODO needs more pla msg: A ROS message of type `MissionStateStamped`, containing the whether mission is selected or finished. """ - if msg.mission_state == 0: - self.mission_selected = False - self.mission_finished = True - - elif msg.mission_state == 1: - self.mission_selected = False - self.mission_finished = False + msg_data = msg.mission_states + self.mission_selected = bool(msg_data.mission_selected) + self.mission_finished = bool(msg_data.mission_finished) - elif msg.mission_state == 2: - self.mission_selected = True - self.mission_finished = False - def check_hardware(self, msg: HardwareStatesStamped): #TODO + def check_hardware(self, msg: HardwareStatesStamped): """ Records the states of hardware components diff --git a/src/moa/moa_controllers/test/test_sys_status.py b/src/moa/moa_controllers/test/test_sys_status.py index 9b0248ab..fb5f40e0 100644 --- a/src/moa/moa_controllers/test/test_sys_status.py +++ b/src/moa/moa_controllers/test/test_sys_status.py @@ -67,19 +67,28 @@ def test_state_4(node: as_status): # TODO to complete assert node.update_state(prod=0) == 4 -def test_check_mission_status(node: as_status): # TODO need to sort out planning/complete - msg = MissionStatesStamped(mission_state = 0) +def test_check_mission_status(node: as_status): # TODO need to sort out planning/complete + msg = MissionStatesStamped() + msg.mission_states.mission_selected = 0 + msg.mission_states.mission_finished = 1 node.check_mission_status(msg) assert node.mission_selected == False and node.mission_finished == True - msg = MissionStatesStamped(mission_state = 1) + msg.mission_states.mission_selected = 0 + msg.mission_states.mission_finished = 0 node.check_mission_status(msg) assert node.mission_selected == False and node.mission_finished == False - msg = MissionStatesStamped(mission_state = 2) + msg.mission_states.mission_selected = 1 + msg.mission_states.mission_finished = 0 node.check_mission_status(msg) assert node.mission_selected == True and node.mission_finished == False + msg.mission_states.mission_selected = 1 + msg.mission_states.mission_finished = 1 + node.check_mission_status(msg) + assert node.mission_selected == True and node.mission_finished == True + def test_check_car_stopped(node: as_status): msg = AckermannDriveStamped() diff --git a/src/moa/moa_msgs/CMakeLists.txt b/src/moa/moa_msgs/CMakeLists.txt index 32787247..9d4a5e74 100644 --- a/src/moa/moa_msgs/CMakeLists.txt +++ b/src/moa/moa_msgs/CMakeLists.txt @@ -22,6 +22,7 @@ set(msg_files "msg/ConeMapStamped.msg" "msg/HardwareStates.msg" "msg/HardwareStatesStamped.msg" + "msg/MissionStates.msg" "msg/MissionStatesStamped.msg" ) set(srv_files diff --git a/src/moa/moa_msgs/msg/MissionStates.msg b/src/moa/moa_msgs/msg/MissionStates.msg new file mode 100644 index 00000000..1237fc97 --- /dev/null +++ b/src/moa/moa_msgs/msg/MissionStates.msg @@ -0,0 +1,2 @@ +uint8 mission_selected +uint8 mission_finished \ No newline at end of file diff --git a/src/moa/moa_msgs/msg/MissionStatesStamped.msg b/src/moa/moa_msgs/msg/MissionStatesStamped.msg index 17cb761e..11f709e2 100644 --- a/src/moa/moa_msgs/msg/MissionStatesStamped.msg +++ b/src/moa/moa_msgs/msg/MissionStatesStamped.msg @@ -1,5 +1,5 @@ std_msgs/Header header -uint8 mission_state +MissionStates mission_states From fff7b59a4580c717eb209cc3381bd2aa29ae5560 Mon Sep 17 00:00:00 2001 From: Adam Keating Date: Sun, 25 Jun 2023 23:43:08 +0000 Subject: [PATCH 24/25] Added can_id ros argument for the ack_to_can node --- src/moa/moa_bringup/launch/base.py | 19 +++++++++++++++---- .../moa_controllers/ack_to_can.py | 11 ++++++++++- 2 files changed, 25 insertions(+), 5 deletions(-) diff --git a/src/moa/moa_bringup/launch/base.py b/src/moa/moa_bringup/launch/base.py index de80f321..d895eef4 100644 --- a/src/moa/moa_bringup/launch/base.py +++ b/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( @@ -26,10 +35,12 @@ def generate_launch_description(): launch_ros.actions.Node( package='moa_controllers', executable='as_status_node', - name='as_status_node'), + name='as_status_node', + ), launch_ros.actions.Node( package='foxglove_bridge', executable='foxglove_bridge', - name='foxglove_bridge'), - ]) \ No newline at end of file + name='foxglove_bridge' + ), + ]) \ No newline at end of file diff --git a/src/moa/moa_controllers/moa_controllers/ack_to_can.py b/src/moa/moa_controllers/moa_controllers/ack_to_can.py index ad1b4cdf..ad5a35db 100644 --- a/src/moa/moa_controllers/moa_controllers/ack_to_can.py +++ b/src/moa/moa_controllers/moa_controllers/ack_to_can.py @@ -5,6 +5,7 @@ from rclpy.node import Node import numpy as np from math import pi +from rcl_interfaces.msg import ParameterDescriptor # Ros Imports from ackermann_msgs.msg import AckermannDriveStamped @@ -15,6 +16,14 @@ 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 @@ -116,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: From 451d1a609b8da87887365dd4b34defd329fdc52b Mon Sep 17 00:00:00 2001 From: Tanish Bhatt Date: Sun, 14 Jul 2024 21:36:44 +1200 Subject: [PATCH 25/25] system status to can node --- .../moa_controllers/sys_status_to_can.py | 81 +++++++++++++++++++ 1 file changed, 81 insertions(+) create mode 100644 src/moa/moa_controllers/moa_controllers/sys_status_to_can.py diff --git a/src/moa/moa_controllers/moa_controllers/sys_status_to_can.py b/src/moa/moa_controllers/moa_controllers/sys_status_to_can.py new file mode 100644 index 00000000..c790f505 --- /dev/null +++ b/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() \ No newline at end of file