Added parameters from yamal file into the ros2 nodes for easier confi…

…guration

asv_setup/config/robots/freya.yaml

@@ -50,6 +50,9 @@
           scaling: [ 1.0,  1.0,  1.0, 1.0, 1.0, 1.0]
 
     blackbox:
-      data_logging_rate: 5.0 # Log all data 5 times per second
+      data_logging_rate: 5.0 # [logings/second], Recomended: 5.0 logings per second
+
+    internal_status:
+      psm_read_rate: 2.0 # [readings/second], Recomended 2.0 readings per second OBS! Try keeping under 2.0 as otherwise I2C comunication to Power Sense Module might become bad as psm needs time to sample data
 
     computer: "pc-debug"

mission/blackbox/blackbox/blackbox_node.py

@@ -71,9 +71,10 @@ def __init__(self):
         )
 
         # Logs all the newest data 10 times per second
-        self.declare_parameter("blackbox.data_logging_rate", 0.1)  # Providing a default value
+        self.declare_parameter("blackbox.data_logging_rate", 1.0)  # Providing a default value 1.0 => 1 samplings per second, verry slow
         DATA_LOGING_RATE = self.get_parameter("blackbox.data_logging_rate").get_parameter_value().double_value
-        self.logger_timer = self.create_timer(1.0/DATA_LOGING_RATE, self.logger)
+        timer_period = 1.0/DATA_LOGING_RATE
+        self.logger_timer = self.create_timer(timer_period, self.logger)
 
         # Debuging ----------
         self.get_logger().info(

mission/blackbox/launch/blackbox.launch.py

@@ -5,7 +5,6 @@
 
 def generate_launch_description():
     # Path to the YAML file
-    # If the YAML file is not within a package, specify the absolute path directly
     yaml_file_path = os.path.join(
         get_package_share_directory("blackbox"),
         "../../../../",  # Go to the workspace

mission/internal_status/internal_status/power_sense_module_lib.py

@@ -1,3 +1,4 @@
+import time
 import smbus
 from MCP342x import MCP342x
 
@@ -11,14 +12,15 @@ def __init__(self):
 
         # init of I2C bus communication
         self.bus = smbus.SMBus(1)
+        time.sleep(1) # A short pause because sometimes I2C is slow to conect
         self.channel_voltage = MCP342x(self.bus,
                                        self.i2c_adress,
                                        channel=1,
                                        resolution=18)  # voltage
         self.channel_current = MCP342x(self.bus,
                                        self.i2c_adress,
                                        channel=0,
-                                       resolution=18)  # current   
+                                       resolution=18)  # current  
 
 
         # Convertion ratios taken from PSM datasheet at: https://bluerobotics.com/store/comm-control-power/control/psm-asm-r2-rp/
@@ -35,7 +37,7 @@ def get_voltage(self):
             return system_voltage
 
         except IOError as e:
-            return -1
+            return e
 
     def get_current(self):
         try:
@@ -46,7 +48,7 @@ def get_current(self):
             return system_current
 
         except IOError as e:
-            return -1
+            return e
 
     def shutdown(self):
         self.bus.close()

mission/internal_status/internal_status/power_sense_module_publisher.py

@@ -8,11 +8,17 @@ class MinimalPublisher(Node):
 
 
     def __init__(self):
+        # Node setup ----------
         super().__init__('PSM_publisher')
         self.PSM = internal_status.power_sense_module_lib.PowerSenseModule()
+
         self.publisher_current = self.create_publisher(Float32, '/asv/power_sense_module/current', 5)
         self.publisher_voltage = self.create_publisher(Float32, '/asv/power_sense_module/voltage', 5)
-        timer_period = 0.5
+
+        # Data gathering cycle ----------
+        self.declare_parameter("internal_status.psm_read_rate", 1.0)  # Providing a default value 1.0 => 1 second delay per data gathering
+        psm_read_rate = self.get_parameter("internal_status.psm_read_rate").get_parameter_value().double_value
+        timer_period = 1.0/psm_read_rate
         self.timer = self.create_timer(timer_period, self.timer_callback)
 
 

mission/internal_status/launch/internal_status_launch.py

@@ -1,10 +1,22 @@
 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 = 'internal_status',
-            executable = 'power_sense_module_publisher'
+            executable = 'power_sense_module_publisher',
+            output='screen',
+            parameters=[yaml_file_path],
         )
     ])

mission/internal_status/setup.py

@@ -3,6 +3,7 @@
 from setuptools import find_packages, setup
 
 package_name = 'internal_status'
+yaml_file_dir = "asv_setup/"
 
 setup(
     name=package_name,
@@ -12,7 +13,8 @@
         ('share/ament_index/resource_index/packages',
             ['resource/' + package_name]),
         ('share/' + package_name, ['package.xml']),
-        (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*')))
+        (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))),
+        (os.path.join('share', yaml_file_dir, 'config/robots'), glob('robots/*.yaml')),
     ],
     install_requires=['setuptools'],
     zip_safe=True,