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/.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_bringup/launch/base.py b/src/moa/moa_bringup/launch/base.py
index d9d11005..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(
@@ -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'),
- ])
\ 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 47a69a14..ad5a35db 100644
--- a/src/moa/moa_controllers/moa_controllers/ack_to_can.py
+++ b/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:
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..0609f4ac
--- /dev/null
+++ b/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()
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
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/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',
],
},
)
diff --git a/src/moa/moa_controllers/test/test_ackermann.py b/src/moa/moa_controllers/test/test_ackermann.py
index af6a9636..99296e58 100644
--- a/src/moa/moa_controllers/test/test_ackermann.py
+++ b/src/moa/moa_controllers/test/test_ackermann.py
@@ -1,14 +1,13 @@
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
-@pytest.fixture(autouse=True, scope='session')
+@pytest.fixture(autouse=True, scope='module')
def node():
rclpy.init(args=None)
test_ack = ack_to_can()
@@ -17,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()
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..fb5f40e0
--- /dev/null
+++ b/src/moa/moa_controllers/test/test_sys_status.py
@@ -0,0 +1,125 @@
+import pytest
+import rclpy
+from moa_controllers.sys_status import as_status
+from moa_msgs.msg import HardwareStatesStamped, MissionStatesStamped
+from ackermann_msgs.msg import AckermannDriveStamped
+
+
+@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_state_0(node: as_status):
+ node.hw_states['ebs_active'] = True
+ node.mission_finished = True
+ node.car_stopped = True
+
+ assert node.update_state(prod=0) == 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(prod=0) == 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(prod=0) == 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(prod=0) == 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(prod=0) == 4
+
+
+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.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.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()
+
+ 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
diff --git a/src/moa/moa_msgs/CMakeLists.txt b/src/moa/moa_msgs/CMakeLists.txt
index ab667214..9d4a5e74 100644
--- a/src/moa/moa_msgs/CMakeLists.txt
+++ b/src/moa/moa_msgs/CMakeLists.txt
@@ -20,6 +20,10 @@ set(msg_files
"msg/ConeStamped.msg"
"msg/ConeMap.msg"
"msg/ConeMapStamped.msg"
+ "msg/HardwareStates.msg"
+ "msg/HardwareStatesStamped.msg"
+ "msg/MissionStates.msg"
+ "msg/MissionStatesStamped.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..412b20ff
--- /dev/null
+++ b/src/moa/moa_msgs/msg/HardwareStates.msg
@@ -0,0 +1,6 @@
+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
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
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
new file mode 100644
index 00000000..11f709e2
--- /dev/null
+++ b/src/moa/moa_msgs/msg/MissionStatesStamped.msg
@@ -0,0 +1,5 @@
+std_msgs/Header header
+
+MissionStates mission_states
+
+
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