Merge pull request #85 from vortexntnu/feature/njord

Feature/njord

asv_setup/config/params/gnc.yaml

@@ -20,6 +20,6 @@ controllers:
 
 joystick:
   scaling:
-    surge: 0
-    sway: 0
-    yaw: 0
+    surge: 100.0
+    sway: 100.0
+    yaw: 100.0

asv_setup/config/robots/ASV.yaml

@@ -0,0 +1,13 @@
+failsafe:
+  publishers:
+    failSafeStatus: '/failSafeStatus'
+    hardwareOperationMode: '/hardWareOperationMode'
+  subscribers:
+    softWareKillSwitch: '/softWareKillSwitch'
+    softWareOperationMode: '/softWareOperationMode'
+
+  gpio:
+    gpioSoftWareKillSwitch: 13      #pin 33
+    gpioSoftWareOperationMode: 18    #pin 31
+    gpioFailSafeStatus: 15         #pin 35
+    gpioHardWareOperationMode: 16   #pin 29

asv_setup/config/robots/naglfar.yaml

@@ -31,7 +31,7 @@ propulsion:
       yaw:   true
   thrusters:
     num: 4
-    configuration_matrix_NED:
+    configuration_matrix: #NED
       [[ 0.70711,   0.70711,   0.70711,   0.70711], # Surge
        [ -0.70711,  0.70711,   -0.70711,  0.70711], # Sway
        [ 0.27738,   0.27738,  -0.27738,  -0.27738]] # Yaw
@@ -43,12 +43,12 @@ propulsion:
 
     rate_of_change:
       max: 0 # Maximum rate of change in newton per second for a thruster
-    map: [0, 1, 2, 3] # Disclose during thruster mapping
+    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:     [33.9, 33.9, 48.0, 7.6, 3.2, 9.9]
-      scaling: [ 0.4,  0.4,  0.6, 0.3, 0.7, 0.4]
+      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/config/robots/temp_config.yaml

@@ -46,7 +46,7 @@ propulsion:
 
     rate_of_change:
       max: 1 # Maximum rate of change in newton per second for a thruster
-    map: [0, 1, 2, 3]  
+    map: [3,1,0,2]  
     direction: [1,  1,  1, 1]
     offset: [0, 0, 0, 0] # Offset IN PWM!
     thrust_range: [-0.7, 0.7] # range in percentage -1.0 to 1.0 # NOTE!: Thrust_range moves with offset
@@ -98,4 +98,4 @@ joystick:
     heave: 60
     roll: 35
     pitch: -30
-    yaw: 20
+    yaw: 20

asv_setup/config/topics.yaml

@@ -6,8 +6,7 @@ asv:
     output: "/thrust/thruster_forces" # Actuator forces
 
   thruster_manager: 
-    torque: "/thrust/torque_input"
-    force: "/thrust/force_input"
+    wrench: "/thrust/wrench_input"
     output: "/thrust/thruster_forces" # Actuator forces
 
 guidance_interface:

asv_setup/launch/freya.launch

@@ -0,0 +1,42 @@
+<?xml version="1.0"?>
+<launch>
+    <env name="ROSCONSOLE_FORMAT" value="[${severity}] [${time}] [${node}]: ${message}" />
+
+    <!-- Input arguments -->
+    <arg name="type" default="real"/> <!-- real | simulator -->
+
+    <!-- General parameters -->
+    <rosparam command="load" file="$(find asv_setup)/config/environments/trondheim_freshwater.yaml"/>
+    <rosparam command="load" file="$(find asv_setup)/config/params/gnc.yaml"/>
+    <rosparam command="load" file="$(find asv_setup)/config/robots/temp_config.yaml"/>
+    <rosparam command="load" file="$(find asv_setup)/config/topics.yaml"/>
+
+    <!-- Transforms -->
+    <node pkg="tf2_ros" type="static_transform_publisher" name="base_to_mru_broadcaster"
+        args="0.1 0 0 0 0 0  body mru" />
+
+    <node pkg="tf2_ros" type="static_transform_publisher" name="base_to_gnss_broadcaster"
+        args="0.1 0 0 0 0 0  body gnss" />
+
+    <!-- Sensors -->
+    <node pkg="seapath_gnss_ros_driver" type="seapath_gnss_ros_driver_node" name="seapath_gnss_ros_driver_node" output="screen"/>
+    <node pkg="mru_ros_driver" type="mru_ros_driver_node" name="mru_ros_driver_node" output="screen"/>
+
+    <node pkg="seapath_ahrs" type="seapath_ahrs_node" name="seapath_ahrs_node" output="screen"/>
+    <node pkg="seapath_ekf" type="seapath_ekf_node" name="seapath_ekf_node" output="screen"/>
+
+    <!-- Motion -->
+    <rosparam file="$(find lqr_controller)/params/config.yaml" command="load" />
+    <node pkg="lqr_controller" type="lqr_controller_node.py" name="lqr_controller_node" output="screen" />
+
+    <node pkg="thruster_allocator"  type="thruster_allocator_node" name="allocator"  output="screen"/>
+    <node pkg="thruster_interface"  type="thruster_interface_node" name="thruster_interface_node"  output="screen"/>
+
+    <!-- Navigation -->
+    <rosparam file="$(find lqr_path_following)/params/config.yaml" command="load" />
+    <node pkg="lqr_path_following" type="lqr_path_following_node.py" name="lqr_path_following" output="screen" />
+
+    <!-- Mission: Consider moving to separate launch -->
+    <!-- <node pkg="finite_state_machine" type="lqr_box_test.py" name="lqr_box_test" output="screen"/> -->
+
+</launch>

asv_setup/launch/pc.launch

@@ -3,9 +3,13 @@
   <env name="ROSCONSOLE_FORMAT" value="[${severity}] [${time}] [${node}]: ${message}" />
   <arg name="ns_joystick"   default="joystick"/>
 
+  <rosparam command="load" file="$(find asv_setup)/config/params/gnc.yaml"/>
+
   <!-- Joystick nodes -->
   <group ns="$(arg ns_joystick)">
     <node pkg="joy"                name="joystick_driver"    type="joy_node"              output="screen" args="_deadzone:=0.15 _autorepeat_rate:=100"/>
   </group>
 
+  <node pkg="joystick_interface" type="joystick_interface.py" name="joystick_interface" output="screen"/>
+
 </launch>

asv_setup/launch/tf.launch

@@ -0,0 +1,21 @@
+<?xml version="1.0" ?>
+<launch>
+    <arg name="PI" value="3.14159265"/>
+
+    <arg name="PI_OVER_TWO"             value="$(eval 0.5 * arg('PI'))"/>
+    <arg name="PI_OVER_FOUR"            value="$(eval 0.25 * arg('PI'))"/>
+    <arg name="PI_THREE_QUARTERS"       value="$(eval 0.75 * arg('PI'))"/>
+    <arg name="NEG_PI_THREE_QUARTERS"   value="$(eval -0.75 * arg('PI'))"/>
+    <arg name="NEG_PI_OVER_FOUR"        value="$(eval -0.25 * arg('PI'))"/>
+    <arg name="PI_AND_A_HALF"           value="$(eval 1.5 * arg('PI'))"/>
+    <arg name="ZED2_ANGLE"              value="$(eval 0.175 * arg('PI'))"/>
+
+
+    <!-- Lidar -->
+    <node pkg="tf2_ros" type="static_transform_publisher" name="base_link_to_lidar" args="0 0 0.03565 0 0 0 base_link os_lidar" />
+
+    <!-- ZED2 Camera -->
+    <node pkg="tf2_ros" type="static_transform_publisher" name="base_link_to_zed2_camera_center" args="-0.03005 0 0.2203 0 0 0 base_link zed2i_camera_center" />
+
+
+</launch> 

logging/logging_njord/CMakeLists.txt

@@ -0,0 +1,15 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(logging_njord)
+
+find_package(catkin REQUIRED COMPONENTS
+  rospy
+  sensor_msgs
+  std_msgs
+)
+
+catkin_package(
+)
+
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+)

logging/logging_njord/package.xml

@@ -0,0 +1,31 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>logging_njord</name>
+  <version>0.0.0</version>
+  <description>The logging_njord package</description>
+
+  <maintainer email="[email protected]">aleksander</maintainer>
+
+  <license>MIT</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>rospy</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>std_msgs</build_depend>
+
+  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>sensor_msgs</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

logging/logging_njord/src/logging_ros_node.py

@@ -0,0 +1,142 @@
+#!/usr/bin/env python3
+
+import rospy
+import csv
+import numpy as np
+from pathlib import Path
+from datetime import datetime
+from threading import Lock
+from nav_msgs.msg import Odometry
+from sensor_msgs.msg import BatteryState
+
+
+class DataLoggerNode:
+
+    def __init__(self):
+        rospy.init_node('logger_node', anonymous=True)
+
+        # Create subscribers for the desired topics
+        self.odom_sub = rospy.Subscriber('/estimator/pose', Odometry,
+                                         self.odom_callback)
+        self.battery_sub = rospy.Subscriber('/internal/status/bms',
+                                            BatteryState,
+                                            self.battery_callback)
+
+        self.log_array = []  # Array to store logged data
+        self.log_lock = Lock()  # Lock for thread safety
+
+        timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
+        csv_file_name = f"data_log_{timestamp}.csv"
+        script_directory = Path(__file__).resolve().parent
+        csv_file_path = script_directory.parent / 'logs' / csv_file_name
+
+        self.csv_file = open(csv_file_name, 'w')
+        self.csv_writer = csv.writer(self.csv_file, delimiter=',')
+
+        self.csv_writer.writerow([
+            'latitude', 'longitude', 'altitude', 'heading', 'speed',
+            'power_consumption', 'battery%', 'timestep'
+        ])
+
+        self.log_rate = rospy.Rate(1)
+
+        self.current_odom = np.zeros(5)
+        self.current_battery1 = np.zeros(2)
+        self.current_battery2 = np.zeros(2)
+        self.current_battery_total = np.zeros(2)
+
+        self.prev_timestamp = rospy.get_time()
+
+        self.max_voltage = 24.6
+        self.min_voltage = 19.0
+
+        self.log_threshold = 100  # Number of entries to accumulate before writing to file
+
+    def quaternion_to_yaw(self, q):
+        x = q.x
+        y = q.y
+        z = q.z
+        w = q.w
+        siny_cosp = 2.0 * (w * z + x * y)
+        cosy_cosp = 1.0 - 2.0 * (y * y + z * z)
+        yaw = np.arctan2(siny_cosp, cosy_cosp)
+
+        return yaw
+
+    def find_battery_percentage_left(self, voltage):
+        return round((voltage - self.min_voltage) /
+                     (self.max_voltage - self.min_voltage) * 100, 2)
+
+    def odom_callback(self, data):
+        self.log_lock.acquire()
+        self.current_odom = [
+            data.pose.pose.position.x, data.pose.pose.position.y,
+            data.pose.pose.position.z,
+            self.quaternion_to_yaw(data.pose.pose.orientation),
+            np.sqrt(data.twist.twist.linear.x**2 +
+                    data.twist.twist.linear.y**2)
+        ]
+        self.log_lock.release()
+
+    def battery_callback(self, data):
+        self.log_lock.acquire()
+        if (data.location == 'ttyUSB0'):
+            self.current_battery1 = [
+                data.voltage * data.current,
+                self.find_battery_percentage_left(data.voltage)
+            ]
+        elif (data.location == 'ttyUSB1'):
+            self.current_battery2 = [
+                data.voltage * data.current,
+                self.find_battery_percentage_left(data.voltage)
+            ]
+
+        self.current_battery_total = [
+            self.current_battery1[0] + self.current_battery2[0],
+            (self.current_battery1[1] + self.current_battery2[1]) / 2
+        ]
+
+        self.log_lock.release()
+
+    def timer_callback(self):
+        self.log_data()
+
+    def log_data(self):
+        now = rospy.get_time()
+        timestep = now - self.prev_timestamp
+        self.prev_timestamp = now
+
+        self.log_lock.acquire()
+        data = [self.current_odom, self.current_battery_total, timestep]
+        self.log_lock.release()
+
+        with self.log_lock:
+            self.log_array.append(data)
+
+        rospy.loginfo("Logged data from %s")
+
+        if len(self.log_array) >= self.log_threshold:
+            self.write_to_file()
+
+    def write_to_file(self):
+        with self.log_lock:
+            for entry in self.log_array:
+                self.csv_writer.writerow(entry)
+            self.log_array = []  # Clear the log array
+
+    def run(self):
+        while not rospy.is_shutdown():
+            self.log_data()
+            self.log_rate.sleep()
+
+        self.csv_file.close()
+
+    def shutdown(self):
+        self.write_to_file()  # Write any remaining data before shutting down
+        self.csv_file.close()
+
+
+if __name__ == '__main__':
+    logger = DataLoggerNode()
+    rospy.on_shutdown(logger.shutdown)
+    logger.run()

mission/failsafe_interface/CMakeLists.txt

@@ -0,0 +1,20 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(failsafe_interface)
+
+find_package(catkin REQUIRED COMPONENTS
+  rospy
+  vortex_msgs
+  std_msgs
+  message_generation
+  roslint
+)
+
+roslint_python()
+
+catkin_package(
+  CATKIN_DEPENDS
+  rospy
+  vortex_msgs
+  std_msgs
+  message_runtime
+)