From 4ff11bc2a0811413b5ea01cbad262c9cdbb9a836 Mon Sep 17 00:00:00 2001
From: vasek <pritzvac@fel.cvut.cz>
Date: Sun, 17 Dec 2023 23:46:12 +0100
Subject: [PATCH] added config and launch file for mavros (pixhawk) imu

---
 .../vins_republisher_openvins_mavros.launch   | 60 +++++++++++++++++++
 1 file changed, 60 insertions(+)
 create mode 100644 launch/vins_republisher_openvins_mavros.launch

diff --git a/launch/vins_republisher_openvins_mavros.launch b/launch/vins_republisher_openvins_mavros.launch
new file mode 100644
index 0000000..29ddafb
--- /dev/null
+++ b/launch/vins_republisher_openvins_mavros.launch
@@ -0,0 +1,60 @@
+<launch>
+
+  <arg name="UAV_NAME" default="$(optenv UAV_NAME uav1)"/>
+
+  <arg name="camera_pitch" default="0.0"/>
+
+  <arg name="mrs_vins_world_frame" default="$(arg UAV_NAME)/mrs_vins_world"/>
+  <arg name="vins_world_frame" default="global"/>
+  <arg name="fcu_frame" default="$(arg UAV_NAME)/fcu"/>
+  <arg name="vins_fcu_frame" default="imu"/>
+  <arg name="vins_fcu_front_frame" default="$(arg UAV_NAME)/vins_body_front"/>
+
+  <!-- will it run using GNU debugger? -->
+  <arg name="DEBUG" default="false" />
+  <arg unless="$(arg DEBUG)" name="launch_prefix_debug" value=""/>
+  <arg     if="$(arg DEBUG)" name="launch_prefix_debug" value="debug_roslaunch"/>
+
+  <!-- nodelet settings-->
+  <arg name="standalone" default="true" />
+  <arg name="manager" default="$(arg UAV_NAME)_vinsrepublisher_manager" />
+  <arg name="n_threads" default="8" />
+  <arg unless="$(arg standalone)" name="nodelet" value="load"/>
+  <arg     if="$(arg standalone)" name="nodelet" value="standalone"/>
+  <arg unless="$(arg standalone)" name="nodelet_manager" value="$(arg manager)"/>
+  <arg     if="$(arg standalone)" name="nodelet_manager" value=""/>
+
+  <group ns="$(arg UAV_NAME)">
+
+      <!-- ================= Static transformations ================= -->
+    <!-- TF between the VINS world frame and MRS VINS world frame -->
+    <!-- <node pkg="tf2_ros" type="static_transform_publisher" name="tf_vins_to_vins_origin" args="0.0 0.0 0.0 1.5708 0.0 0.0 $(arg vins_world_frame) $(arg mrs_vins_world_frame)" /> -->
+    <node pkg="tf2_ros" type="static_transform_publisher" name="tf_vins_to_vins_origin" args="0.0 0.0 0.0 3.1415926535 0.0 0.0 $(arg vins_world_frame) $(arg mrs_vins_world_frame)" />
+
+    <!-- TF between the FCU of the UAV and the VINS body (IMU) frame -->
+    <node pkg="tf2_ros" type="static_transform_publisher" name="tf_vins_fcu_to_mrs_fcu" args="0.085 0.0 0.13 0 0.0 0 $(arg fcu_frame) $(arg vins_fcu_front_frame)" />
+    <!-- <node pkg="tf2_ros" type="static_transform_publisher" name="tf_vins_fcu_to_mrs_fcu" args="0.1 0.0 -0.15 -1.5708 0.0 0.0 $(arg fcu_frame) $(arg vins_fcu_front_frame)" /> -->
+
+    <!-- TF for variable pitch of the camera -->
+    <node pkg="tf2_ros" type="static_transform_publisher" name="tf_vins_fcu_to_mrs_fcu2" args="0 0 0 0 0 $(arg camera_pitch) $(arg vins_fcu_front_frame) $(arg vins_fcu_frame)" />
+
+      <!-- ================= VinsRepublisher nodelet ================ -->
+    <node pkg="nodelet" type="nodelet" name="vins_republisher" args="$(arg nodelet) vins_republisher/VinsRepublisher $(arg nodelet_manager)" launch-prefix="$(arg launch_prefix_debug)" output="screen">
+
+      <rosparam file="$(find mrs_vins_republisher)/config/default.yaml" command="load" />
+
+      <param name="uav_name" value="$(arg UAV_NAME)"/>
+
+      <!-- set to true if velocity is defined in the IMU frame -->
+      <param name="rotate_velocity" value="true"/>
+
+      <remap from="~vins_odom_in" to="ov_msckf/odomimu"/>
+      <remap from="~vins_odom_out" to="~odom"/>
+
+      <param name="fcu_frame" value="$(arg fcu_frame)"/>
+      <param name="mrs_vins_world_frame" value="$(arg mrs_vins_world_frame)"/>
+      <param name="vins_fcu_frame" value="$(arg vins_fcu_frame)"/>
+
+    </node>
+  </group>
+</launch>