Merge branch 'development' of github.com:vortexntnu/vortex-asv into f…

…eature/bms_system

asv_setup/CMakeLists.txt

@@ -5,28 +5,20 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
-# find dependencies
 find_package(ament_cmake REQUIRED)
-# uncomment the following section in order to fill in
-# further dependencies manually.
-# find_package(<dependency> REQUIRED)
-
-# Install launch files.
-install(DIRECTORY
-  launch
-  DESTINATION share/${PROJECT_NAME}/
-)
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
-  # the following line skips the linter which checks for copyrights
-  # comment the line when a copyright and license is added to all source files
   set(ament_cmake_copyright_FOUND TRUE)
-  # the following line skips cpplint (only works in a git repo)
-  # comment the line when this package is in a git repo and when
-  # a copyright and license is added to all source files
   set(ament_cmake_cpplint_FOUND TRUE)
   ament_lint_auto_find_test_dependencies()
 endif()
 
+# Install launch files.
+install(DIRECTORY
+  config
+  launch
+  DESTINATION share/${PROJECT_NAME}/
+)
+
 ament_package()

asv_setup/config/robots/freya.yaml

@@ -0,0 +1,52 @@
+# This file defines parameters specific to Freya
+#
+# When looking at the ASV from above, the thruster placement is:   
+#    /\          /\
+#   /  \  front /  \
+#  /    \      /    \
+#  |=3↗=|      |=0↖=|
+#  |    |      |    |
+#  |    |      |    |
+#  |    |======|    |
+#  |    |      |    |
+#  |    |      |    |
+#  |    |      |    |
+#  |=2↖=|==||==|=1↗=|
+#
+/**:
+  ros__parameters:
+    physical:
+      mass_kg: 0 
+      displacement_m3: 0
+      buoyancy: 0   # kg
+      center_of_mass: [0, 0, 0]        # mm (x,y,z)
+      center_of_buoyancy: [0, 0, 0]   # mm (x,y,z)
+
+
+    propulsion:
+      dofs:
+        num: 3
+        which:
+          surge: true
+          sway:  true
+          yaw:   true
+      thrusters:
+        num: 4
+        min: -100
+        max: 100
+        configuration_matrix: #NED
+          [0.70711, 0.70711, 0.70711, 0.70711, 
+          -0.70711, 0.70711, -0.70711, 0.70711, 
+          0.27738, 0.27738, -0.27738, -0.27738]
+
+        rate_of_change:
+          max: 0 # Maximum rate of change in newton per second for a thruster
+        thruster_to_pin_map: [1, 3, 2, 0] # I.e. if thruster_to_pin = [1, 3, 2, 0] then thruster 0 is pin 1 etc..
+        direction: [1, 1, 1, 1] # Disclose during thruster mapping
+        offset: [0, 0, 0, 0] # Disclose during thruster mapping
+      command:
+        wrench:
+          max:     [150.0, 150.0, 150.0, 150.0, 150.0, 150.0]
+          scaling: [ 1.0,  1.0,  1.0, 1.0, 1.0, 1.0]
+
+    computer: "pc-debug"

asv_setup/launch/pc.launch.py

@@ -0,0 +1,42 @@
+import os
+from launch import LaunchDescription
+from launch.actions import SetEnvironmentVariable, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+
+def generate_launch_description():
+    # Set environment variable
+    set_env_var = SetEnvironmentVariable(
+        name='ROSCONSOLE_FORMAT',
+        value='[${severity}] [${time}] [${node}]: ${message}'
+    )
+
+    # Joystick node
+    joy_node = Node(
+        package='joy',
+        executable='joy_node',
+        name='joystick_driver',
+        output='screen',
+        parameters=[
+            {'deadzone': 0.15},
+            {'autorepeat_rate': 100.0},
+        ],
+        remappings=[
+            ('/joy', '/joystick/joy'),
+        ],
+    )
+
+    # Joystick interface launch
+    joystick_interface_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(get_package_share_directory('joystick_interface'), 'launch/joystick_interface.launch.py')
+        )
+    )
+
+    # Return launch description
+    return LaunchDescription([
+        set_env_var,
+        joy_node,
+        joystick_interface_launch
+    ])

asv_setup/launch/pc.launch.yaml

@@ -1,23 +0,0 @@
-launch:
-  - set_env:
-      name: ROSCONSOLE_FORMAT
-      value: "[${severity}] [${time}] [${node}]: ${message}"
-
-  - group:
-      - node:
-          pkg: joy
-          exec: joy_node
-          name: joystick_driver
-          output: screen
-          param:
-            - name: _deadzone
-              value: 0.15
-            - name: _autorepeat_rate
-              value: 100
-          remap:
-          -
-                from: "/joy"
-                to: "/joystick/joy"
-
-  - include:
-      file: "src/vortex-asv/mission/joystick_interface/launch/joystick_interface.launch.yaml" 

asv_setup/launch/tf.launch.py

@@ -0,0 +1,65 @@
+import math
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSDurabilityPolicy, QoSHistoryPolicy
+from launch.actions import ExecuteProcess
+
+
+def generate_launch_description():
+    # command line arguments to get desired QoS settings
+    echo_tf_static = ExecuteProcess(
+        cmd=['ros2', 'topic', 'echo', '--qos-durability', 'transient_local', '--qos-reliability', 'reliable', '/tf_static'],
+        output='screen'
+    )
+
+    # base_link (NED) to base_link (SEU) tf.
+    tf_base_link_ned_to_base_link_seu = Node(
+        package='tf2_ros',
+        name='base_link_ned_to_base_link_seu',
+        executable='static_transform_publisher',
+        arguments=['--x'              , '0',
+                   '--y'              , '0',
+                   '--z'              , '0',
+                   '--roll'           , '0',
+                   '--pitch'          , str(math.pi),
+                   '--yaw'            , '0',
+                   '--frame-id'       , 'base_link',
+                   '--child-frame-id' , 'base_link_SEU'],
+    )
+
+    tf_base_link_to_lidar = Node(
+        package='tf2_ros',
+        name='base_link_to_lidar',
+        executable='static_transform_publisher',
+        arguments=['--x'              , '-0.0145',
+                   '--y'              , '0',
+                   '--z'              , '-0.392425',
+                   '--roll'           , str(math.pi),
+                   '--pitch'          , '0',
+                   '--yaw'            , '0',
+                   '--frame-id'       , 'base_link',
+                   '--child-frame-id' , 'os_lidar'],
+    )
+
+    # base_link to zed2i_camera_center tf.
+    tf_base_link_to_zed2_camera_center = Node(
+        package='tf2_ros',
+        name='base_link_to_zed2_camera_center',
+        executable='static_transform_publisher',
+        arguments=['--x'              , '0.3005',
+                   '--y'              , '0',
+                   '--z'              , '-0.22036',
+                   '--roll'           , str(math.pi),
+                   '--pitch'          , '0',
+                   '--yaw'            , '0',
+                   '--frame-id'       , 'base_link',
+                   '--child-frame-id' , 'zed2i_camera_center'],
+    )
+
+    return LaunchDescription([
+        tf_base_link_ned_to_base_link_seu,
+        tf_base_link_to_lidar,
+        tf_base_link_to_zed2_camera_center,
+        echo_tf_static
+    ])

mission/internal_status/README.md

@@ -0,0 +1,22 @@
+# Internal Status 
+
+This package contains a library which allows us to get current and voltage values from the Power Sense Module (PSM), and a ros node which publishes this data to the whole ROS environment.
+
+## PSM lib
+
+The ASV as a PSM. It takes voltage and current data going into internal electrical instruments on board. This library is a driver, which allows us to get the voltage (in V) and current (in A) values from PSM through I2C.
+The I2C address is 0X69.
+* Typical values for voltage: around 21V
+* Typical values for current: around 0.6A
+
+
+## PSM node
+
+This ros node just publishes the current and voltage data we get from the lib to the entire ROS environment.
+### Publishes to 
+* /asv/power_sense_module/current for the current data [A]
+* /asv/power_sense_module/voltage for the voltage data [V]
+
+
+
+

mission/internal_status/internal_status/power_sense_module_lib.py

@@ -0,0 +1,52 @@
+import smbus
+from MCP342x import MCP342x
+
+class PowerSenseModule:
+
+    def __init__(self):
+
+        # Parameters
+        # to read voltage and current from ADC on PDB through I2C
+        self.i2c_adress = 0x69   
+
+        # init of I2C bus communication
+        self.bus = smbus.SMBus(1)
+        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   
+
+
+        # Convertion ratios taken from PSM datasheet at: https://bluerobotics.com/store/comm-control-power/control/psm-asm-r2-rp/
+        self.psm_to_battery_voltage = 11.0  # V/V
+        self.psm_to_battery_current_scale_factor = 37.8788  # A/V
+        self.psm_to_battery_current_offset = 0.330  # V  
+
+
+    def get_voltage(self):
+        # Sometimes an I/O timeout or error happens, it will run again when the error disappears
+        try:  
+            system_voltage = (self.channel_voltage.convert_and_read() *
+                             self.psm_to_battery_voltage)
+            return system_voltage
+
+        except IOError:
+            return
+
+    def get_current(self):
+        try:
+            system_current = (self.channel_current.convert_and_read() - 
+                            self.psm_to_battery_current_offset
+                            ) * self.psm_to_battery_current_scale_factor
+
+            return system_current
+
+        except IOError:
+            return
+
+    def shutdown(self):
+        self.bus.close()

mission/internal_status/internal_status/power_sense_module_publisher.py

@@ -0,0 +1,48 @@
+import internal_status.power_sense_module_lib
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import Float32
+
+
+class MinimalPublisher(Node):
+
+
+    def __init__(self):
+        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', 1)
+        self.publisher_voltage = self.create_publisher(Float32, '/asv/power_sense_module/voltage', 1)
+        timer_period = 0.5
+        self.timer = self.create_timer(timer_period, self.timer_callback)
+
+
+    def timer_callback(self):
+        current = Float32()
+        voltage = Float32()
+        #call the two functions of the power_sense_module_lib to get the current and voltage values of the PSM
+        current.data = self.PSM.get_current()
+        voltage.data = self.PSM.get_voltage()
+        self.publisher_current.publish(current)  #publish current value to the "current topic" 
+        self.get_logger().info('Publishing PSM current: "%s"' % current.data)
+        self.publisher_voltage.publish(voltage)  #publish voltage value to the "voltge topic"
+        self.get_logger().info('Publishing PSM voltage: "%s"' % voltage.data)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    minimal_publisher = MinimalPublisher()
+
+    rclpy.spin(minimal_publisher)
+
+    # Destroy the node explicitly
+    # (optional - otherwise it will be done automatically
+    # when the garbage collector destroys the node object)
+    minimal_publisher.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()
+
+

mission/internal_status/launch/internal_status_launch.py

@@ -0,0 +1,10 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node 
+
+def generate_launch_description():
+    return LaunchDescription([
+        Node(
+            package = 'internal_status',
+            executable = 'power_sense_module_publisher'
+        )
+    ])

mission/internal_status/package.xml

@@ -0,0 +1,26 @@
+<?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>internal_status</name>
+  <version>0.0.0</version>
+  <description>Publisher of the PSM voltage and current data</description>
+  <maintainer email="[email protected]">vortex</maintainer>
+  <license>Apache License 2.0</license>
+
+
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>power_sense_module_lib</exec_depend>
+  <exec_depend>ros2launch</exec_depend>
+
+
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

mission/internal_status/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/internal_status
+[install]
+install_scripts=$base/lib/internal_status