feat(#128): changed some code

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()

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)

acoustics/acoustics_data_record/acoustics_data/.gitignore

@@ -0,0 +1 @@
+*

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,
+            ])

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()

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],
+        ),
+    ])

acoustics/acoustics_data_record/package.xml

@@ -0,0 +1,19 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>acoustics_data_record</name>
+  <version>1.0.0</version>
+  <description>Records data from the acoustics</description>
+  <maintainer email="[email protected]">vortex</maintainer>
+  <license>MIT</license>
+
+  <!-- Execution dependencies -->
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>ros2launch</exec_depend>
+
+  <!-- Export the build type as ament_cmake for CMake projects -->
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>