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
+
+