From 41af6eb5e1b6da942269783d74d2bb4617ea38c5 Mon Sep 17 00:00:00 2001
From: Talha <talha@karasu.me>
Date: Fri, 3 Jan 2025 22:44:17 +0200
Subject: [PATCH] create a seperate control config for simulation (#110)

* create a seperate control config for simulation

* remove remapping from sim bridge

* use config_file param to load rosparams

* add pid params for sim
---
 auv_bringup/launch/start.launch               |  1 +
 auv_bringup/launch/start_navigation.launch    |  2 +
 auv_control/auv_control/launch/start.launch   |  7 ++--
 .../auv_sim_bridge/config/control_config.yaml | 40 +++++++++++++++++++
 .../auv_sim_bridge/simulation_mock_ros.h      | 12 +-----
 .../launch/inc/start_bridge.launch            |  1 +
 6 files changed, 49 insertions(+), 14 deletions(-)
 create mode 100644 auv_sim/auv_sim_bridge/config/control_config.yaml

diff --git a/auv_bringup/launch/start.launch b/auv_bringup/launch/start.launch
index dfa212b..d03e0c8 100644
--- a/auv_bringup/launch/start.launch
+++ b/auv_bringup/launch/start.launch
@@ -52,6 +52,7 @@
     <arg name="control_rate" value="$(arg control_rate)" />
     <arg name="use_gui" value="$(arg use_gui)" />
     <arg name="start_state_publisher" value="false" />
+    <arg name="config_file" value="$(find auv_control)/config/default.yaml" />
   </include>
 
   <include file="$(find auv_bringup)/launch/inc/logging.launch.xml">
diff --git a/auv_bringup/launch/start_navigation.launch b/auv_bringup/launch/start_navigation.launch
index 6f928ec..5d8e019 100644
--- a/auv_bringup/launch/start_navigation.launch
+++ b/auv_bringup/launch/start_navigation.launch
@@ -4,6 +4,7 @@
   <arg name="control_rate" default="20.0" />
   <arg name="use_gui" default="false" />
   <arg name="start_state_publisher" default="false" />
+  <arg name="config_file" default="$(find auv_control)/config/default.yaml" />
 
   <include file="$(find auv_localization)/launch/start.launch">
     <arg name="namespace" value="$(arg namespace)" />
@@ -19,6 +20,7 @@
     <arg name="rate" value="$(arg control_rate)" />
     <arg name="use_gui" value="$(arg use_gui)" />
     <arg name="start_state_publisher" value="$(arg start_state_publisher)" />
+    <arg name="config_file" value="$(arg config_file)"/>
   </include>
 
   <include file="$(find auv_mapping)/launch/start.launch">
diff --git a/auv_control/auv_control/launch/start.launch b/auv_control/auv_control/launch/start.launch
index fe58909..de6e698 100644
--- a/auv_control/auv_control/launch/start.launch
+++ b/auv_control/auv_control/launch/start.launch
@@ -4,6 +4,7 @@
   <arg name="rate" default="20" />
   <arg name="use_gui" default="false" />
   <arg name="start_state_publisher" default="false" />
+  <arg name="config_file" default="$(find auv_control)/config/default.yaml" />
 
   <include file="$(find auv_description)/launch/start_state_publisher.launch"
     if="$(arg start_state_publisher)">
@@ -14,7 +15,7 @@
   <group ns="$(arg namespace)">
     <node pkg="auv_control" type="auv_control_node" name="auv_control_node" output="screen">
       <param name="rate" value="$(arg rate)" />
-      <param name="config_file" value="$(find auv_control)/config/default.yaml" />
+      <param name="config_file" value="$(arg config_file)" />
 
       <remap from="odometry" to="odometry" />
       <remap from="cmd_vel" to="cmd_vel" />
@@ -23,14 +24,14 @@
       <remap from="enable" to="enable" />
 
       <remap from="wrench" to="wrench" />
-      <rosparam file="$(find auv_control)/config/default.yaml" command="load" ns="" />
+      <rosparam file="$(arg config_file)" command="load" ns="" />
     </node>
 
     <node pkg="auv_control" type="thruster_manager_node" name="thruster_manager_node"
       output="screen">
       <param name="rate" value="$(arg rate)" />
       <remap from="power" to="mainboard/power_sensor/power" />
-      <rosparam file="$(find auv_control)/config/default.yaml" command="load" ns="" />
+      <rosparam file="$(arg config_file)" command="load" ns="" />
     </node>
 
     <node pkg="auv_control" type="depth_controller_node.py" name="depth_controller_node"
diff --git a/auv_sim/auv_sim_bridge/config/control_config.yaml b/auv_sim/auv_sim_bridge/config/control_config.yaml
new file mode 100644
index 0000000..6d3b47e
--- /dev/null
+++ b/auv_sim/auv_sim_bridge/config/control_config.yaml
@@ -0,0 +1,40 @@
+---
+model:
+  mass_inertia_matrix:
+    - [27.0, 0, 0, 0, 0, 0]
+    - [0, 27.0, 0, 0, 0, 0]
+    - [0, 0, 27.0, 0, 0, 0]
+    - [0, 0, 0, 0.75, 0, 0]
+    - [0, 0, 0, 0, 2.245, 0]
+    - [0, 0, 0, 0, 0, 2.116]
+
+  linear_damping_matrix:
+    - [30.1481, 0, 0, 0, 0, 0]
+    - [0, 31.4748, 0, 0, 0, 0]
+    - [0, 0, 89.2, 0, 0, 0]
+    - [0, 0, 0, 0.0, 0, 0]
+    - [0, 0, 0, 0, 0.0, 0]
+    - [0, 0, 0, 0, 0, 1.87816]
+
+  quadratic_damping_matrix:
+    - [77.0139, 0, 0, 0, 0, 0]
+    - [0, 155.237, 0, 0, 0, 0]
+    - [0, 0, 136.16, 0, 0, 0]
+    - [0, 0, 0, 0.0, 0, 0]
+    - [0, 0, 0, 0, 0.0, 0]
+    - [0, 0, 0, 0, 0, 7.98053]
+
+#kp: [0.0, 0.0, 0.5, 0.0, 0.0, 2.0, 1.0, 1.0, 1.0, 0.1, 0.0, 1.0]
+#ki: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+#kd: [0.0, 0.0, 0.1, 0.0, 0.0, 0.3, 1.0, 1.0, 1.0, 0.0, 0.0, 1.0]
+
+kp: [0.0, 0.0, 0.5, 0.0, 0.0, 2.0, 1.0, 1.0, 1.0, 0.1, 0.0, 1.0]
+ki: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+kd: [0.0, 0.0, 0.1, 0.0, 0.0, 0.3, 1.0, 1.0, 1.0, 0.0, 0.0, 1.0]
+
+
+max_thrust: 60.0 # N
+min_thrust: 0.5 # N
+coeffs_ccw: [12.292935764557399, -3.0745135927968605, -1.0664649834600253, 181.68212348405177, 36.69456713973092, 1151.1307183400536]
+coeffs_cw: [-7.65890867108129, -2.272619267631361, 1.5070809906898863, 140.79738360070297, -50.325593642656244, 1954.5975713257544]
+mapping: [0, 1, 2, 3, 4, 5, 6, 7]
diff --git a/auv_sim/auv_sim_bridge/include/auv_sim_bridge/simulation_mock_ros.h b/auv_sim/auv_sim_bridge/include/auv_sim_bridge/simulation_mock_ros.h
index 3d0ebcc..41464ac 100644
--- a/auv_sim/auv_sim_bridge/include/auv_sim_bridge/simulation_mock_ros.h
+++ b/auv_sim/auv_sim_bridge/include/auv_sim_bridge/simulation_mock_ros.h
@@ -87,23 +87,13 @@ class SimulationMockROS {
   void drivePulseCallback(const auv_msgs::MotorCommand &msg) {
     const auto stamp = ros::Time::now();
 
-    // Remapping array: maps input channels to specific thruster indices
-    std::vector<int> channelRemap = {1, 7, 2, 5, 0, 6, 3, 4};
-
     if (msg.channels.size() < kThrusterSize) {
       ROS_WARN("Received MotorCommand with insufficient channels.");
       return;
     }
 
     for (int i = 0; i < kThrusterSize; i++) {
-      if (i < channelRemap.size() && channelRemap[i] < msg.channels.size()) {
-        int remappedChannel = channelRemap[i];
-        thrusters_[i].publish(msg.channels.at(remappedChannel), stamp);
-      } else {
-        ROS_WARN(
-            "Invalid channel remapping or insufficient channels for "
-            "remapping.");
-      }
+      thrusters_[i].publish(msg.channels.at(i), stamp);
     }
   }
 
diff --git a/auv_sim/auv_sim_bringup/launch/inc/start_bridge.launch b/auv_sim/auv_sim_bringup/launch/inc/start_bridge.launch
index c2c485b..f28cf85 100644
--- a/auv_sim/auv_sim_bringup/launch/inc/start_bridge.launch
+++ b/auv_sim/auv_sim_bringup/launch/inc/start_bridge.launch
@@ -35,6 +35,7 @@
     <arg name="control_rate" value="$(arg control_rate)" />
     <arg name="use_gui" value="$(arg use_gui)" />
     <arg name="start_state_publisher" value="false" />
+    <arg name="config_file" value="$(find auv_sim_bridge)/config/control_config.yaml"/>
   </include>
 
 </launch>