From 03df381383abd37b3e92263a665efbd5bfa338e1 Mon Sep 17 00:00:00 2001
From: Moju Zhao <chou@jsk.imi.i.u-tokyo.ac.jp>
Date: Tue, 14 Mar 2023 01:02:13 +0900
Subject: [PATCH] [Mini Quadrotor] update the parameter to perform VIO based
 localization

---
 robots/mini_quadrotor/config/StateEstimation.yaml        | 7 +++++--
 robots/mini_quadrotor/launch/includes/sensors.launch.xml | 1 +
 2 files changed, 6 insertions(+), 2 deletions(-)

diff --git a/robots/mini_quadrotor/config/StateEstimation.yaml b/robots/mini_quadrotor/config/StateEstimation.yaml
index 3a1109305..6ef452637 100644
--- a/robots/mini_quadrotor/config/StateEstimation.yaml
+++ b/robots/mini_quadrotor/config/StateEstimation.yaml
@@ -46,8 +46,11 @@ sensor_plugin:
                 estimate_mode: 3 # 1<<0 + 1<<1
         mocap:
                 estimate_mode: 6 # 1<<1 + 1<<2
-        vo:
+        vo1:
                 estimate_mode: 1 # 1<<0
-                vo_sub_topic_name: realsense/odom/throttle
+                vo_sub_topic_name: /quadrotor/camera/odom/sample
                 sensor_frame: realsense_pose_frame
+                z_vel_mode: false
+                fusion_mode: 0 # ONLY POS MODE
+                throttle_rate: 120 # Hz
 
diff --git a/robots/mini_quadrotor/launch/includes/sensors.launch.xml b/robots/mini_quadrotor/launch/includes/sensors.launch.xml
index 10c1fb961..dd57c2683 100644
--- a/robots/mini_quadrotor/launch/includes/sensors.launch.xml
+++ b/robots/mini_quadrotor/launch/includes/sensors.launch.xml
@@ -31,5 +31,6 @@
     <!-- basic configuration for sensors (e.g. noise sigma) -->
     <rosparam file="$(find hydrus)/config/sensors/imu/spinal.yaml" command="load" />
     <rosparam file="$(find aerial_robot_base)/config/sensors/mocap.yaml" command="load" />
+    <rosparam file="$(find hydrus)/config/sensors/vo/realsense_t265.yaml" command="load" />
   </group>
 </launch>