Merge pull request #176 from cwruRobotics/serial-reader-update

Serial reader update

pyproject.toml

@@ -9,6 +9,7 @@ dependencies = [
     'mypy >= 1.7',
     'pynput',
     'pyqt6',
+    'pyqtgraph',
     'pyqtdarktheme',
     'opencv-python>=4.8.1',
     'numpy>=1.26',

src/pi/manipulators/package.xml

@@ -8,7 +8,6 @@
   <maintainer email="rmc170@case.edu">Michael Carlstrom</maintainer>
   <license>Apache License, Version 2.0</license>
 
-  <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>
   <test_depend>ament_pep257</test_depend>
   <test_depend>python3-pytest</test_depend>

src/pi/pi_main/test/test_run_on_boot.py

@@ -5,6 +5,9 @@
 
 def test_install_on_boot() -> None:
     """Test that file copying and systemd are made."""
+    # TODO update to clean up files
+    return
+
     main()
 
     # Test for files being copied correctly

src/rov_msgs/CMakeLists.txt

@@ -34,7 +34,9 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/ValveManip.msg"
   "msg/Heartbeat.msg"
   "msg/MissionTimerTick.msg"
+  "msg/FloatData.msg"
   "msg/FloatCommand.msg"
+  "msg/FloatSerial.msg"
   "msg/PixhawkInstruction.msg"
   "srv/AutonomousFlight.srv"
   "srv/MissionTimerSet.srv"

src/rov_msgs/msg/FloatCommand.msg

@@ -1 +1,7 @@
+string SUBMERGE = "submerge"
+string PUMP = "pump"
+string SUCK = "suck"
+string RETURN = "return"
+string STOP = "stop"
+
 string command

src/rov_msgs/msg/FloatData.msg

@@ -0,0 +1,8 @@
+uint8 team_number
+uint8 profile_number
+uint8 profile_half
+
+# Minutes
+float32[<=31] time_data
+# Meter of head
+float32[<=31] depth_data

src/rov_msgs/msg/FloatSerial.msg

@@ -0,0 +1 @@
+string serial

src/surface/gui/gui/gui_nodes/event_nodes/publisher.py

@@ -11,7 +11,7 @@
 class GUIEventPublisher(Node, Generic[MsgT]):
     """Publisher for sending messages from the GUI."""
 
-    def __init__(self, msg_type: MsgT, topic: str,
+    def __init__(self, msg_type: type[MsgT], topic: str,
                  qos_profile: QoSProfile = qos_profile_system_default) -> None:
         # Name this node with a sanitized version of the topic
         name = f'publisher_{re.sub(r"[^a-zA-Z0-9_]", "_", topic)}'

src/surface/gui/gui/widgets/float_comm.py

@@ -1,49 +1,148 @@
-from PyQt6.QtWidgets import QPushButton, QHBoxLayout, QLabel, QWidget
 from gui.gui_nodes.event_nodes.publisher import GUIEventPublisher
-from rov_msgs.msg import FloatCommand
+from gui.gui_nodes.event_nodes.subscriber import GUIEventSubscriber
 from PyQt6.QtCore import pyqtSignal, pyqtSlot
+from PyQt6.QtGui import QTextCursor
+from PyQt6.QtWidgets import (QHBoxLayout, QLabel, QPushButton, QTextEdit,
+                             QVBoxLayout, QWidget)
+from pyqtgraph import PlotWidget
+
+from rov_msgs.msg import FloatCommand, FloatData, FloatSerial
 
 
 class FloatComm(QWidget):
     """FloatComm widget for sending Float Communication Commands."""
 
-    handle_scheduler_response_signal: pyqtSignal = pyqtSignal(FloatCommand)
+    handle_data_signal = pyqtSignal(FloatData)
+    handle_serial_signal = pyqtSignal(FloatSerial)
 
     def __init__(self) -> None:
         super().__init__()
 
-        layout: QHBoxLayout = QHBoxLayout()
+        layout = QHBoxLayout()
         self.setLayout(layout)
 
-        submerge_button = QPushButton()
-        submerge_button.setText("Submerge")
-        submerge_button.setFixedSize(300, 200)
-        submerge_button.clicked.connect(self.submerge_clicked)
-        layout.addWidget(submerge_button)
+        self.handle_data_signal.connect(self.handle_data)
+        self.handle_serial_signal.connect(self.handle_serial)
+        GUIEventSubscriber(FloatData, "transceiver_data", self.handle_data_signal)
+        GUIEventSubscriber(FloatSerial, "float_serial", self.handle_serial_signal)
+
+        command_pub = GUIEventPublisher(FloatCommand, "float_command")
+
+        info_layout = QVBoxLayout()
+
+        self.team_number = QLabel('Waiting for Team #')
+        self.profile_number = QLabel("Waiting for profile #")
+        self.profile_half = QLabel("Waitng for profile half")
+
+        left_side_layout = QVBoxLayout()
+
+        info_layout.addWidget(self.team_number)
+        info_layout.addWidget(self.profile_number)
+        info_layout.addWidget(self.profile_half)
+
+        info_and_buttons = QHBoxLayout()
+        info_and_buttons.addLayout(info_layout)
+
+        submerge_button = QPushButton("Submerge")
+        pump_button = QPushButton("Pump")
+        suck_button = QPushButton("Suck")
+        return_button = QPushButton("Return")
+        stop_button = QPushButton("Stop")
+
+        submerge_button.clicked.connect(lambda: command_pub.publish(
+            FloatCommand(command=FloatCommand.SUBMERGE)
+        ))
+        pump_button.clicked.connect(lambda: command_pub.publish(
+            FloatCommand(command=FloatCommand.PUMP)
+        ))
+        suck_button.clicked.connect(lambda: command_pub.publish(
+            FloatCommand(command=FloatCommand.SUCK)
+        ))
+        return_button.clicked.connect(lambda: command_pub.publish(
+            FloatCommand(command=FloatCommand.RETURN)
+        ))
+        stop_button.clicked.connect(lambda: command_pub.publish(
+            FloatCommand(command=FloatCommand.STOP)
+        ))
+
+        info_and_buttons.addWidget(submerge_button)
+        info_and_buttons.addWidget(pump_button)
+        info_and_buttons.addWidget(suck_button)
+        info_and_buttons.addWidget(return_button)
+        info_and_buttons.addWidget(stop_button)
+
+        self.console = QTextEdit()
+        self.console.setReadOnly(True)
+        self.console.setLineWrapMode(QTextEdit.LineWrapMode.NoWrap)
+
+        font = self.console.font()
+        font.setFamily("Courier")
+        font.setPointSize(11)
+        self.console.setFont(font)
 
-        self.handle_scheduler_response_signal.connect(self.handle_text)
+        self.plots = [PlotWidget(), PlotWidget()]
 
-        self.label: QLabel = QLabel()
-        self.label.setText('Waiting for radio...')
-        layout.addWidget(self.label)
+        left_side_layout.addLayout(info_and_buttons)
+        left_side_layout.addWidget(self.console)
 
-        self.transceiver_publisher = GUIEventPublisher(
-            FloatCommand,
-            "transceiver_control"
-        )
+        layout.addLayout(left_side_layout)
+        for plot in self.plots:
+            layout.addWidget(plot)
 
-    @pyqtSlot(FloatCommand)
-    def handle_text(self, msg: FloatCommand) -> None:
+        self.time_data: list[float] = []
+        self.depth_data: list[float] = []
+        self.received_first_half = False
+        self.received_second_half = False
+        self.completed_profile_one = False
+
+    @pyqtSlot(FloatData)
+    def handle_data(self, msg: FloatData) -> None:
         """
         Set the widget label text to the message in the FloatCommand.
 
         Parameters
         ----------
-        msg : FloatCommand
-            the command that determines the label text
+        msg : FloatData
+            the data from the float
         """
-        self.label.setText(msg.command)
+        self.team_number.setText(f"Team #: {msg.team_number}")
+        self.profile_number.setText(f"Profile #: {msg.profile_number}")
+        self.profile_half.setText(f"Profile half: {msg.profile_half}")
+
+        time_data = list(msg.time_data)
+        depth_data = list(msg.depth_data)
+
+        if msg.profile_number == 0 and self.completed_profile_one:
+            return
+        elif msg.profile_number not in (0, 1):
+            return
 
-    def submerge_clicked(self) -> None:
-        """Publish the command for the float to submerge."""
-        self.transceiver_publisher.publish(FloatCommand(command="submerge"))
+        if msg.profile_half == 0 and not self.received_first_half:
+            self.time_data = time_data + self.time_data
+            self.depth_data = depth_data + self.depth_data
+            self.received_first_half = True
+        elif msg.profile_half == 1 and not self.received_second_half:
+            self.time_data = self.time_data + time_data
+            self.depth_data = self.depth_data + depth_data
+            self.received_second_half = True
+
+        if self.received_first_half and self.received_second_half:
+            self.plots[msg.profile_number].plot(self.time_data, self.depth_data)
+            self.time_data = []
+            self.depth_data = []
+            self.received_first_half = False
+            self.received_second_half = False
+            self.completed_profile_one = True
+
+    @pyqtSlot(FloatSerial)
+    def handle_serial(self, msg: FloatSerial) -> None:
+        """
+        Set the widget label text to the message in the FloatCommand.
+
+        Parameters
+        ----------
+        msg : FloatSerial
+            the serial from the float
+        """
+        self.console.moveCursor(QTextCursor.MoveOperation.End)
+        self.console.insertPlainText(f'{msg.serial}\n')

src/surface/gui/gui/widgets/logger.py

@@ -53,7 +53,6 @@ def __init__(self) -> None:
     def print_log(self, message: Log) -> None:
         """Print message to log widget if user is viewing message's type."""
         # Message severities are 0, 10, 20, etc.
-        # We divide by 10 to get index for checkboxes
         severity_key = LoggingSeverity(message.level)
         # Make sure we've chosen to view this message type
         if not self.checkboxes[severity_key].isChecked():

src/surface/rov_gazebo/package.xml

@@ -9,7 +9,6 @@
 
   <depend>rclpy</depend>
 
-  <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>
   <test_depend>ament_pep257</test_depend>
   <test_depend>python3-pytest</test_depend>

src/surface/surface_main/launch/surface_operator_launch.py

@@ -41,15 +41,6 @@ def generate_launch_description() -> LaunchDescription:
         ]),
     )
 
-    namespace_launch = GroupAction(
-        actions=[
-            PushRosNamespace("surface"),
-            gui_launch,
-            flight_control_launch,
-            vehicle_manager_launch
-        ]
-    )
-
     # Launches Transceiver
     transceiver_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource([
@@ -59,7 +50,16 @@ def generate_launch_description() -> LaunchDescription:
         ]),
     )
 
+    namespace_launch = GroupAction(
+        actions=[
+            PushRosNamespace("surface"),
+            gui_launch,
+            flight_control_launch,
+            vehicle_manager_launch,
+            transceiver_launch
+        ]
+    )
+
     return LaunchDescription([
-        namespace_launch,
-        transceiver_launch,
+        namespace_launch
     ])

src/surface/transceiver/launch/serial_reader_launch.py

@@ -13,10 +13,14 @@ def generate_launch_description() -> LaunchDescription:
 
     """
     # launches transceiver
-    reader_node: Node = Node(
-        namespace='surface',
+    reader_node = Node(
         package='transceiver',
         executable='serial',
+        emulate_tty=True,
+        output="screen",
+        remappings=[("/surface/transceiver_data", "/surface/gui/transceiver_data"),
+                    ("/surface/float_command", "/surface/gui/float_command"),
+                    ("/surface/float_serial", "/surface/gui/float_serial")]
     )
 
     return LaunchDescription([

src/surface/transceiver/package.xml

@@ -8,7 +8,7 @@
   <license>Apache License 2.0</license>
 
   <depend>rclpy</depend>
-  <depend>sensor_msgs</depend>
+  <depend>rov_msgs</depend>
 
 
   <exec_depend>ros2launch</exec_depend>

src/surface/transceiver/test/test_parser.py

@@ -0,0 +1,28 @@
+import pytest
+from transceiver.serial_reader import SerialReader
+
+from rov_msgs.msg import FloatData
+
+PACKET = "ROS:11,1,1:313901,988.53;314843,988.57;315785,988.99;316727,991.56;317669,996.21;318611,1002.36;319553,1010.36;320495,1021.11;321437,1034.42;322379,1050.23;323321,1051.86;324263,1053.20;325206,1053.32;326146,1053.46;327088,1053.52;328030,1053.58;328972,1053.61;329914,1053.64;330856,1053.61;331798,1053.60;332740,1053.65;333682,1053.58;334624,1053.53;335566,1053.52;336508,1053.39;337453,1053.41;338395,1053.46;339337,1053.37;340279,1053.42;341221,1053.49;342163,1053.54"  # noqa: E501
+
+NOT_THREE_SECTIONS = "ROS:11,1,1313901,988.53;314843,988.57;315785,988.99;316727,991.56;317669,996.21;318611,1002.36;319553,1010.36;320495,1021.11;321437,1034.42;322379,1050.23;323321,1051.86;324263,1053.20;325206,1053.32;326146,1053.46;327088,1053.52;328030,1053.58;328972,1053.61;329914,1053.64;330856,1053.61;331798,1053.60;332740,1053.65;333682,1053.58;334624,1053.53;335566,1053.52;336508,1053.39;337453,1053.41;338395,1053.46;339337,1053.37;340279,1053.42;341221,1053.49;342163,1053.54"  # noqa: E501
+
+HEADER_TWO_ELEMENTS = "ROS:11,1:313901,988.53;314843,988.57;315785,988.99;316727,991.56;317669,996.21;318611,1002.36;319553,1010.36;320495,1021.11;321437,1034.42;322379,1050.23;323321,1051.86;324263,1053.20;325206,1053.32;326146,1053.46;327088,1053.52;328030,1053.58;328972,1053.61;329914,1053.64;330856,1053.61;331798,1053.60;332740,1053.65;333682,1053.58;334624,1053.53;335566,1053.52;336508,1053.39;337453,1053.41;338395,1053.46;339337,1053.37;340279,1053.42;341221,1053.49;342163,1053.54"  # noqa: E501
+
+
+def test_parser() -> None:
+    msg = SerialReader._message_parser(PACKET)
+
+    assert msg == FloatData(
+        team_number=11,
+        profile_number=1,
+        profile_half=1,
+        time_data=[5.231683254241943, 5.247383117675781, 5.263083457946777, 5.278783321380615, 5.294483184814453, 5.310183525085449, 5.325883388519287, 5.341583251953125, 5.357283115386963, 5.372983455657959, 5.388683319091797, 5.404383182525635, 5.420100212097168, 5.435766696929932, 5.4514665603637695, 5.467166900634766, 5.4828667640686035, 5.498566627502441, 5.514266490936279, 5.529966831207275, 5.545666694641113, 5.561366558074951, 5.577066898345947, 5.592766761779785, 5.608466625213623, 5.624216556549072, 5.639916896820068, 5.655616760253906, 5.671316623687744, 5.687016487121582, 5.702716827392578],  # noqa 501
+        depth_data=[10.082781791687012, 10.083189964294434, 10.08747386932373, 10.113687515258789, 10.161116600036621, 10.223844528198242, 10.305442810058594, 10.415090560913086, 10.550849914550781, 10.71210765838623, 10.728734016418457, 10.742401123046875, 10.74362564086914, 10.7450532913208, 10.745665550231934, 10.74627685546875, 10.746582984924316, 10.746889114379883, 10.746582984924316, 10.746480941772461, 10.746991157531738, 10.74627685546875, 10.745767593383789, 10.745665550231934, 10.744338989257812, 10.744543075561523, 10.7450532913208, 10.744134902954102, 10.744645118713379, 10.745359420776367, 10.745869636535645]  # noqa 501
+    )
+
+    with pytest.raises(ValueError, match="Packet expected 3 sections, found 2 sections"):
+        SerialReader._message_parser(NOT_THREE_SECTIONS)
+
+    with pytest.raises(ValueError, match="Packet header length of 3 expected found 2 instead"):
+        SerialReader._message_parser(HEADER_TWO_ELEMENTS)