Skip to content

Commit

Permalink
start float single
Browse files Browse the repository at this point in the history
  • Loading branch information
InvincibleRMC committed Jun 15, 2024
1 parent b1a1509 commit 8e6a88b
Show file tree
Hide file tree
Showing 5 changed files with 48 additions and 15 deletions.
1 change: 1 addition & 0 deletions src/rov_msgs/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down
4 changes: 4 additions & 0 deletions src/rov_msgs/msg/FloatSingle.msg
Original file line number Diff line number Diff line change
@@ -0,0 +1,4 @@
uint8 team_number
uint32 time_ms
float32 pressure
float32 average_pressure
32 changes: 24 additions & 8 deletions src/surface/gui/gui/widgets/float_comm.py
Original file line number Diff line number Diff line change
Expand Up @@ -6,14 +6,15 @@
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):
"""FloatComm widget for sending Float Communication Commands."""

handle_data_signal = pyqtSignal(FloatData)
handle_serial_signal = pyqtSignal(FloatSerial)
handle_data_single_signal = pyqtSignal(FloatSingle)

def __init__(self) -> None:
super().__init__()
Expand All @@ -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)

Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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}")
3 changes: 2 additions & 1 deletion src/surface/transceiver/launch/serial_reader_launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -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([
Expand Down
23 changes: 17 additions & 6 deletions src/surface/transceiver/transceiver/serial_reader.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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)

Expand Down Expand Up @@ -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)
Expand All @@ -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()
Expand Down

0 comments on commit 8e6a88b

Please sign in to comment.