Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat: added simple GUI to visualize AUV position and internal status #496

Merged
merged 12 commits into from
Nov 4, 2024
Empty file.
295 changes: 295 additions & 0 deletions mission/gui_auv/auv_gui/auv_gui.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,295 @@
import math
import sys
from threading import Thread
from typing import List, Optional

import matplotlib.pyplot as plt
import numpy as np
import rclpy
from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas
from nav_msgs.msg import Odometry
from PyQt6.QtCore import QTimer
from PyQt6.QtWidgets import (
QApplication,
QGridLayout,
QLabel,
QMainWindow,
QTabWidget,
QVBoxLayout,
QWidget,
)
from rclpy.executors import MultiThreadedExecutor
from rclpy.node import Node
from std_msgs.msg import Float32

# --- Quaternion to Euler angles ---


def quaternion_to_euler(x: float, y: float, z: float, w: float) -> List[float]:
"""
Convert a quaternion to Euler angles (roll, pitch, yaw).

Args:
x (float): The x component of the quaternion.
y (float): The y component of the quaternion.
z (float): The z component of the quaternion.
w (float): The w component of the quaternion.

Returns:
List[float]: A list of Euler angles [roll, pitch, yaw].
"""

# Roll (x-axis rotation)
sinr_cosp = 2 * (w * x + y * z)
cosr_cosp = 1 - 2 * (x * x + y * y)
roll = np.degrees(math.atan2(sinr_cosp, cosr_cosp))

# Pitch (y-axis rotation)
sinp = 2 * (w * y - z * x)
pitch = np.degrees(math.asin(sinp))

# Yaw (z-axis rotation)
siny_cosp = 2 * (w * z + x * y)
cosy_cosp = 1 - 2 * (y * y + z * z)
yaw = np.degrees(math.atan2(siny_cosp, cosy_cosp))

return [roll, pitch, yaw]


# --- GUI Node ---


class GuiNode(Node):
"""ROS2 Node that subscribes to odometry data and stores x, y positions."""

def __init__(self) -> None:
"""Initialize the GuiNode and set up the odometry subscriber."""
super().__init__("auv_gui_node")

# ROS2 parameters
self.declare_parameter("odom_topic", "/nucleus/odom")
self.declare_parameter("current_topic", "/auv/power_sense_module/current")
self.declare_parameter("voltage_topic", "/auv/power_sense_module/voltage")
self.declare_parameter("temperature_topic", "/auv/temperature")
self.declare_parameter("pressure_topic", "/auv/pressure")
self.declare_parameter("history_length", 30)

odom_topic = self.get_parameter("odom_topic").get_parameter_value().string_value
current_topic = self.get_parameter("current_topic").get_parameter_value().string_value
voltage_topic = self.get_parameter("voltage_topic").get_parameter_value().string_value
temperature_topic = self.get_parameter("temperature_topic").get_parameter_value().string_value
pressure_topic = self.get_parameter("pressure_topic").get_parameter_value().string_value

# Subscriber to the /nucleus/odom topic
self.subscription = self.create_subscription(Odometry, odom_topic, self.odom_callback, 10)

# Variables to store odometry data
self.xpos_data: List[float] = [] # x position
self.ypos_data: List[float] = [] # y position
self.zpos_data: List[float] = [] # z position

self.w_data: List[float] = [] # w component of the quaternion
self.x_data: List[float] = [] # x component of the quaternion
self.y_data: List[float] = [] # y component of the quaternion
self.z_data: List[float] = [] # z component of the quaternion

self.roll: Optional[float] = None
self.pitch: Optional[float] = None
self.yaw: Optional[float] = None

# Subscribe to internal status topics
self.current_subscriber = self.create_subscription(Float32, current_topic, self.current_callback, 5)
self.voltage_subscriber = self.create_subscription(Float32, voltage_topic, self.voltage_callback, 5)
self.temperature_subscriber = self.create_subscription(Float32, temperature_topic, self.temperature_callback, 5)
self.pressure_subscriber = self.create_subscription(Float32, pressure_topic, self.pressure_callback, 5)

# Variables for internal status
self.current = 0.0
self.voltage = 0.0
self.temperature = 0.0
self.pressure = 0.0

# --- Callback functions ---

def odom_callback(self, msg: Odometry) -> None:
"""Callback function that is triggered when an odometry message is received."""
# Extract x, y, z positions from the odometry message
x = msg.pose.pose.position.x
y = msg.pose.pose.position.y
z = -(msg.pose.pose.position.z)
self.xpos_data.append(x)
self.ypos_data.append(y)
self.zpos_data.append(z)

# Extract the quaternion components from the odometry message
w = msg.pose.pose.orientation.w
x = msg.pose.pose.orientation.x
y = msg.pose.pose.orientation.y
z = msg.pose.pose.orientation.z
self.roll, self.pitch, self.yaw = quaternion_to_euler(x, y, z, w)

# Limit the stored data for real-time plotting (avoid memory overflow)
max_data_points = self.get_parameter("history_length").get_parameter_value().integer_value
if len(self.x_data) > max_data_points:
self.xpos_data.pop(0)
self.ypos_data.pop(0)
self.zpos_data.pop(0)

def current_callback(self, msg: Float32) -> None:
"""Callback function that is triggered when a current message is received."""
self.current = msg.data

def voltage_callback(self, msg: Float32) -> None:
"""Callback function that is triggered when a voltage message is received."""
self.voltage = msg.data

def temperature_callback(self, msg: Float32) -> None:
"""Callback function that is triggered when a temperature message is received."""
self.temperature = msg.data

def pressure_callback(self, msg: Float32) -> None:
"""Callback function that is triggered when a pressure message is received."""
self.pressure = msg.data


# --- Plotting ---


class PlotCanvas(FigureCanvas):
"""A canvas widget for plotting odometry data using matplotlib."""

def __init__(self, gui_node: GuiNode, parent: Optional[QWidget] = None) -> None:
"""Initialize the PlotCanvas."""
# Set up the 3D plot
self.gui_node = gui_node
self.fig = plt.figure()
self.ax = self.fig.add_subplot(111, projection='3d')

# Initialize a red dot for the current position
(self.current_position_dot,) = self.ax.plot([], [], [], 'ro')

super().__init__(self.fig)
self.setParent(parent)

# Set labels and title for the 3D plot
self.ax.set_xlabel("X")
self.ax.set_ylabel("Y")
self.ax.set_zlabel("Z")
self.ax.set_title("Position")

# Initialize data lists for 3D plot
self.x_data: List[float] = []
self.y_data: List[float] = []
self.z_data: List[float] = []
(self.line,) = self.ax.plot([], [], [], 'b-')

def update_plot(self, x_data: List[float], y_data: List[float], z_data: List[float]) -> None:
"""Update the 3D plot with the latest odometry data."""
# Convert lists to numpy arrays to ensure compatibility with the plot functions
x_data = np.array(x_data, dtype=float)
y_data = np.array(y_data, dtype=float)
z_data = np.array(z_data, dtype=float)

# Check if the arrays are non-empty before updating the plot
if len(x_data) > 0 and len(y_data) > 0 and len(z_data) > 0:
self.line.set_data(x_data, y_data)
self.line.set_3d_properties(z_data)

# Update the current position dot
self.current_position_dot.set_data(x_data[-1:], y_data[-1:])
self.current_position_dot.set_3d_properties(z_data[-1:])

# Update the limits for the 3D plot around the latest data point
x_latest = x_data[-1]
y_latest = y_data[-1]
z_latest = z_data[-1]
margin = 2.5 # Define a margin around the latest point

self.ax.set_xlim(x_latest - margin, x_latest + margin)
self.ax.set_ylim(y_latest - margin, y_latest + margin)
self.ax.set_zlim(z_latest - margin, z_latest + margin)

self.fig.canvas.draw()
self.fig.canvas.flush_events()


def run_ros_node(ros_node: GuiNode, executor: MultiThreadedExecutor) -> None:
"""Run the ROS2 node in a separate thread using a MultiThreadedExecutor."""
rclpy.spin(ros_node, executor)


def main(args: Optional[List[str]] = None) -> None:
"""The main function to initialize ROS2 and the GUI application."""
rclpy.init(args=args)
ros_node = GuiNode()
executor = MultiThreadedExecutor()

# Run ROS in a separate thread
ros_thread = Thread(target=run_ros_node, args=(ros_node, executor), daemon=True)
ros_thread.start()

# Setup the PyQt5 application and window
app = QApplication(sys.argv)
gui = QMainWindow()
gui.setWindowTitle("Vortex GUI")
gui.setGeometry(100, 100, 600, 400)

# Create the tab widget
tabs = QTabWidget()
tabs.setTabPosition(QTabWidget.TabPosition.North)
tabs.setMovable(True)

# --- Position Tab ---
position_widget = QWidget()
layout = QGridLayout(position_widget) # grid layout

plot_canvas = PlotCanvas(ros_node, position_widget)
layout.addWidget(plot_canvas, 0, 0)

current_pos = QLabel(parent=position_widget)
layout.addWidget(current_pos, 0, 1)

tabs.addTab(position_widget, "Position")

# --- Internal Status Tab ---
internal_widget = QWidget()
internal_layout = QVBoxLayout(internal_widget)

internal_status_label = QLabel(parent=internal_widget)
internal_layout.addWidget(internal_status_label)
tabs.addTab(internal_widget, "Internal")

gui.setCentralWidget(tabs)
gui.showMaximized()

# Use a QTimer to update plot, current position, and internal status in the main thread
def update_gui() -> None:
plot_canvas.update_plot(ros_node.xpos_data, ros_node.ypos_data, ros_node.zpos_data)
if len(ros_node.xpos_data) > 0 and ros_node.roll is not None:
position_text = f"Current Position:\nX: {ros_node.xpos_data[-1]:.2f}\nY: {ros_node.ypos_data[-1]:.2f}\nZ: {ros_node.zpos_data[-1]:.2f}"
orientation_text = f"Current Orientation:\nRoll: {ros_node.roll:.2f}\nPitch: {ros_node.pitch:.2f}\nYaw: {ros_node.yaw:.2f}"
current_pos.setText(position_text + "\n\n\n" + orientation_text)

# Update internal status
internal_status_label.setText(
f"Internal Status:\n"
f"Current: {ros_node.current:.2f}\n"
f"Voltage: {ros_node.voltage:.2f}\n"
f"Temperature: {ros_node.temperature:.2f}\n"
f"Pressure: {ros_node.pressure:.2f}"
)

# Set up the timer to call update_gui every 100ms
timer = QTimer()
timer.timeout.connect(update_gui)
timer.start(100)

app.exec()

ros_node.destroy_node()
rclpy.shutdown()
sys.exit()


if __name__ == '__main__':
main()
9 changes: 9 additions & 0 deletions mission/gui_auv/config/gui_params.yaml
Original file line number Diff line number Diff line change
@@ -0,0 +1,9 @@
/**:
ros__parameters:
gui_auv:
odom_topic: "/nucleus/odom"
current_topic: "/auv/power_sense_module/current"
voltage_topic: "/auv/power_sense_module/voltage"
temperature_topic: "/auv/temperature"
pressure_topic: "/auv/pressure"
history_length: 30 # seconds
19 changes: 19 additions & 0 deletions mission/gui_auv/package.xml
Original file line number Diff line number Diff line change
@@ -0,0 +1,19 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>auv-gui</name>
<version>0.0.0</version>
<description>AUV GUI for ROS2</description>
<maintainer email="[email protected]">sondre</maintainer>
<license>MIT</license>

<test_depend>python3-pytest</test_depend>

<exec_depend>rclpy</exec_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>sensor_msgs</exec_depend>

<export>
<build_type>ament_python</build_type>
</export>
</package>
21 changes: 21 additions & 0 deletions mission/gui_auv/setup.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,21 @@
from setuptools import setup

PACKAGE_NAME = 'auv_gui'

setup(
name=PACKAGE_NAME,
version='0.0.0',
packages=[PACKAGE_NAME],
install_requires=['setuptools'],
zip_safe=True,
maintainer='sondre',
maintainer_email='[email protected]',
description='AUV GUI for ROS2',
license='MIT',
tests_require=['pytest'],
entry_points={
'console_scripts': [
'auv-gui = auv_gui.auv_gui:main',
],
},
)