From ed9e6e0ba01735ad62d46b6ad1e5aa379f3ac6d5 Mon Sep 17 00:00:00 2001 From: PizzaAllTheWay Date: Wed, 14 Aug 2024 20:25:38 +0200 Subject: [PATCH] added acoustics, need config --- .../acoustics_data_record/CMakeLists.txt | 29 ++ acoustics/acoustics_data_record/README | 6 + .../acoustics_data_record/__init__.py | 0 .../acoustics_data_record_lib.py | 78 +++++ .../acoustics_data_record_node.py | 197 ++++++++++++ .../acoustics_data_record/launch/__init__.py | 0 .../launch/acoustics_data_record.launch.py | 24 ++ acoustics/acoustics_data_record/package.xml | 19 ++ .../utilities/display_acoustics_data_live.py | 275 +++++++++++++++++ acoustics/acoustics_interface/CMakeLists.txt | 31 ++ acoustics/acoustics_interface/README | 2 + .../acoustics_interface/__init__.py | 0 .../acoustics_interface_lib.py | 287 ++++++++++++++++++ .../acoustics_interface_node.py | 98 ++++++ .../acoustics_interface/launch/__init__.py | 0 .../launch/acoustics_interface_launch.py | 24 ++ acoustics/acoustics_interface/package.xml | 19 ++ 17 files changed, 1089 insertions(+) create mode 100644 acoustics/acoustics_data_record/CMakeLists.txt create mode 100644 acoustics/acoustics_data_record/README create mode 100644 acoustics/acoustics_data_record/acoustics_data_record/__init__.py create mode 100644 acoustics/acoustics_data_record/acoustics_data_record/acoustics_data_record_lib.py create mode 100644 acoustics/acoustics_data_record/acoustics_data_record/acoustics_data_record_node.py create mode 100644 acoustics/acoustics_data_record/launch/__init__.py create mode 100644 acoustics/acoustics_data_record/launch/acoustics_data_record.launch.py create mode 100644 acoustics/acoustics_data_record/package.xml create mode 100644 acoustics/acoustics_data_record/utilities/display_acoustics_data_live.py create mode 100644 acoustics/acoustics_interface/CMakeLists.txt create mode 100644 acoustics/acoustics_interface/README create mode 100644 acoustics/acoustics_interface/acoustics_interface/__init__.py create mode 100644 acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py create mode 100644 acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py create mode 100644 acoustics/acoustics_interface/launch/__init__.py create mode 100644 acoustics/acoustics_interface/launch/acoustics_interface_launch.py create mode 100644 acoustics/acoustics_interface/package.xml diff --git a/acoustics/acoustics_data_record/CMakeLists.txt b/acoustics/acoustics_data_record/CMakeLists.txt new file mode 100644 index 000000000..ee711f38e --- /dev/null +++ b/acoustics/acoustics_data_record/CMakeLists.txt @@ -0,0 +1,29 @@ +cmake_minimum_required(VERSION 3.8) +project(acoustics_data_record) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(rclpy REQUIRED) +find_package(std_msgs REQUIRED) + +# Install launch files. +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME} +) + +install(PROGRAMS + ${PROJECT_NAME}/acoustics_data_record_lib.py + ${PROJECT_NAME}/acoustics_data_record_node.py + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() \ No newline at end of file diff --git a/acoustics/acoustics_data_record/README b/acoustics/acoustics_data_record/README new file mode 100644 index 000000000..5e2451090 --- /dev/null +++ b/acoustics/acoustics_data_record/README @@ -0,0 +1,6 @@ +A handy ROS2 node to record data from acoustic PCB +Has the ability to display data live in real time +OBS!: Make sure the Acoustics Interface Node is running before starting acoustics recording node, otherwise you will be recording nothing :P + +Use utilities/display_acoustics_data_live.py to display data in real time while it is being saved into the .csv file through the ROS2 node +(Verry cool to look at, would recoment ;DDD) diff --git a/acoustics/acoustics_data_record/acoustics_data_record/__init__.py b/acoustics/acoustics_data_record/acoustics_data_record/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/acoustics/acoustics_data_record/acoustics_data_record/acoustics_data_record_lib.py b/acoustics/acoustics_data_record/acoustics_data_record/acoustics_data_record_lib.py new file mode 100644 index 000000000..d148f75df --- /dev/null +++ b/acoustics/acoustics_data_record/acoustics_data_record/acoustics_data_record_lib.py @@ -0,0 +1,78 @@ +# Python Libraries +import time +import csv +from datetime import datetime + + + +class AcousticsDataRecordLib: + def __init__(self, + ROS2_PACKAGE_DIRECTORY = "" + ): + # Global variables for .csv file manipulation ---------- + # Get the path for the directory where we will store our data + self.acoustics_data_directory = ROS2_PACKAGE_DIRECTORY + "acoustics_data/" + + timestamp = time.strftime('%Y-%m-%d_%H:%M:%S') + data_file_name = 'acoustics_data_' + timestamp + '.csv' + self.data_file_location = self.acoustics_data_directory + data_file_name + + self.csv_headers = [ + "Time", + + "Hydrophone1", + "Hydrophone2", + "Hydrophone3", + "Hydrophone4", + "Hydrophone5", + + "FilterResponse", + "FFT", + "Peaks", + + "TDOA", + "Position", + ] + + # Make new .csv file for loging blackbox data ---------- + with open(self.data_file_location, mode="w", newline="") as csv_file: + writer = csv.writer(csv_file) + writer.writerow(self.csv_headers) + + # Methods for external uses ---------- + def log_data_to_csv_file(self, + hydrophone1 = [0], + hydrophone2 = [0], + hydrophone3 = [0], + hydrophone4 = [0], + hydrophone5 = [0], + + filter_response = [0], + fft = [0], + peaks = [0], + + tdoa = [0.0], + position = [0.0], + ): + # Get current time in hours, minutes, seconds and miliseconds + current_time = datetime.now().strftime("%H:%M:%S.%f")[:-3] + + # Write to .csv file + with open(self.data_file_location, mode="a", newline="") as csv_file: + writer = csv.writer(csv_file) + writer.writerow([ + current_time, + + hydrophone1, + hydrophone2, + hydrophone3, + hydrophone4, + hydrophone5, + + filter_response, + fft, + peaks, + + tdoa, + position, + ]) \ No newline at end of file diff --git a/acoustics/acoustics_data_record/acoustics_data_record/acoustics_data_record_node.py b/acoustics/acoustics_data_record/acoustics_data_record/acoustics_data_record_node.py new file mode 100644 index 000000000..61a1e9d5d --- /dev/null +++ b/acoustics/acoustics_data_record/acoustics_data_record/acoustics_data_record_node.py @@ -0,0 +1,197 @@ +#!/usr/bin/env python3 + +# ROS2 libraries +import rclpy +from rclpy.node import Node +from std_msgs.msg import Float32MultiArray, Int32MultiArray +from ament_index_python.packages import get_package_share_directory + +# Python libraries +import array + +# Custom libraries +from acoustics_data_record_lib import AcousticsDataRecordLib + + + +class AcousticsDataRecordNode(Node): + def __init__(self): + # Variables for seting upp loging correctly + hydrophoneDataSize = (2**10) * 3 # 1 hydrophone buffer is 2^10 long, Each hydrophone data has 3 buffers full of this data + DSPDataSize = 2**10 # DSP (Digital Signal Processing) has 2^10 long data + TDOADataSize = 5 # TDOA (Time Difference Of Arrival) has 5 hydrophones it has times for + positionDataSize = 3 # position only has X, Y, Z basicaly 3 elements + + # Initialize ROS2 node + super().__init__('acoustics_data_record_node') + + # Initialize Subscribers ---------- + # Start listening to Hydrophone data + self.subscriberHydrophone1 = self.create_subscription( + Int32MultiArray, + '/acoustics/hydrophone1', + self.hydrophone1_callback, + 5) + self.hydropone1Data = array.array("i", [0] * hydrophoneDataSize) + + self.subscriberHydrophone2 = self.create_subscription( + Int32MultiArray, + '/acoustics/hydrophone2', + self.hydrophone2_callback, + 5) + self.hydropone2Data = array.array("i", [0] * hydrophoneDataSize) + + self.subscriberHydrophone3 = self.create_subscription( + Int32MultiArray, + '/acoustics/hydrophone3', + self.hydrophone3_callback, + 5) + self.hydropone3Data = array.array("i", [0] * hydrophoneDataSize) + + self.subscriberHydrophone4 = self.create_subscription( + Int32MultiArray, + '/acoustics/hydrophone4', + self.hydrophone4_callback, + 5) + self.hydropone4Data = array.array("i", [0] * hydrophoneDataSize) + + self.subscriberHydrophone5 = self.create_subscription( + Int32MultiArray, + '/acoustics/hydrophone5', + self.hydrophone5_callback, + 5) + self.hydropone5Data = array.array("i", [0] * hydrophoneDataSize) + + # Start listening to DSP (Digital Signal Processing) data + self.subscriberFilterResponse = self.create_subscription( + Int32MultiArray, + '/acoustics/filter_response', + self.filter_response_callback, + 5) + self.filterResponseData = array.array("i", [0] * DSPDataSize) + + self.subscriberFFT = self.create_subscription( + Int32MultiArray, + '/acoustics/fft', + self.fft_callback, + 5) + self.FFTData = array.array("i", [0] * DSPDataSize) + + self.subscriberPeaks = self.create_subscription( + Int32MultiArray, + '/acoustics/peaks', + self.peaks_callback, + 5) + self.peaksData = array.array("i", [0] * DSPDataSize) + + # Start listening to Multilateration data + self.subscriberTDOAResponse = self.create_subscription( + Float32MultiArray, + '/acoustics/time_difference_of_arrival', + self.tdoa_callback, + 5) + self.TDOAData = array.array("f", [0.0] * TDOADataSize) + + self.subscriberPositionResponse = self.create_subscription( + Float32MultiArray, + '/acoustics/position', + self.position_callback, + 5) + self.positionData = array.array("f", [0.0] * positionDataSize) + + # Initialize logger ---------- + # Get package directory location + ros2_package_directory_location = get_package_share_directory("acoustics_data_record") + ros2_package_directory_location = ros2_package_directory_location + "/../../../../" # go back to workspace + ros2_package_directory_location = ros2_package_directory_location + "src/vortex-asv/acoustics/acoustics_data_record/" # Navigate to this package + + # Make blackbox loging file + self.acoustics_data_record = AcousticsDataRecordLib( + ROS2_PACKAGE_DIRECTORY = ros2_package_directory_location + ) + + # Logs all the newest data 1 time(s) per second + self.declare_parameter("acoustics.data_logging_rate", 1.0) # Providing a default value 1.0 => 1 samplings per second, verry slow + DATA_LOGING_RATE = self.get_parameter("acoustics.data_logging_rate").get_parameter_value().double_value + timer_period = 1.0/DATA_LOGING_RATE + self.logger_timer = self.create_timer(timer_period, self.logger) + + # Debuging ---------- + self.get_logger().info( + "Started logging data for topics: \n" + "/acoustics/hydrophone1 [Int32MultiArray] \n" + "/acoustics/hydrophone2 [Int32MultiArray] \n" + "/acoustics/hydrophone3 [Int32MultiArray] \n" + "/acoustics/hydrophone4 [Int32MultiArray] \n" + "/acoustics/hydrophone5 [Int32MultiArray] \n" + "/acoustics/filter_response [Int32MultiArray] \n" + "/acoustics/fft [Int32MultiArray] \n" + "/acoustics/peaks [Int32MultiArray] \n" + "/acoustics/time_difference_of_arrival [Float32MultiArray] \n" + "/acoustics/position [Float32MultiArray] \n" + ) + + # Callback methods for diffrenet topics + def hydrophone1_callback(self, msg): + self.hydropone1Data = msg.data + + def hydrophone2_callback(self, msg): + self.hydropone2Data = msg.data + + def hydrophone3_callback(self, msg): + self.hydropone3Data = msg.data + + def hydrophone4_callback(self, msg): + self.hydropone4Data = msg.data + + def hydrophone5_callback(self, msg): + self.hydropone5Data = msg.data + + def filter_response_callback(self, msg): + self.filterResponseData = msg.data + + def fft_callback(self, msg): + self.FFTData = msg.data + + def peaks_callback(self, msg): + self.peaksData = msg.data + + def tdoa_callback(self, msg): + self.TDOAData = msg.data + + def position_callback(self, msg): + self.positionData = msg.data + + # The logger that logs all the data + def logger(self): + self.acoustics_data_record.log_data_to_csv_file( + hydrophone1=self.hydropone1Data, + hydrophone2=self.hydropone2Data, + hydrophone3=self.hydropone3Data, + hydrophone4=self.hydropone4Data, + hydrophone5=self.hydropone5Data, + + filter_response=self.filterResponseData, + fft=self.FFTData, + peaks=self.peaksData, + + tdoa=self.TDOAData, + position=self.positionData, + ) + + + +def main(): + # Initialize ROS2 + rclpy.init() + + # Start the ROS2 node and continue forever until exit + acoustics_data_record_node = AcousticsDataRecordNode() + rclpy.spin(acoustics_data_record_node) + + # Destroy the node explicitly once ROS2 stops runing + acoustics_data_record_node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/acoustics/acoustics_data_record/launch/__init__.py b/acoustics/acoustics_data_record/launch/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/acoustics/acoustics_data_record/launch/acoustics_data_record.launch.py b/acoustics/acoustics_data_record/launch/acoustics_data_record.launch.py new file mode 100644 index 000000000..508f088b4 --- /dev/null +++ b/acoustics/acoustics_data_record/launch/acoustics_data_record.launch.py @@ -0,0 +1,24 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import os + +def generate_launch_description(): + # Path to the YAML file + yaml_file_path = os.path.join( + get_package_share_directory("blackbox"), + "../../../../", # Go to the workspace + "src/vortex-asv/asv_setup/config/robots/", # Go inside where yamal files are located at + 'freya.yaml' + ) + + return LaunchDescription([ + Node( + package='acoustics_data_record', + namespace='acoustics_data_record', + executable='acoustics_data_record_node.py', + name='acoustics_data_record_node', + output='screen', + parameters=[yaml_file_path], + ), + ]) \ No newline at end of file diff --git a/acoustics/acoustics_data_record/package.xml b/acoustics/acoustics_data_record/package.xml new file mode 100644 index 000000000..22c36c11f --- /dev/null +++ b/acoustics/acoustics_data_record/package.xml @@ -0,0 +1,19 @@ + + + + acoustics_data_record + 1.0.0 + Records data from the acoustics + vortex + MIT + + + rclpy + std_msgs + ros2launch + + + + ament_cmake + + diff --git a/acoustics/acoustics_data_record/utilities/display_acoustics_data_live.py b/acoustics/acoustics_data_record/utilities/display_acoustics_data_live.py new file mode 100644 index 000000000..b1c1e3de8 --- /dev/null +++ b/acoustics/acoustics_data_record/utilities/display_acoustics_data_live.py @@ -0,0 +1,275 @@ +#!/usr/bin/env python3 + +# Libraries for file manipulation +import os +import sys +import ast +import glob + +# Libraries for handling data structures +import pandas as pd +import numpy as np +import array + +# Libraries for anmation +import matplotlib.animation as animation +import matplotlib.gridspec as gridspec +import matplotlib.pyplot as plt + + + +# Variables for seting upp data structures correctly +hydrophoneDataSize = (2**10) * 3 # 1 hydrophone buffer is 2^10 long, Each hydrophone data has 3 buffers full of this data +DSPDataSize = 2**10 # DSP (Digital Signal Processing) has 2^10 long data +TDOADataSize = 5 # TDOA (Time Difference Of Arrival) has 5 hydrophones it has times for +positionDataSize = 3 # position only has X, Y, Z basicaly 3 elements + +# Important variables for later processing of data +SAMPLE_RATE = 430_000 # 430 kHz +MAX_FREQUENCY_TO_SHOW = 60_000 # 60 kHz +FPS = 1 + + + +# Make a good plot layout ================================================== +fig = plt.figure() +# Create an outer GridSpec for the two columns +outer_gs = gridspec.GridSpec(1, 2, figure=fig, width_ratios=[1, 1]) +# Create an inner GridSpec for the first column +gs_hydrophone = gridspec.GridSpecFromSubplotSpec( + 5, 1, subplot_spec=outer_gs[0], hspace=0.1 +) +# Create an inner GridSpec for the second column, with height ratios for the 70%/30% split +gs_dsp = gridspec.GridSpecFromSubplotSpec( + 2, 1, subplot_spec=outer_gs[1], height_ratios=[7, 3], hspace=0.3 +) + +hydrophoneAxis = [None] * 5 + +# Add subplots in the first column for hydrophone data +for i in range(5): + hydrophoneAxis[i] = fig.add_subplot( + gs_hydrophone[i, 0], sharex=hydrophoneAxis[0] if i else None + ) + hydrophoneAxis[i].label_outer() +fig.text(0.25, 0.965, "Hydrophone Data", ha="center") + +# Add subplots in the second column +FFTAxis = fig.add_subplot(gs_dsp[0]) +filterAxis = fig.add_subplot(gs_dsp[1]) + +# Plot type so that the size is dynamic +plt.tight_layout() + +# Select nice color pallet for graphs +colorSoftPurple = (168 / 255, 140 / 255, 220 / 255) +colorSoftBlue = (135 / 255, 206 / 255, 250 / 255) +colorSoftGreen = (122 / 255, 200 / 255, 122 / 255) + + + +# .CSV Setup ================================================== +# Get Directory of the .csv files +PACKAGE_DIR = os.path.dirname(os.path.dirname(os.path.abspath(__file__))) +ACOUSTICS_CSV_FILE_DIR = PACKAGE_DIR + "/acoustics_data" + +# List of all the acoustic files +acousticsCSVFiles = csv_files = glob.glob(ACOUSTICS_CSV_FILE_DIR + "/acoustics_data_" + "*.csv") + +# Get the latest csv file name for acoustics data +acousticsCSVFile = max(acousticsCSVFiles, key=os.path.getctime) + + + +def convertPandasObjectToIntArray(pandasObject): + pandasString = pandasObject.iloc[0].strip("array('i', ").rstrip(')') + pandasIntArray = [int(x.strip()) for x in pandasString.strip('[]').split(',')] + + return pandasIntArray + +def convertPandasObjectToFloatArray(pandasObject): + pandasString = pandasObject.iloc[0].strip("array('f', ").rstrip(')') + pandasFloatArray = [float(x.strip()) for x in pandasString.strip('[]').split(',')] + + return pandasFloatArray + + + +def getAcousticsData(): + # Variables that will be filled with latest acoustics data ---------- + hydrophone1 = [0] * hydrophoneDataSize + hydrophone2 = [0] * hydrophoneDataSize + hydrophone3 = [0] * hydrophoneDataSize + hydrophone4 = [0] * hydrophoneDataSize + hydrophone5 = [0] * hydrophoneDataSize + + unfilteredData = [0] * DSPDataSize + filteredData = [0] * DSPDataSize + FFTData = [0] * DSPDataSize + peaksData = [0] * DSPDataSize + FFTAmplitudeData = [0] * DSPDataSize + FFTFrequencyData = [0] * DSPDataSize + peaksAmplitudeData = [0] * DSPDataSize + peaksFrequencyData = [0] * DSPDataSize + + tdoaData = [0.0] * TDOADataSize + positonData = [0.0] * positionDataSize + + # Read latest acoustics data ---------- + acousticsDataFrame = pd.read_csv(acousticsCSVFile) + latestAcousticsData = acousticsDataFrame.tail(1) + + try: + # Get latest hydrophone data + hydrophone1 = convertPandasObjectToIntArray(latestAcousticsData["Hydrophone1"]) + hydrophone2 = convertPandasObjectToIntArray(latestAcousticsData["Hydrophone2"]) + hydrophone3 = convertPandasObjectToIntArray(latestAcousticsData["Hydrophone3"]) + hydrophone4 = convertPandasObjectToIntArray(latestAcousticsData["Hydrophone4"]) + hydrophone5 = convertPandasObjectToIntArray(latestAcousticsData["Hydrophone5"]) + + # Unfiltered data is special as it is the same as Hydrohone 1 first 1024 values + # This is because Acoustics PCB uses Hydrophone 1 to perform DSP + # Hydrohones have a ring buffer the size of 3 buffers each containing 1024 values (2^10) + # We always use the first ring buffer of Hydrophone 1 to performe DSP + # That is why unfiltered data is the same as Hydrphne 1 first buffer + unfilteredData = hydrophone1[0:1024] + + # Get DSP data + filteredData = convertPandasObjectToIntArray(latestAcousticsData["FilterResponse"]) # Also known as Filter response to the raw unfiltered data + FFTData = convertPandasObjectToIntArray(latestAcousticsData["FFT"]) + peaksData = convertPandasObjectToIntArray(latestAcousticsData["Peaks"]) + + # Get multilateration data + tdoaData = convertPandasObjectToFloatArray(latestAcousticsData["TDOA"]) + positonData = convertPandasObjectToFloatArray(latestAcousticsData["Position"]) + except: + print("ERROR: Coulden't read acoustics data") + + # Post process DSP data to desired scale and amount ---------- + # 1. Convert FFTData to its corresponding frequency amount + # 2. Cut out big FFT frequencies out as they are not relevant + # 3. Cut out big peak frequencies as they are not relevant + sampleLength = len(FFTData) + maxFrequencyIndex = int(MAX_FREQUENCY_TO_SHOW * sampleLength / SAMPLE_RATE) + + FFTAmplitudeData = FFTData[0:maxFrequencyIndex] + FFTFrequencyData = [(i * (SAMPLE_RATE / sampleLength)) for i in range(sampleLength)] + FFTFrequencyData = FFTFrequencyData[0:maxFrequencyIndex] + + # Peaks data is special as each peak data value is a array of [Amplitude, Frequency, Phase] of the peak + # We want to get amplitude and frequency, dont really care about the phase + try: + tempAmplitude = [] + tempFrequency = [] + for i in range(1, len(peaksData), 3): + if peaksData[i] < MAX_FREQUENCY_TO_SHOW: + tempAmplitude += [peaksData[i - 1]] + tempFrequency += [peaksData[i]] + + peaksAmplitudeData = tempAmplitude + peaksFrequencyData = tempFrequency + except: + print("ERROR processing DSP data") + + # return processed data ---------- + return [ + hydrophone1, + hydrophone2, + hydrophone3, + hydrophone4, + hydrophone5, + + unfilteredData, + + filteredData, + FFTAmplitudeData, + FFTFrequencyData, + peaksAmplitudeData, + peaksFrequencyData, + + tdoaData, + positonData, + ] + + + +def display_live_data(frame): + # Get latest acoustics data + acousticsData = getAcousticsData() + + # Set the lates acoustics data in apropriate variables + hydrophoneData = [ + acousticsData[0], # Hydrophone 1 + acousticsData[1], # Hydrophone 2 + acousticsData[2], # Hydrophone 3 + acousticsData[3], # Hydrophone 4 + acousticsData[4], # Hydrophone 5 + ] + + unfilteredData = acousticsData[5] + + filterData = acousticsData[6] + FFTAmplitudeData = acousticsData[7] + FFTFrequencyData = acousticsData[8] + peaksAmplitudeData = acousticsData[9] + peaksFrequencyData = acousticsData[10] + + tdoaData = acousticsData[11] # Currently not in use + positionData = acousticsData[12] # Currently not in use + + # Plot hydrophone data + for i in range(5): + xHydrophone = list(range(len(hydrophoneData[i][::]))) + hydrophoneAxis[i].clear() + hydrophoneAxis[i].plot( + xHydrophone, + hydrophoneData[i], + label=f"Hydrophone {i + 1}", + color=colorSoftBlue, + alpha=1, + ) + hydrophoneAxis[i].legend(loc="upper right", fontsize="xx-small") + + # Plot Filter response + xRaw = list(range(len(unfilteredData))) + xFilter = list(range(len(filterData))) + filterAxis.clear() + filterAxis.set_title("Filter response") + filterAxis.plot(xRaw, unfilteredData, label="Raw", color=colorSoftBlue, alpha=0.5) + filterAxis.plot( + xFilter, filterData, label="Filter", color=colorSoftGreen, alpha=0.7 + ) + filterAxis.legend(loc="upper right", fontsize="xx-small") + + # Plot FFT data + FFTAxis.clear() + FFTAxis.set_title("FFT") + FFTAxis.set_xlabel("Frequency [Hz]") + FFTAxis.set_ylabel("Amplitude") + FFTAxis.bar( + FFTFrequencyData, + FFTAmplitudeData, + label="FFT", + color=colorSoftPurple, + alpha=1, + width=500, + ) + FFTAxis.scatter( + peaksFrequencyData, + peaksAmplitudeData, + label="Peaks", + color="red", + alpha=0.7, + s=30, + linewidths=1.4, + marker="x", + ) + FFTAxis.legend(loc="upper right", fontsize="xx-small") + + # Print out the unused Multilateration data + print(f"TDOA Data: {tdoaData} | Position Data: {positionData}") + +# Plotting live data +ani = animation.FuncAnimation(fig, display_live_data, interval=1000/FPS) +plt.show() + diff --git a/acoustics/acoustics_interface/CMakeLists.txt b/acoustics/acoustics_interface/CMakeLists.txt new file mode 100644 index 000000000..82d472cbc --- /dev/null +++ b/acoustics/acoustics_interface/CMakeLists.txt @@ -0,0 +1,31 @@ +cmake_minimum_required(VERSION 3.8) +project(acoustics_interface) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(rclpy REQUIRED) +find_package(std_msgs REQUIRED) + +ament_python_install_package(acoustics_interface) + +# Install launch files. +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME} +) + +# Executables +install(PROGRAMS + ${PROJECT_NAME}/acoustics_interface_node.py + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() \ No newline at end of file diff --git a/acoustics/acoustics_interface/README b/acoustics/acoustics_interface/README new file mode 100644 index 000000000..25c1a8eac --- /dev/null +++ b/acoustics/acoustics_interface/README @@ -0,0 +1,2 @@ +Interface to comunicate with acoustics PCB and Teensy 4.1 MCU +OBS!: Make sure to connect Teensy 4.1 MCU to the ethernet \ No newline at end of file diff --git a/acoustics/acoustics_interface/acoustics_interface/__init__.py b/acoustics/acoustics_interface/acoustics_interface/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py new file mode 100644 index 000000000..50d43c994 --- /dev/null +++ b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py @@ -0,0 +1,287 @@ +# Setting up libraries +import os +import sys +from socket import * +import netifaces as ni +from enum import Enum +import errno +import time + + +class TeensyCommunicationUDP: + """ + This class is responsible for the RPI side of teensy-RPI UDP communication. It is + implemented with a singleton pattern for convenience. + + Note: Private members are denoted by _member_name + + Attributes: + ----------- + _TEENSY_IP (string): self-explanatory + _TEENSY_PORT (int): teensy's data port + _MY_PORT (int): the device's data port + _MAX_PACKAGE_SIZE_RECEIVED (int): max size (in bytes) of UDP receive buffer + _TIMEOUT (int): socket timeout when reading data + _clientSocket (socket): UDP socket for teensy communication + _timeoutMax (int): time to wait before retrying handshake + _data_string (str): buffer for received teensy data + _data_target (str): the field of `acoustics_data` that is written to + acoustics_data (dict[str, list[int]]): containter for data from teensy + + Methods: + -------- + init_communication(frequenciesOfInterest: list[tuple[int, int]]) -> None: + Sets up socket for communication with teensy and waits for handshake + fetch_data() -> None: + Reads a full data message from teensy and saves it + _write_to_target() -> None: + Writes _data_string to the correct field of acoustics_data + _parse_data_string(is_float: bool) -> list[float] | list[int] | None: + Converts _data_string into a list of either floats or integers + _get_ip() -> None: + Gets the IP address of the device + _send_acknowledge_signal() -> None: + Sends the _INITIALIZATION_MESSAGE to teensy + _check_if_available() -> None: + Checks the UDP message buffer for a READY message + _send_frequencies_of_interest(frequenciesOfInterest: list[tuple[float, float]]) -> None: + Sends the list of frequencies and variances to teensy + + """ + # Teensy networking Setup + _TEENSY_IP = "10.0.0.111" + _TEENSY_PORT = 8888 + _MY_PORT = 9999 + _MAX_PACKAGE_SIZE_RECEIVED = 65536 + _TIMEOUT = 1 + _address = (_TEENSY_IP, _TEENSY_PORT) + + _INITIALIZATION_MESSAGE = "HELLO :D" # This is a message only sent once to establish 2 way communication between Teensy and client + + _clientSocket = socket(AF_INET, SOCK_DGRAM) + + _timeoutMax = 10 + _data_string = "" + _data_target = "" + acoustics_data = { + "HYDROPHONE_1": [0], + "HYDROPHONE_2": [0], + "HYDROPHONE_3": [0], + "HYDROPHONE_4": [0], + "HYDROPHONE_5": [0], + "SAMPLES_FILTERED": [0], + "FFT": [0], + "PEAK": [0], + "TDOA": [0], + "LOCATION": [0] + } + + @classmethod + def init_communication(cls, frequenciesOfInterest: list[tuple[int, int]]) -> None: + """ + Sets up communication with teensy + + Parameters: + frequenciesOfInterest (list[tuple[int, int]]): List of frequencies to look for + """ + assert len(frequenciesOfInterest) == 10, "Frequency list has to have exactly 10 entries" + + _frequenciesOfInterest = frequenciesOfInterest + + cls.MY_IP = cls._get_ip() + + # Socket setup + cls._clientSocket.settimeout(cls._TIMEOUT) + cls._clientSocket.bind((cls.MY_IP, cls._MY_PORT)) + cls._clientSocket.setblocking(False) + + cls._send_acknowledge_signal() + timeStart = time.time() + + # Wait for READY signal + while not cls._check_if_available(): + print("Did not receive READY signal. Will wait.") + time.sleep(1) + + if time.time() - timeStart > cls._timeoutMax: + print("Gave up on receiving READY. Sending acknowledge signal again") + # Start over + timeStart = time.time() + cls._send_acknowledge_signal() + + print("READY signal received, sending frequencies...") + cls._send_frequencies_of_interest(frequenciesOfInterest) + + @classmethod + def fetch_data(cls) -> None: + """ + Gets data from teensy and stores it in `acoustics_data` + """ + i = 0 + + while True: + data = cls._get_raw_data() + + if data == None: + return + + if data not in cls.acoustics_data.keys(): + cls._data_string += data + else: + cls._write_to_target() + cls._data_target = data + + # Ah yes, my good friend code safety + i += 1 + + if i > 1000: + i = 0 + print("Max tries exceeded") + break + + @classmethod + def _write_to_target(cls) -> None: + """ + Writes to the current target in `acoustics_data` and clears the data string + """ + if cls._data_target == "TDOA" or cls._data_target == "LOCATION": + data = cls._parse_data_string(is_float=True) + else: + data = cls._parse_data_string(is_float=False) + + if data == None: + cls._data_string = "" + return + + cls.acoustics_data[cls._data_target] = data + + cls._data_string = "" + + @classmethod + def _get_raw_data(cls) -> str | None: + """ + Gets a message from teensy + + Returns: + The message in the UDP buffer if there is one + """ + try: + rec_data, _ = cls._clientSocket.recvfrom(cls._MAX_PACKAGE_SIZE_RECEIVED) + messageReceived = rec_data.decode() + return messageReceived + except error as e: # `error` is really `socket.error` + if e.errno == errno.EWOULDBLOCK: + pass + else: + print("Socket error: ", e) + + @classmethod + def _parse_data_string(cls, is_float: bool) -> list[float] | list[int] | None: + """ + Converts _data_string to a list + + Parameters: + is_float (bool): whether _data_string should be seen as a list of floats or ints + + Returns: + The converted list + """ + if cls._data_string == '': return + + try: + # Format data from CSV string to floats, ignore last value + if is_float: + return list(map(float, cls._data_string.split(",")[:-1])) + else: + return list(map(int, cls._data_string.split(",")[:-1])) + except Exception as e: + print(f"The string '{cls._data_string}' caused an error when parsing") + print(f"The exception was: {e}") + + # stackoverflow <3 + # https://stackoverflow.com/questions/166506/finding-local-ip-addresses-using-pythons-stdlib + @classmethod + def _get_ip(cls) -> None: + """ + Gets the device's IP address + """ + s = socket(AF_INET, SOCK_DGRAM) + s.settimeout(0) + try: + # doesn't even have to be reachable + s.connect((cls._TEENSY_IP, 1)) + IP = s.getsockname()[0] + except Exception: + IP = '127.0.0.1' + finally: + s.close() + + return IP + + @classmethod + def _send_acknowledge_signal(cls) -> None: + """ + Sends "HELLO :D to teensy + """ + try: + cls._clientSocket.sendto(cls._INITIALIZATION_MESSAGE.encode(), cls._address) + print("DEBUGING: Sent acknowledge package") + except Exception as e: + print("Error from send_acknowledge_signal") + print(e) + pass + + @classmethod + def _check_if_available(cls) -> None: + """ + Checks if READY has been received + + Note: The while loop here may not be necessary, it is just there to make absolutely sure that *all* + the data in the UDP buffer is read out when waiting for ready signal, to avoid strange bugs + """ + try: + i = 0 + while True: + # Read data + message = cls._get_raw_data() + # Check if there is no more data left + if message == None: + return False + + # Check if correct signal was sent + if message == "READY": + return True + + i += 1 + + if i > 200: + i = 0 + print("Max tries exceeded") + break + except Exception as e: + print(f"check_if_available rased exception: {e}") + return False + + @classmethod + def _send_frequencies_of_interest(cls, frequenciesOfInterest: list[tuple[float, float]]) -> None: + """ + Sends the list of frequencies with variance to teensy + + Parameters: + frequenciesOfInterest (list[tuple[float, float]]): The list of frequencies w/ variance + """ + try: + + # Format (CSV): xxx,x,xx,x...,x (frequency list comes first, then variances) + assert len(frequenciesOfInterest) == 10, "List of frequencies has to be ten entries long!" + + # ten messages in total, one message for each entry to work around the max packet size + for (frequency, variance) in frequenciesOfInterest: + frequency_variance_msg = f"{str(frequency)},{str(variance)}," + + # print(self.address); + cls._clientSocket.sendto(frequency_variance_msg.encode(), cls._address) + except: + print("Couldn't send Frequency data") + + diff --git a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py new file mode 100644 index 000000000..d448421d4 --- /dev/null +++ b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py @@ -0,0 +1,98 @@ +#!/usr/bin/python3 + +import rclpy +from rclpy.node import Node +import rclpy.logging +from std_msgs.msg import Int32MultiArray, Float32MultiArray +from acoustics_interface.acoustics_interface_lib import TeensyCommunicationUDP + +class AcousticsInterfaceNode(Node): + """ + Publishes Acoustics data to ROS2 + + Methods: + data_update() -> None: + calls fetch_data() from acoustics_interface + data_publisher(self) -> None: + publishes data to ROS2 topics + """ + def __init__(self) -> None: + """ + Sets up acoustics logging and publishers, also sets up teensy communication + """ + super().__init__('acoustics_interface') + rclpy.logging.initialize() + + self._hydrophone_1_publisher = self.create_publisher(Int32MultiArray, '/acoustics/hydrophone1', 5) + self._hydrophone_2_publisher = self.create_publisher(Int32MultiArray, '/acoustics/hydrophone2', 5) + self._hydrophone_3_publisher = self.create_publisher(Int32MultiArray, '/acoustics/hydrophone3', 5) + self._hydrophone_4_publisher = self.create_publisher(Int32MultiArray, '/acoustics/hydrophone4', 5) + self._hydrophone_5_publisher = self.create_publisher(Int32MultiArray, '/acoustics/hydrophone5', 5) + + self._filter_response_publisher = self.create_publisher(Int32MultiArray, '/acoustics/filter_response', 5) + self._fft_publisher = self.create_publisher(Int32MultiArray, '/acoustics/fft', 5) + self._peak_publisher = self.create_publisher(Int32MultiArray, '/acoustics/peaks', 5) + self._tdoa_publisher = self.create_publisher(Float32MultiArray, '/acoustics/time_difference_of_arrival', 5) + self._position_publisher = self.create_publisher(Float32MultiArray, '/acoustics/position', 5) + + + # Logs all the newest data + self.declare_parameter("acoustics.data_logging_rate", 1.0) # Providing a default value 1.0 => 1 samplings per second, verry slow + DATA_LOGING_RATE = self.get_parameter("acoustics.data_logging_rate").get_parameter_value().double_value + timer_period = 1.0/DATA_LOGING_RATE + + self._timer_data_update = self.create_timer(0.001, self.data_update) + self._timer_data_publisher = self.create_timer(timer_period, self.data_publisher) + + # Declare Frequency of interest parameters to send to Acoustics PCB to look out for + # This list has to be exactly 10 entries long (20 elements (10 frequencies + 10 variances)) + # format [(FREQUENCY, FREQUENCY_VARIANCE), ...] + self.declare_parameter("acoustics.frequencies_of_interest", [0] * 20) + FREQUENCIES_OF_INTEREST_PARAMETERS = self.get_parameter("acoustics.frequencies_of_interest").get_parameter_value().integer_array_value + + frequenciesOfInterest = [] + for i in range(0, len(FREQUENCIES_OF_INTEREST_PARAMETERS), 2): + frequenciesOfInterest += [(FREQUENCIES_OF_INTEREST_PARAMETERS[i], FREQUENCIES_OF_INTEREST_PARAMETERS[i + 1])] + + # Initialize comunication with Acoustics PCB + self.get_logger().info("Initializing comunication with Acoustics") + self.get_logger().info("Acoustics PCB MCU IP: 10.0.0.111") + self.get_logger().info("Trying to connect...") + + TeensyCommunicationUDP.init_communication(frequenciesOfInterest) + + self.get_logger().info("Sucsefully connected to Acoustics PCB MCU :D") + + + def data_update(self) -> None: + TeensyCommunicationUDP.fetch_data() + + def data_publisher(self) -> None: + """Publishes to topics""" + self._hydrophone_1_publisher.publish(Int32MultiArray(data=TeensyCommunicationUDP.acoustics_data["HYDROPHONE_1"])) + self._hydrophone_2_publisher.publish(Int32MultiArray(data=TeensyCommunicationUDP.acoustics_data["HYDROPHONE_2"])) + self._hydrophone_3_publisher.publish(Int32MultiArray(data=TeensyCommunicationUDP.acoustics_data["HYDROPHONE_3"])) + self._hydrophone_4_publisher.publish(Int32MultiArray(data=TeensyCommunicationUDP.acoustics_data["HYDROPHONE_4"])) + self._hydrophone_5_publisher.publish(Int32MultiArray(data=TeensyCommunicationUDP.acoustics_data["HYDROPHONE_5"])) + + self._filter_response_publisher.publish(Int32MultiArray(data=TeensyCommunicationUDP.acoustics_data["SAMPLES_FILTERED"])) + self._fft_publisher.publish(Int32MultiArray(data=TeensyCommunicationUDP.acoustics_data["FFT"])) + self._peak_publisher.publish(Int32MultiArray(data=TeensyCommunicationUDP.acoustics_data["PEAK"])) + + self._tdoa_publisher.publish(Float32MultiArray(data=TeensyCommunicationUDP.acoustics_data["TDOA"])) + self._position_publisher.publish(Float32MultiArray(data=TeensyCommunicationUDP.acoustics_data["LOCATION"])) + +def main(args=None): + rclpy.init(args=args) + + node = AcousticsInterfaceNode() + + rclpy.spin(node) + + node.destroy_node() + rclpy.shutdown() + pass + +if __name__ == "__main__": + main() + \ No newline at end of file diff --git a/acoustics/acoustics_interface/launch/__init__.py b/acoustics/acoustics_interface/launch/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/acoustics/acoustics_interface/launch/acoustics_interface_launch.py b/acoustics/acoustics_interface/launch/acoustics_interface_launch.py new file mode 100644 index 000000000..0388f5061 --- /dev/null +++ b/acoustics/acoustics_interface/launch/acoustics_interface_launch.py @@ -0,0 +1,24 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import os + +def generate_launch_description(): + # Path to the YAML file + yaml_file_path = os.path.join( + get_package_share_directory("blackbox"), + "../../../../", # Go to the workspace + "src/vortex-asv/asv_setup/config/robots/", # Go inside where yamal files are located at + 'freya.yaml' + ) + + return LaunchDescription([ + Node( + package='acoustics_interface', + namespace='acoustics_interface', + executable='acoustics_interface_node.py', + name='acoustics_interface_node', + output='screen', + parameters=[yaml_file_path], + ), + ]) \ No newline at end of file diff --git a/acoustics/acoustics_interface/package.xml b/acoustics/acoustics_interface/package.xml new file mode 100644 index 000000000..fdb44caf9 --- /dev/null +++ b/acoustics/acoustics_interface/package.xml @@ -0,0 +1,19 @@ + + + + acoustics_interface + 1.0.0 + Communicates and gets data from acoustics PCB + vortex + MIT + + + rclpy + std_msgs + ros2launch + + + + ament_cmake + +