From c6b8d9cb1730fb0acd0ed0902388a4f213ce2724 Mon Sep 17 00:00:00 2001 From: Sondre Haugen Date: Sun, 13 Oct 2024 13:36:23 +0200 Subject: [PATCH 01/12] feat: simple GUI --- mission/gui_auv/auv_gui/__init__.py | 0 mission/gui_auv/auv_gui/auv_gui.py | 133 ++++++++++++++++++++++++++++ mission/gui_auv/package.xml | 19 ++++ mission/gui_auv/setup.py | 21 +++++ 4 files changed, 173 insertions(+) create mode 100644 mission/gui_auv/auv_gui/__init__.py create mode 100644 mission/gui_auv/auv_gui/auv_gui.py create mode 100644 mission/gui_auv/package.xml create mode 100644 mission/gui_auv/setup.py diff --git a/mission/gui_auv/auv_gui/__init__.py b/mission/gui_auv/auv_gui/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/mission/gui_auv/auv_gui/auv_gui.py b/mission/gui_auv/auv_gui/auv_gui.py new file mode 100644 index 000000000..52acd349f --- /dev/null +++ b/mission/gui_auv/auv_gui/auv_gui.py @@ -0,0 +1,133 @@ +import rclpy +from rclpy.node import Node +from nav_msgs.msg import Odometry +from PyQt5.QtWidgets import QApplication, QMainWindow, QPushButton, QVBoxLayout, QWidget +from PyQt5.QtCore import QTimer +import sys +import matplotlib.pyplot as plt +from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas +from threading import Thread +from rclpy.executors import MultiThreadedExecutor + +class GuiNode(Node): + def __init__(self): + super().__init__("simple_gui_node") + + # Subscriber to the /nucleus/odom topic + self.subscription = self.create_subscription(Odometry, '/nucleus/odom', self.odom_callback, 10) + + # Variables to store odometry data + self.x_data = [] + self.y_data = [] + + self.counter_ = 0 + self.get_logger().info("Subscribed to /nucleus/odom") + + def odom_callback(self, msg): + # Extract x and y positions from the odometry message + x = msg.pose.pose.position.x + y = msg.pose.pose.position.y + self.x_data.append(x) + self.y_data.append(y) + + # self.get_logger().info(f"Received odometry: x={x}, y={y}") + + # Limit the stored data for real-time plotting (avoid memory overflow) + # Store 30 seconds worth of data at 100Hz + max_data_points = 30 * 100 # 30 seconds * 100 Hz + if len(self.x_data) > max_data_points: + self.x_data.pop(0) + self.y_data.pop(0) + +class PlotCanvas(FigureCanvas): + def __init__(self, gui_node, parent=None): + self.gui_node = gui_node # Store a reference to the ROS node + self.fig, self.ax = plt.subplots() + super().__init__(self.fig) + self.setParent(parent) + + self.ax.set_xlabel("X Position") + self.ax.set_ylabel("Y Position") + self.ax.set_title("Odometry") + + self.x_data = [] + self.y_data = [] + self.line, = self.ax.plot([], [], 'b-') + + # Mouse click event handler + self.mpl_connect("button_press_event", self.on_click) + + def on_click(self, event): + """Handle mouse click event on the plot.""" + if event.inaxes is not None: + x_click = event.xdata + y_click = event.ydata + # Use the logger from the ROS node to log the click event + self.gui_node.get_logger().info(f"Clicked at: x={x_click}, y={y_click}") + + def update_plot(self, x_data, y_data): + """Update the plot with the latest odometry data.""" + self.line.set_data(x_data, y_data) # Update the data of the line object + + if x_data and y_data: + x_latest = x_data[-1] + y_latest = y_data[-1] + margin = 1 # 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.draw() + +def run_ros_node(ros_node, executor): + rclpy.spin(ros_node, executor) + +def main(args=None): + 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("AUV Odometry GUI") + gui.setGeometry(100, 100, 600, 400) + + # Create a central widget and layout for the GUI + central_widget = QWidget() + layout = QVBoxLayout(central_widget) + + plot_canvas = PlotCanvas(ros_node, central_widget) + layout.addWidget(plot_canvas) + + # Add buttons or other GUI elements if needed (optional) + button1 = QPushButton("Send command", parent=central_widget) + button1.clicked.connect(lambda: ros_node.get_logger().info("Command sent")) # Clears the plot when pressed + layout.addWidget(button1) + + gui.setCentralWidget(central_widget) + gui.show() + + # Use a QTimer to update the plot in the main thread + def update_plot(): + plot_canvas.update_plot(ros_node.x_data, ros_node.y_data) + + # Set up the timer to call update_plot every 100ms + timer = QTimer() + timer.timeout.connect(update_plot) + timer.start(100) # 100 ms interval + + sys.exit(app.exec()) + + # Clean up + ros_node.get_logger().info("Shutting down") + ros_node.destroy_node() + executor.shutdown() + ros_thread.join() + +if __name__ == '__main__': + main() diff --git a/mission/gui_auv/package.xml b/mission/gui_auv/package.xml new file mode 100644 index 000000000..dd89ae4cd --- /dev/null +++ b/mission/gui_auv/package.xml @@ -0,0 +1,19 @@ + + + + auv-gui + 0.0.0 + AUV GUI for ROS2 + sondre + MIT + + python3-pytest + + rclpy + geometry_msgs + sensor_msgs + + + ament_python + + diff --git a/mission/gui_auv/setup.py b/mission/gui_auv/setup.py new file mode 100644 index 000000000..f023139ae --- /dev/null +++ b/mission/gui_auv/setup.py @@ -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='sondre@example.com', + description='AUV GUI for ROS2', + license='MIT', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'auv-gui = auv_gui.auv_gui:main', + ], + }, +) From b177cfa631a16464494cc3e20c355205fdb34b31 Mon Sep 17 00:00:00 2001 From: Sondre Haugen Date: Sun, 13 Oct 2024 13:45:49 +0200 Subject: [PATCH 02/12] refactor: linting fixes --- mission/gui_auv/auv_gui/auv_gui.py | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/mission/gui_auv/auv_gui/auv_gui.py b/mission/gui_auv/auv_gui/auv_gui.py index 52acd349f..411655afb 100644 --- a/mission/gui_auv/auv_gui/auv_gui.py +++ b/mission/gui_auv/auv_gui/auv_gui.py @@ -1,25 +1,25 @@ +import sys +from threading import Thread +import matplotlib.pyplot as plt import rclpy from rclpy.node import Node +from rclpy.executors import MultiThreadedExecutor from nav_msgs.msg import Odometry from PyQt5.QtWidgets import QApplication, QMainWindow, QPushButton, QVBoxLayout, QWidget from PyQt5.QtCore import QTimer -import sys -import matplotlib.pyplot as plt from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas -from threading import Thread -from rclpy.executors import MultiThreadedExecutor class GuiNode(Node): def __init__(self): super().__init__("simple_gui_node") - + # Subscriber to the /nucleus/odom topic self.subscription = self.create_subscription(Odometry, '/nucleus/odom', self.odom_callback, 10) - + # Variables to store odometry data self.x_data = [] self.y_data = [] - + self.counter_ = 0 self.get_logger().info("Subscribed to /nucleus/odom") @@ -31,7 +31,7 @@ def odom_callback(self, msg): self.y_data.append(y) # self.get_logger().info(f"Received odometry: x={x}, y={y}") - + # Limit the stored data for real-time plotting (avoid memory overflow) # Store 30 seconds worth of data at 100Hz max_data_points = 30 * 100 # 30 seconds * 100 Hz @@ -100,7 +100,7 @@ def main(args=None): # Create a central widget and layout for the GUI central_widget = QWidget() layout = QVBoxLayout(central_widget) - + plot_canvas = PlotCanvas(ros_node, central_widget) layout.addWidget(plot_canvas) From 1e4f198f0f3781465b8993b02e1879f4bafcf578 Mon Sep 17 00:00:00 2001 From: Sondre Haugen Date: Sun, 13 Oct 2024 14:04:46 +0200 Subject: [PATCH 03/12] refactor: pre-commit fixes, passes all --- mission/gui_auv/auv_gui/auv_gui.py | 86 ++++++++++++++++++++---------- mission/gui_auv/setup.py | 8 +-- 2 files changed, 63 insertions(+), 31 deletions(-) diff --git a/mission/gui_auv/auv_gui/auv_gui.py b/mission/gui_auv/auv_gui/auv_gui.py index 411655afb..01a8cb3be 100644 --- a/mission/gui_auv/auv_gui/auv_gui.py +++ b/mission/gui_auv/auv_gui/auv_gui.py @@ -1,37 +1,43 @@ import sys from threading import Thread +from typing import List, Optional + import matplotlib.pyplot as plt import rclpy -from rclpy.node import Node -from rclpy.executors import MultiThreadedExecutor +from matplotlib.backend_bases import MouseEvent +from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas from nav_msgs.msg import Odometry -from PyQt5.QtWidgets import QApplication, QMainWindow, QPushButton, QVBoxLayout, QWidget from PyQt5.QtCore import QTimer -from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas +from PyQt5.QtWidgets import QApplication, QMainWindow, QPushButton, QVBoxLayout, QWidget +from rclpy.executors import MultiThreadedExecutor +from rclpy.node import Node + class GuiNode(Node): - def __init__(self): + """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__("simple_gui_node") # Subscriber to the /nucleus/odom topic self.subscription = self.create_subscription(Odometry, '/nucleus/odom', self.odom_callback, 10) # Variables to store odometry data - self.x_data = [] - self.y_data = [] + self.x_data: List[float] = [] + self.y_data: List[float] = [] self.counter_ = 0 self.get_logger().info("Subscribed to /nucleus/odom") - def odom_callback(self, msg): + def odom_callback(self, msg: Odometry) -> None: + """Callback function that is triggered when an odometry message is received.""" # Extract x and y positions from the odometry message x = msg.pose.pose.position.x y = msg.pose.pose.position.y self.x_data.append(x) self.y_data.append(y) - # self.get_logger().info(f"Received odometry: x={x}, y={y}") - # Limit the stored data for real-time plotting (avoid memory overflow) # Store 30 seconds worth of data at 100Hz max_data_points = 30 * 100 # 30 seconds * 100 Hz @@ -39,8 +45,18 @@ def odom_callback(self, msg): self.x_data.pop(0) self.y_data.pop(0) + class PlotCanvas(FigureCanvas): - def __init__(self, gui_node, parent=None): + """A canvas widget for plotting odometry data using matplotlib.""" + + def __init__(self, gui_node: GuiNode, parent: Optional[QWidget] = None) -> None: + """ + Initialize the PlotCanvas with a reference to the ROS node and configure the plot. + + Args: + gui_node (GuiNode): A reference to the ROS node. + parent (Optional[QWidget]): The parent widget (default: None). + """ self.gui_node = gui_node # Store a reference to the ROS node self.fig, self.ax = plt.subplots() super().__init__(self.fig) @@ -50,23 +66,34 @@ def __init__(self, gui_node, parent=None): self.ax.set_ylabel("Y Position") self.ax.set_title("Odometry") - self.x_data = [] - self.y_data = [] - self.line, = self.ax.plot([], [], 'b-') + self.x_data: List[float] = [] + self.y_data: List[float] = [] + (self.line,) = self.ax.plot([], [], 'b-') # Mouse click event handler self.mpl_connect("button_press_event", self.on_click) - def on_click(self, event): - """Handle mouse click event on the plot.""" + def on_click(self, event: MouseEvent) -> None: + """ + Handle mouse click event on the plot and log the clicked coordinates. + + Args: + event: The mouse click event from matplotlib. + """ if event.inaxes is not None: x_click = event.xdata y_click = event.ydata # Use the logger from the ROS node to log the click event self.gui_node.get_logger().info(f"Clicked at: x={x_click}, y={y_click}") - def update_plot(self, x_data, y_data): - """Update the plot with the latest odometry data.""" + def update_plot(self, x_data: List[float], y_data: List[float]) -> None: + """ + Update the plot with the latest odometry data. + + Args: + x_data (List[float]): The list of x positions. + y_data (List[float]): The list of y positions. + """ self.line.set_data(x_data, y_data) # Update the data of the line object if x_data and y_data: @@ -79,10 +106,20 @@ def update_plot(self, x_data, y_data): self.draw() -def run_ros_node(ros_node, executor): + +def run_ros_node(ros_node: GuiNode, executor: MultiThreadedExecutor) -> None: + """ + Run the ROS2 node in a separate thread using a MultiThreadedExecutor. + + Args: + ros_node (GuiNode): The ROS2 node to run. + executor (MultiThreadedExecutor): The executor to use for spinning the node. + """ rclpy.spin(ros_node, executor) -def main(args=None): + +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() @@ -106,14 +143,14 @@ def main(args=None): # Add buttons or other GUI elements if needed (optional) button1 = QPushButton("Send command", parent=central_widget) - button1.clicked.connect(lambda: ros_node.get_logger().info("Command sent")) # Clears the plot when pressed + button1.clicked.connect(lambda: ros_node.get_logger().info("Command sent")) layout.addWidget(button1) gui.setCentralWidget(central_widget) gui.show() # Use a QTimer to update the plot in the main thread - def update_plot(): + def update_plot() -> None: plot_canvas.update_plot(ros_node.x_data, ros_node.y_data) # Set up the timer to call update_plot every 100ms @@ -123,11 +160,6 @@ def update_plot(): sys.exit(app.exec()) - # Clean up - ros_node.get_logger().info("Shutting down") - ros_node.destroy_node() - executor.shutdown() - ros_thread.join() if __name__ == '__main__': main() diff --git a/mission/gui_auv/setup.py b/mission/gui_auv/setup.py index f023139ae..1fb1f2478 100644 --- a/mission/gui_auv/setup.py +++ b/mission/gui_auv/setup.py @@ -1,17 +1,17 @@ from setuptools import setup -package_name = 'auv_gui' +PACKAGE_NAME = 'auv_gui' setup( - name=package_name, + name=PACKAGE_NAME, version='0.0.0', - packages=[package_name], + packages=[PACKAGE_NAME], install_requires=['setuptools'], zip_safe=True, maintainer='sondre', maintainer_email='sondre@example.com', description='AUV GUI for ROS2', - license='MIT', + license='Apache License 2.0', tests_require=['pytest'], entry_points={ 'console_scripts': [ From fc75a99330d8338c3c587e8c925afcc3dbb8f91e Mon Sep 17 00:00:00 2001 From: Sondre Haugen Date: Sun, 13 Oct 2024 14:11:47 +0200 Subject: [PATCH 04/12] refactor: simplified docstrings --- mission/gui_auv/auv_gui/auv_gui.py | 25 ++++--------------------- 1 file changed, 4 insertions(+), 21 deletions(-) diff --git a/mission/gui_auv/auv_gui/auv_gui.py b/mission/gui_auv/auv_gui/auv_gui.py index 01a8cb3be..04eadafda 100644 --- a/mission/gui_auv/auv_gui/auv_gui.py +++ b/mission/gui_auv/auv_gui/auv_gui.py @@ -50,13 +50,7 @@ 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 with a reference to the ROS node and configure the plot. - - Args: - gui_node (GuiNode): A reference to the ROS node. - parent (Optional[QWidget]): The parent widget (default: None). - """ + """Initialize the PlotCanvas with a reference to the ROS node and configure the plot.""" self.gui_node = gui_node # Store a reference to the ROS node self.fig, self.ax = plt.subplots() super().__init__(self.fig) @@ -74,12 +68,7 @@ def __init__(self, gui_node: GuiNode, parent: Optional[QWidget] = None) -> None: self.mpl_connect("button_press_event", self.on_click) def on_click(self, event: MouseEvent) -> None: - """ - Handle mouse click event on the plot and log the clicked coordinates. - - Args: - event: The mouse click event from matplotlib. - """ + """Handle mouse click event on the plot and log the clicked coordinates.""" if event.inaxes is not None: x_click = event.xdata y_click = event.ydata @@ -108,13 +97,7 @@ def update_plot(self, x_data: List[float], y_data: List[float]) -> None: def run_ros_node(ros_node: GuiNode, executor: MultiThreadedExecutor) -> None: - """ - Run the ROS2 node in a separate thread using a MultiThreadedExecutor. - - Args: - ros_node (GuiNode): The ROS2 node to run. - executor (MultiThreadedExecutor): The executor to use for spinning the node. - """ + """Run the ROS2 node in a separate thread using a MultiThreadedExecutor.""" rclpy.spin(ros_node, executor) @@ -142,7 +125,7 @@ def main(args: Optional[List[str]] = None) -> None: layout.addWidget(plot_canvas) # Add buttons or other GUI elements if needed (optional) - button1 = QPushButton("Send command", parent=central_widget) + button1 = QPushButton("Send a command", parent=central_widget) button1.clicked.connect(lambda: ros_node.get_logger().info("Command sent")) layout.addWidget(button1) From 59fe31bdc35328bba3b26ce8c5405c6e04b9f441 Mon Sep 17 00:00:00 2001 From: Sondre Haugen Date: Sun, 13 Oct 2024 15:58:09 +0200 Subject: [PATCH 05/12] feat: 3D plot of drone pos --- mission/gui_auv/auv_gui/auv_gui.py | 54 ++++++++++++++++++------------ 1 file changed, 32 insertions(+), 22 deletions(-) diff --git a/mission/gui_auv/auv_gui/auv_gui.py b/mission/gui_auv/auv_gui/auv_gui.py index 04eadafda..50e3c0d68 100644 --- a/mission/gui_auv/auv_gui/auv_gui.py +++ b/mission/gui_auv/auv_gui/auv_gui.py @@ -3,8 +3,8 @@ from typing import List, Optional import matplotlib.pyplot as plt +import numpy as np import rclpy -from matplotlib.backend_bases import MouseEvent from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas from nav_msgs.msg import Odometry from PyQt5.QtCore import QTimer @@ -26,6 +26,7 @@ def __init__(self) -> None: # Variables to store odometry data self.x_data: List[float] = [] self.y_data: List[float] = [] + self.z_data: List[float] = [] self.counter_ = 0 self.get_logger().info("Subscribed to /nucleus/odom") @@ -35,8 +36,10 @@ def odom_callback(self, msg: Odometry) -> None: # Extract x and y positions from the odometry message x = msg.pose.pose.position.x y = msg.pose.pose.position.y + z = -(msg.pose.pose.position.z) self.x_data.append(x) self.y_data.append(y) + self.z_data.append(z) # Limit the stored data for real-time plotting (avoid memory overflow) # Store 30 seconds worth of data at 100Hz @@ -44,6 +47,7 @@ def odom_callback(self, msg: Odometry) -> None: if len(self.x_data) > max_data_points: self.x_data.pop(0) self.y_data.pop(0) + self.z_data.pop(0) class PlotCanvas(FigureCanvas): @@ -52,48 +56,54 @@ class PlotCanvas(FigureCanvas): def __init__(self, gui_node: GuiNode, parent: Optional[QWidget] = None) -> None: """Initialize the PlotCanvas with a reference to the ROS node and configure the plot.""" self.gui_node = gui_node # Store a reference to the ROS node - self.fig, self.ax = plt.subplots() + self.fig = plt.figure() + self.ax = self.fig.add_subplot(111, projection='3d') # Set up 3D projection super().__init__(self.fig) self.setParent(parent) - self.ax.set_xlabel("X Position") - self.ax.set_ylabel("Y Position") + # 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("Odometry") + # Initialize data lists for 3D plot self.x_data: List[float] = [] self.y_data: List[float] = [] - (self.line,) = self.ax.plot([], [], 'b-') + self.z_data: List[float] = [] + (self.line,) = self.ax.plot([], [], [], 'b-') # 3D line plot - # Mouse click event handler - self.mpl_connect("button_press_event", self.on_click) - - def on_click(self, event: MouseEvent) -> None: - """Handle mouse click event on the plot and log the clicked coordinates.""" - if event.inaxes is not None: - x_click = event.xdata - y_click = event.ydata - # Use the logger from the ROS node to log the click event - self.gui_node.get_logger().info(f"Clicked at: x={x_click}, y={y_click}") - - def update_plot(self, x_data: List[float], y_data: List[float]) -> None: + def update_plot(self, x_data: List[float], y_data: List[float], z_data: List[float]) -> None: """ - Update the plot with the latest odometry data. + Update the 3D plot with the latest odometry data. Args: x_data (List[float]): The list of x positions. y_data (List[float]): The list of y positions. + z_data (List[float]): The list of z positions. """ - self.line.set_data(x_data, y_data) # Update the data of the line object + # 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) # Update the 2D projection data + self.line.set_3d_properties(z_data) # Update the z-data for the 3D line - if x_data and y_data: + # 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 = 1 # 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.draw() + self.fig.canvas.draw() + self.fig.canvas.flush_events() def run_ros_node(ros_node: GuiNode, executor: MultiThreadedExecutor) -> None: @@ -134,7 +144,7 @@ def main(args: Optional[List[str]] = None) -> None: # Use a QTimer to update the plot in the main thread def update_plot() -> None: - plot_canvas.update_plot(ros_node.x_data, ros_node.y_data) + plot_canvas.update_plot(ros_node.x_data, ros_node.y_data, ros_node.z_data) # Set up the timer to call update_plot every 100ms timer = QTimer() From 614f456c5fe9769eda63f23cd590818a8d224ff3 Mon Sep 17 00:00:00 2001 From: Sondre Haugen Date: Sun, 13 Oct 2024 16:52:58 +0200 Subject: [PATCH 06/12] feat: display current pose --- mission/gui_auv/auv_gui/auv_gui.py | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/mission/gui_auv/auv_gui/auv_gui.py b/mission/gui_auv/auv_gui/auv_gui.py index 50e3c0d68..98cbc0a15 100644 --- a/mission/gui_auv/auv_gui/auv_gui.py +++ b/mission/gui_auv/auv_gui/auv_gui.py @@ -8,7 +8,7 @@ from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas from nav_msgs.msg import Odometry from PyQt5.QtCore import QTimer -from PyQt5.QtWidgets import QApplication, QMainWindow, QPushButton, QVBoxLayout, QWidget +from PyQt5.QtWidgets import QApplication, QLabel, QMainWindow, QVBoxLayout, QWidget from rclpy.executors import MultiThreadedExecutor from rclpy.node import Node @@ -124,7 +124,7 @@ def main(args: Optional[List[str]] = None) -> None: # Setup the PyQt5 application and window app = QApplication(sys.argv) gui = QMainWindow() - gui.setWindowTitle("AUV Odometry GUI") + gui.setWindowTitle("Vortex GUI") gui.setGeometry(100, 100, 600, 400) # Create a central widget and layout for the GUI @@ -135,9 +135,12 @@ def main(args: Optional[List[str]] = None) -> None: layout.addWidget(plot_canvas) # Add buttons or other GUI elements if needed (optional) - button1 = QPushButton("Send a command", parent=central_widget) - button1.clicked.connect(lambda: ros_node.get_logger().info("Command sent")) - layout.addWidget(button1) + # button1 = QPushButton("Send a command", parent=central_widget) + # button1.clicked.connect(lambda: ros_node.get_logger().info("Command sent")) + # layout.addWidget(button1) + + current_pos = QLabel(parent=central_widget) + layout.addWidget(current_pos) gui.setCentralWidget(central_widget) gui.show() @@ -145,6 +148,7 @@ def main(args: Optional[List[str]] = None) -> None: # Use a QTimer to update the plot in the main thread def update_plot() -> None: plot_canvas.update_plot(ros_node.x_data, ros_node.y_data, ros_node.z_data) + current_pos.setText(f"Current Position:\nX: {ros_node.x_data[-1]:.2f}\nY: {ros_node.y_data[-1]:.2f}\nZ: {ros_node.z_data[-1]:.2f}") # Set up the timer to call update_plot every 100ms timer = QTimer() From 0beb5541be6c3f13a6bc464a15e8dc3f0b956be4 Mon Sep 17 00:00:00 2001 From: Sondre Haugen Date: Thu, 17 Oct 2024 11:11:27 +0200 Subject: [PATCH 07/12] feat: tab for internal status --- mission/gui_auv/auv_gui/auv_gui.py | 99 ++++++++++++++++++++++-------- mission/gui_auv/setup.py | 4 +- 2 files changed, 76 insertions(+), 27 deletions(-) diff --git a/mission/gui_auv/auv_gui/auv_gui.py b/mission/gui_auv/auv_gui/auv_gui.py index 98cbc0a15..955e7f2f1 100644 --- a/mission/gui_auv/auv_gui/auv_gui.py +++ b/mission/gui_auv/auv_gui/auv_gui.py @@ -8,9 +8,20 @@ from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas from nav_msgs.msg import Odometry from PyQt5.QtCore import QTimer -from PyQt5.QtWidgets import QApplication, QLabel, QMainWindow, QVBoxLayout, QWidget +from PyQt5.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 + +# --- GUI Node --- class GuiNode(Node): @@ -18,7 +29,7 @@ class GuiNode(Node): def __init__(self) -> None: """Initialize the GuiNode and set up the odometry subscriber.""" - super().__init__("simple_gui_node") + super().__init__("auv_gui_node") # Subscriber to the /nucleus/odom topic self.subscription = self.create_subscription(Odometry, '/nucleus/odom', self.odom_callback, 10) @@ -28,27 +39,46 @@ def __init__(self) -> None: self.y_data: List[float] = [] self.z_data: List[float] = [] - self.counter_ = 0 self.get_logger().info("Subscribed to /nucleus/odom") + # Subscribe to internal status topics + self.internal_status_subscriber = self.create_subscription(Float32, "/auv/power_sense_module/current", self.current_callback, 5) + self.internal_status_subscriber = self.create_subscription(Float32, "/auv/power_sense_module/voltage", self.voltage_callback, 5) + + # Variables for internal status + self.current = 0.0 + self.voltage = 0.0 + + # --- Callback functions --- + def odom_callback(self, msg: Odometry) -> None: """Callback function that is triggered when an odometry message is received.""" - # Extract x and y positions from the odometry message + # 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) + z = -(msg.pose.pose.position.z) # Inverted to match reality... self.x_data.append(x) self.y_data.append(y) self.z_data.append(z) # Limit the stored data for real-time plotting (avoid memory overflow) - # Store 30 seconds worth of data at 100Hz max_data_points = 30 * 100 # 30 seconds * 100 Hz if len(self.x_data) > max_data_points: self.x_data.pop(0) self.y_data.pop(0) self.z_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 + + +# --- Plotting --- + class PlotCanvas(FigureCanvas): """A canvas widget for plotting odometry data using matplotlib.""" @@ -65,7 +95,7 @@ def __init__(self, gui_node: GuiNode, parent: Optional[QWidget] = None) -> None: self.ax.set_xlabel("X") self.ax.set_ylabel("Y") self.ax.set_zlabel("Z") - self.ax.set_title("Odometry") + self.ax.set_title("Position") # Initialize data lists for 3D plot self.x_data: List[float] = [] @@ -127,35 +157,54 @@ def main(args: Optional[List[str]] = None) -> None: gui.setWindowTitle("Vortex GUI") gui.setGeometry(100, 100, 600, 400) - # Create a central widget and layout for the GUI - central_widget = QWidget() - layout = QVBoxLayout(central_widget) + # Create the tab widget + tabs = QTabWidget() + tabs.setTabPosition(QTabWidget.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) - plot_canvas = PlotCanvas(ros_node, central_widget) - layout.addWidget(plot_canvas) + current_pos = QLabel(parent=position_widget) + layout.addWidget(current_pos, 0, 1) - # Add buttons or other GUI elements if needed (optional) - # button1 = QPushButton("Send a command", parent=central_widget) - # button1.clicked.connect(lambda: ros_node.get_logger().info("Command sent")) - # layout.addWidget(button1) + tabs.addTab(position_widget, "Position") - current_pos = QLabel(parent=central_widget) - layout.addWidget(current_pos) + # --- Internal Status Tab --- + internal_widget = QWidget() + internal_layout = QVBoxLayout(internal_widget) - gui.setCentralWidget(central_widget) + internal_status_label = QLabel(parent=internal_widget) + internal_layout.addWidget(internal_status_label) + tabs.addTab(internal_widget, "Internal") + + gui.setCentralWidget(tabs) gui.show() - # Use a QTimer to update the plot in the main thread - def update_plot() -> None: + # 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.x_data, ros_node.y_data, ros_node.z_data) - current_pos.setText(f"Current Position:\nX: {ros_node.x_data[-1]:.2f}\nY: {ros_node.y_data[-1]:.2f}\nZ: {ros_node.z_data[-1]:.2f}") + if len(ros_node.x_data) > 0: + current_pos.setText(f"Current Position:\nX: {ros_node.x_data[-1]:.2f}\nY: {ros_node.y_data[-1]:.2f}\nZ: {ros_node.z_data[-1]:.2f}") + + # Update internal status + internal_status_label.setText(f"Internal Status:\nCurrent: {ros_node.current:.2f}\nVoltage: {ros_node.voltage:.2f}") - # Set up the timer to call update_plot every 100ms + # Set up the timer to call update_gui every 100ms timer = QTimer() - timer.timeout.connect(update_plot) + timer.timeout.connect(update_gui) timer.start(100) # 100 ms interval - sys.exit(app.exec()) + app.exec() + + # Cleanup ROS resources + ros_node.destroy_node() + rclpy.shutdown() + sys.exit() if __name__ == '__main__': diff --git a/mission/gui_auv/setup.py b/mission/gui_auv/setup.py index 1fb1f2478..de9acfda1 100644 --- a/mission/gui_auv/setup.py +++ b/mission/gui_auv/setup.py @@ -9,9 +9,9 @@ install_requires=['setuptools'], zip_safe=True, maintainer='sondre', - maintainer_email='sondre@example.com', + maintainer_email='sondre95556888@gmail.com', description='AUV GUI for ROS2', - license='Apache License 2.0', + license='MIT', tests_require=['pytest'], entry_points={ 'console_scripts': [ From 2d98bd8e71a6b0a5e256189fac5dba346f7704a4 Mon Sep 17 00:00:00 2001 From: Sondre Haugen Date: Fri, 18 Oct 2024 15:54:29 +0200 Subject: [PATCH 08/12] feat: display drone orientation --- mission/gui_auv/auv_gui/auv_gui.py | 89 +++++++++++++++++++++++++----- 1 file changed, 75 insertions(+), 14 deletions(-) diff --git a/mission/gui_auv/auv_gui/auv_gui.py b/mission/gui_auv/auv_gui/auv_gui.py index 955e7f2f1..00ae2ef03 100644 --- a/mission/gui_auv/auv_gui/auv_gui.py +++ b/mission/gui_auv/auv_gui/auv_gui.py @@ -1,3 +1,4 @@ +import math import sys from threading import Thread from typing import List, Optional @@ -21,6 +22,40 @@ 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 --- @@ -35,11 +70,18 @@ def __init__(self) -> None: self.subscription = self.create_subscription(Odometry, '/nucleus/odom', self.odom_callback, 10) # Variables to store odometry data - self.x_data: List[float] = [] - self.y_data: List[float] = [] - self.z_data: List[float] = [] + self.xpos_data: List[float] = [] # x position + self.ypos_data: List[float] = [] # y position + self.zpos_data: List[float] = [] # z position - self.get_logger().info("Subscribed to /nucleus/odom") + 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.internal_status_subscriber = self.create_subscription(Float32, "/auv/power_sense_module/current", self.current_callback, 5) @@ -57,16 +99,25 @@ def odom_callback(self, msg: Odometry) -> None: x = msg.pose.pose.position.x y = msg.pose.pose.position.y z = -(msg.pose.pose.position.z) # Inverted to match reality... - self.x_data.append(x) - self.y_data.append(y) - self.z_data.append(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 = quaternion_to_euler(x, y, z, w)[0] + self.pitch = quaternion_to_euler(x, y, z, w)[1] + self.yaw = quaternion_to_euler(x, y, z, w)[2] # Limit the stored data for real-time plotting (avoid memory overflow) max_data_points = 30 * 100 # 30 seconds * 100 Hz if len(self.x_data) > max_data_points: - self.x_data.pop(0) - self.y_data.pop(0) - self.z_data.pop(0) + 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.""" @@ -88,6 +139,10 @@ def __init__(self, gui_node: GuiNode, parent: Optional[QWidget] = None) -> None: self.gui_node = gui_node # Store a reference to the ROS node self.fig = plt.figure() self.ax = self.fig.add_subplot(111, projection='3d') # Set up 3D projection + + (self.line,) = self.ax.plot([], [], [], 'b-') # Initialize an empty 3D line + (self.current_position_dot,) = self.ax.plot([], [], [], 'ro') # Initialize a red dot for the current position + super().__init__(self.fig) self.setParent(parent) @@ -122,11 +177,15 @@ def update_plot(self, x_data: List[float], y_data: List[float], z_data: List[flo self.line.set_data(x_data, y_data) # Update the 2D projection data self.line.set_3d_properties(z_data) # Update the z-data for the 3D line + # Update the current position dot + self.current_position_dot.set_data(x_data[-1:], y_data[-1:]) # Update the 2D projection data + self.current_position_dot.set_3d_properties(z_data[-1:]) # Update the z-data for the 3D dot + # 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 = 1 # Define a margin around the latest point + 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) @@ -187,9 +246,11 @@ def main(args: Optional[List[str]] = None) -> None: # 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.x_data, ros_node.y_data, ros_node.z_data) - if len(ros_node.x_data) > 0: - current_pos.setText(f"Current Position:\nX: {ros_node.x_data[-1]:.2f}\nY: {ros_node.y_data[-1]:.2f}\nZ: {ros_node.z_data[-1]:.2f}") + 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:\nCurrent: {ros_node.current:.2f}\nVoltage: {ros_node.voltage:.2f}") From 1252cc732a415f07682440c7d89a9cc2518ada61 Mon Sep 17 00:00:00 2001 From: Sondre Haugen Date: Fri, 18 Oct 2024 16:12:13 +0200 Subject: [PATCH 09/12] refactor: comments --- mission/gui_auv/auv_gui/auv_gui.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/mission/gui_auv/auv_gui/auv_gui.py b/mission/gui_auv/auv_gui/auv_gui.py index 00ae2ef03..fc324c9c7 100644 --- a/mission/gui_auv/auv_gui/auv_gui.py +++ b/mission/gui_auv/auv_gui/auv_gui.py @@ -136,12 +136,13 @@ class PlotCanvas(FigureCanvas): def __init__(self, gui_node: GuiNode, parent: Optional[QWidget] = None) -> None: """Initialize the PlotCanvas with a reference to the ROS node and configure the plot.""" + # Set up the 3D plot self.gui_node = gui_node # Store a reference to the ROS node self.fig = plt.figure() self.ax = self.fig.add_subplot(111, projection='3d') # Set up 3D projection - (self.line,) = self.ax.plot([], [], [], 'b-') # Initialize an empty 3D line - (self.current_position_dot,) = self.ax.plot([], [], [], 'ro') # Initialize a red dot for the current position + # Initialize a red dot for the current position + (self.current_position_dot,) = self.ax.plot([], [], [], 'ro') super().__init__(self.fig) self.setParent(parent) From 0e3183fb6db0d1f37f3773d7c9281740f3c63f2d Mon Sep 17 00:00:00 2001 From: Sondre Haugen Date: Sun, 3 Nov 2024 14:05:21 +0100 Subject: [PATCH 10/12] feat: temp and pressure status --- mission/gui_auv/auv_gui/auv_gui.py | 50 ++++++++++++++++++------------ 1 file changed, 30 insertions(+), 20 deletions(-) diff --git a/mission/gui_auv/auv_gui/auv_gui.py b/mission/gui_auv/auv_gui/auv_gui.py index fc324c9c7..b0c5217eb 100644 --- a/mission/gui_auv/auv_gui/auv_gui.py +++ b/mission/gui_auv/auv_gui/auv_gui.py @@ -86,10 +86,14 @@ def __init__(self) -> None: # Subscribe to internal status topics self.internal_status_subscriber = self.create_subscription(Float32, "/auv/power_sense_module/current", self.current_callback, 5) self.internal_status_subscriber = self.create_subscription(Float32, "/auv/power_sense_module/voltage", self.voltage_callback, 5) + self.internal_status_subscriber = self.create_subscription(Float32, "/auv/temperature", self.temperature_callback, 5) + self.internal_status_subscriber = self.create_subscription(Float32, "/auv/pressure", 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 --- @@ -98,7 +102,7 @@ def odom_callback(self, msg: Odometry) -> None: # 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) # Inverted to match reality... + z = -(msg.pose.pose.position.z) self.xpos_data.append(x) self.ypos_data.append(y) self.zpos_data.append(z) @@ -127,6 +131,14 @@ 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 --- @@ -135,11 +147,11 @@ 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 with a reference to the ROS node and configure the plot.""" + """Initialize the PlotCanvas.""" # Set up the 3D plot - self.gui_node = gui_node # Store a reference to the ROS node + self.gui_node = gui_node self.fig = plt.figure() - self.ax = self.fig.add_subplot(111, projection='3d') # Set up 3D projection + 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') @@ -157,17 +169,10 @@ def __init__(self, gui_node: GuiNode, parent: Optional[QWidget] = None) -> None: self.x_data: List[float] = [] self.y_data: List[float] = [] self.z_data: List[float] = [] - (self.line,) = self.ax.plot([], [], [], 'b-') # 3D line plot + (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. - - Args: - x_data (List[float]): The list of x positions. - y_data (List[float]): The list of y positions. - z_data (List[float]): The list of z positions. - """ + """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) @@ -175,12 +180,12 @@ def update_plot(self, x_data: List[float], y_data: List[float], z_data: List[flo # 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) # Update the 2D projection data - self.line.set_3d_properties(z_data) # Update the z-data for the 3D line + 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:]) # Update the 2D projection data - self.current_position_dot.set_3d_properties(z_data[-1:]) # Update the z-data for the 3D 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] @@ -254,16 +259,21 @@ def update_gui() -> None: current_pos.setText(position_text + "\n\n\n" + orientation_text) # Update internal status - internal_status_label.setText(f"Internal Status:\nCurrent: {ros_node.current:.2f}\nVoltage: {ros_node.voltage:.2f}") + 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) # 100 ms interval + timer.start(100) app.exec() - # Cleanup ROS resources ros_node.destroy_node() rclpy.shutdown() sys.exit() From 9810f0b525940ee6ac181924bfa7f6f2c6d2d7d0 Mon Sep 17 00:00:00 2001 From: Sondre Haugen Date: Sun, 3 Nov 2024 15:37:09 +0100 Subject: [PATCH 11/12] feat: config file for parameters --- mission/gui_auv/auv_gui/auv_gui.py | 38 +++++++++++++++++--------- mission/gui_auv/config/gui_params.yaml | 0 2 files changed, 25 insertions(+), 13 deletions(-) create mode 100644 mission/gui_auv/config/gui_params.yaml diff --git a/mission/gui_auv/auv_gui/auv_gui.py b/mission/gui_auv/auv_gui/auv_gui.py index b0c5217eb..c0ea054fe 100644 --- a/mission/gui_auv/auv_gui/auv_gui.py +++ b/mission/gui_auv/auv_gui/auv_gui.py @@ -8,8 +8,8 @@ import rclpy from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas from nav_msgs.msg import Odometry -from PyQt5.QtCore import QTimer -from PyQt5.QtWidgets import ( +from PyQt6.QtCore import QTimer +from PyQt6.QtWidgets import ( QApplication, QGridLayout, QLabel, @@ -66,8 +66,22 @@ 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, '/nucleus/odom', self.odom_callback, 10) + self.subscription = self.create_subscription(Odometry, odom_topic, self.odom_callback, 10) # Variables to store odometry data self.xpos_data: List[float] = [] # x position @@ -84,10 +98,10 @@ def __init__(self) -> None: self.yaw: Optional[float] = None # Subscribe to internal status topics - self.internal_status_subscriber = self.create_subscription(Float32, "/auv/power_sense_module/current", self.current_callback, 5) - self.internal_status_subscriber = self.create_subscription(Float32, "/auv/power_sense_module/voltage", self.voltage_callback, 5) - self.internal_status_subscriber = self.create_subscription(Float32, "/auv/temperature", self.temperature_callback, 5) - self.internal_status_subscriber = self.create_subscription(Float32, "/auv/pressure", self.pressure_callback, 5) + 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 @@ -112,12 +126,10 @@ def odom_callback(self, msg: Odometry) -> None: x = msg.pose.pose.orientation.x y = msg.pose.pose.orientation.y z = msg.pose.pose.orientation.z - self.roll = quaternion_to_euler(x, y, z, w)[0] - self.pitch = quaternion_to_euler(x, y, z, w)[1] - self.yaw = quaternion_to_euler(x, y, z, w)[2] + 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 = 30 * 100 # 30 seconds * 100 Hz + 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) @@ -224,7 +236,7 @@ def main(args: Optional[List[str]] = None) -> None: # Create the tab widget tabs = QTabWidget() - tabs.setTabPosition(QTabWidget.North) + tabs.setTabPosition(QTabWidget.TabPosition.North) tabs.setMovable(True) # --- Position Tab --- @@ -248,7 +260,7 @@ def main(args: Optional[List[str]] = None) -> None: tabs.addTab(internal_widget, "Internal") gui.setCentralWidget(tabs) - gui.show() + gui.showMaximized() # Use a QTimer to update plot, current position, and internal status in the main thread def update_gui() -> None: diff --git a/mission/gui_auv/config/gui_params.yaml b/mission/gui_auv/config/gui_params.yaml new file mode 100644 index 000000000..e69de29bb From 915f9209e4624f8d0d1ab136c1fca5737f510704 Mon Sep 17 00:00:00 2001 From: Sondre Haugen Date: Sun, 3 Nov 2024 16:00:18 +0100 Subject: [PATCH 12/12] fix: path config --- mission/gui_auv/config/gui_params.yaml | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/mission/gui_auv/config/gui_params.yaml b/mission/gui_auv/config/gui_params.yaml index e69de29bb..a490595f0 100644 --- a/mission/gui_auv/config/gui_params.yaml +++ b/mission/gui_auv/config/gui_params.yaml @@ -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