diff --git a/src/rov_msgs/CMakeLists.txt b/src/rov_msgs/CMakeLists.txt index 422f3034..147c606a 100644 --- a/src/rov_msgs/CMakeLists.txt +++ b/src/rov_msgs/CMakeLists.txt @@ -38,6 +38,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/FloatData.msg" "msg/FloatCommand.msg" "msg/FloatSerial.msg" + "msg/FloatSingle.msg" "msg/PixhawkInstruction.msg" "srv/AutonomousFlight.srv" "srv/MissionTimerSet.srv" diff --git a/src/rov_msgs/msg/FloatSingle.msg b/src/rov_msgs/msg/FloatSingle.msg new file mode 100644 index 00000000..8f803ce3 --- /dev/null +++ b/src/rov_msgs/msg/FloatSingle.msg @@ -0,0 +1,4 @@ +uint8 team_number +uint32 time_ms +float32 pressure +float32 average_pressure \ No newline at end of file diff --git a/src/surface/gui/gui/widgets/float_comm.py b/src/surface/gui/gui/widgets/float_comm.py index d2cc3ca1..f14fa04a 100644 --- a/src/surface/gui/gui/widgets/float_comm.py +++ b/src/surface/gui/gui/widgets/float_comm.py @@ -6,7 +6,7 @@ QVBoxLayout, QWidget) from pyqtgraph import PlotWidget -from rov_msgs.msg import FloatCommand, FloatData, FloatSerial +from rov_msgs.msg import FloatCommand, FloatData, FloatSerial, FloatSingle class FloatComm(QWidget): @@ -14,6 +14,7 @@ class FloatComm(QWidget): handle_data_signal = pyqtSignal(FloatData) handle_serial_signal = pyqtSignal(FloatSerial) + handle_data_single_signal = pyqtSignal(FloatSingle) def __init__(self) -> None: super().__init__() @@ -23,26 +24,32 @@ def __init__(self) -> None: self.handle_data_signal.connect(self.handle_data) self.handle_serial_signal.connect(self.handle_serial) + self.handle_data_single_signal.connect(self.handle_single) GUIEventSubscriber(FloatData, "transceiver_data", self.handle_data_signal) GUIEventSubscriber(FloatSerial, "float_serial", self.handle_serial_signal) + GUIEventSubscriber(FloatSingle, "transceiver_single", self.handle_data_single_signal) command_pub = GUIEventPublisher(FloatCommand, "float_command") info_layout = QVBoxLayout() + single_layout = QHBoxLayout() + self.team_number = QLabel('Waiting for Team #') - self.profile_number = QLabel("Waiting for profile #") - self.profile_half = QLabel("Waiting for profile half") + self.time = QLabel("Waiting for Time") + self.pressure = QLabel("Waiting for Pressure") + self.average_pressure = QLabel("Waiting for Avg Pressure") - single_layout = QHBoxLayout() + single_layout.addWidget(self.team_number) + single_layout.addWidget(self.time) + single_layout.addWidget(self.pressure) + single_layout.addWidget(self.average_pressure) self.single_time = QLabel('Waiting for Time') self.single_pressure = QLabel('Waiting for Pressure') - single_layout.addWidget(self.team_number) - - left_side_layout = QVBoxLayout() + self.profile_number = QLabel("Waiting for profile #") + self.profile_half = QLabel("Waiting for profile half") - left_side_layout.addLayout(single_layout) info_layout.addWidget(self.profile_number) info_layout.addWidget(self.profile_half) @@ -88,7 +95,9 @@ def __init__(self) -> None: self.plots = [PlotWidget(), PlotWidget()] + left_side_layout = QVBoxLayout() left_side_layout.addLayout(info_and_buttons) + left_side_layout.addLayout(single_layout) left_side_layout.addWidget(self.console) layout.addLayout(left_side_layout) @@ -152,3 +161,10 @@ def handle_serial(self, msg: FloatSerial) -> None: """ self.console.moveCursor(QTextCursor.MoveOperation.End) self.console.insertPlainText(f'{msg.serial}\n') + + @pyqtSlot(FloatSingle) + def handle_single(self, msg: FloatSingle) -> None: + self.team_number.setText(f"Team #: {msg.team_number}") + self.time.setText(f"Time: {msg.time_ms} (ms)") + self.pressure.setText(f"Pressure: {msg.pressure}") + self.average_pressure.setText(f"Avg Pressure: {msg.average_pressure}") diff --git a/src/surface/transceiver/launch/serial_reader_launch.py b/src/surface/transceiver/launch/serial_reader_launch.py index 44685a6f..c4168c84 100644 --- a/src/surface/transceiver/launch/serial_reader_launch.py +++ b/src/surface/transceiver/launch/serial_reader_launch.py @@ -20,7 +20,8 @@ def generate_launch_description() -> LaunchDescription: output="screen", remappings=[("/surface/transceiver_data", "/surface/gui/transceiver_data"), ("/surface/float_command", "/surface/gui/float_command"), - ("/surface/float_serial", "/surface/gui/float_serial")] + ("/surface/float_serial", "/surface/gui/float_serial"), + ("/surface/transceiver_single", "/surface/gui/transceiver_single")] ) return LaunchDescription([ diff --git a/src/surface/transceiver/transceiver/serial_reader.py b/src/surface/transceiver/transceiver/serial_reader.py index 73d5cf47..e4320fc2 100644 --- a/src/surface/transceiver/transceiver/serial_reader.py +++ b/src/surface/transceiver/transceiver/serial_reader.py @@ -8,7 +8,7 @@ from serial import Serial from serial.serialutil import SerialException -from rov_msgs.msg import FloatCommand, FloatData, FloatSerial +from rov_msgs.msg import FloatCommand, FloatData, FloatSerial, FloatSingle MILLISECONDS_TO_SECONDS = 1/1000 SECONDS_TO_MINUTES = 1/60 @@ -36,6 +36,9 @@ def __init__(self) -> None: self.data_publisher = self.create_publisher(FloatData, 'transceiver_data', QoSPresetProfiles.SENSOR_DATA.value) + self.ros_single_publisher = self.create_publisher(FloatSingle, 'transceiver_single', + QoSPresetProfiles.SENSOR_DATA.value) + self.create_subscription(FloatCommand, 'float_command', self.send_command, QoSPresetProfiles.DEFAULT.value) @@ -75,7 +78,9 @@ def ros_publish(self, packet: str) -> None: try: if SerialReaderPacketHandler.is_ros_single_message(packet): - self.serial_packet_handler.handle_ros_single(packet) + single_msg = self.serial_packet_handler.handle_ros_single(packet) + if single_msg: + self.ros_single_publisher.publish(single_msg) else: msg = self.serial_packet_handler.message_parser(packet) self.data_publisher.publish(msg) @@ -94,19 +99,25 @@ def is_ros_single_message(packet: str) -> bool: ros_single = ROS_PACKET + "SINGLE" return packet[:len(ros_single)] == ros_single - def handle_ros_single(self, packet: str) -> None: - if self.surface_pressures.full(): - return - + def handle_ros_single(self, packet: str) -> FloatSingle | None: packet_sections = packet.split(SECTION_SEPARATOR) + team_number = packet_sections[2] data = packet_sections[3] + time_ms = int(data.split(COMMA_SEPARATOR)[0]) pressure = float(data.split(COMMA_SEPARATOR)[1]) + if self.surface_pressures.full(): + return FloatSingle(team_number=team_number, + time_ms=time_ms, + pressure=pressure, + average_pressure=self.surface_pressure) + self.surface_pressures.put(pressure) q = self.surface_pressures.queue avg_pressure = sum(q) / len(q) self.surface_pressure = avg_pressure + return None def message_parser(self, packet: str) -> FloatData: msg = FloatData()