Merge pull request #589 from tongtybj/develop/mini_quadrotor/livox_mi…

…d360 [Mini Quadrotor] implement model with Livox MID360
jsk-ros-pkg · Apr 13, 2024 · e11261e · e11261e
2 parents 9836f15 + 4961c1b
commit e11261e
Show file tree

Hide file tree

Showing 9 changed files with 373 additions and 16 deletions.
diff --git a/robots/mini_quadrotor/CMakeLists.txt b/robots/mini_quadrotor/CMakeLists.txt
@@ -14,6 +14,11 @@ install(DIRECTORY config launch
   USE_SOURCE_PERMISSIONS
 )
 
+install(DIRECTORY bin
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+  USE_SOURCE_PERMISSIONS
+)
+
 mujoco_model_convert(
   ${PROJECT_SOURCE_DIR}
   ${PROJECT_SOURCE_DIR}/config/mujoco_model.yaml)

diff --git a/robots/mini_quadrotor/bin/rosbag_record.sh b/robots/mini_quadrotor/bin/rosbag_record.sh
@@ -0,0 +1,3 @@
+#!/bin/bash
+
+rosrun aerial_robot_base rosbag_control_data.sh ${1:-quadrotor} quadrotor/livox/lidar quadrotor/livox/imu quadrotor/Odometry quadrotor/Odometry_precede ${@:2}
diff --git a/robots/mini_quadrotor/config/Battery.yaml b/robots/mini_quadrotor/config/Battery.yaml
@@ -1,5 +1,5 @@
 bat_info:
         bat_cell: 3
-        bat_resistance: 0.04 # [m Ohm]
-        hovering_current: 15 #[A]
-        low_voltage_thre: 10 #[100%]
+        bat_resistance: 0.05 # [m Ohm]
+        hovering_current: 22 #[A]
+        low_voltage_thre: 6 #[100%]
diff --git a/robots/mini_quadrotor/config/FlightControl.yaml b/robots/mini_quadrotor/config/FlightControl.yaml
@@ -4,7 +4,7 @@ controller:
   xy:
     p_gain: 5.0
     i_gain: 0.05
-    d_gain: 3.0
+    d_gain: 5.0
     limit_sum: 3
     limit_p: 3
     limit_i: 1.5
@@ -36,9 +36,9 @@ controller:
     gyro_moment_compensation: true
     clamp_gain: true
 
-    roll_pitch_p: 20
+    roll_pitch_p: 15 # 20
     roll_pitch_i: 0.5
-    roll_pitch_d: 0.15
+    roll_pitch_d: 0.5 # 1.0 #0.15
 
     yaw_p: 60
     yaw_i: 0.5

diff --git a/robots/mini_quadrotor/config/NavigationConfig.yaml b/robots/mini_quadrotor/config/NavigationConfig.yaml
@@ -8,7 +8,7 @@ navigation:
   # Attitude Control Mode: 4
 
 
-  takeoff_height: 1.0 #1.0
+  takeoff_height: 0.6 #1.0
   outdoor_takeoff_height: 1.5
 
   # teleop operation

diff --git a/robots/mini_quadrotor/config/StateEstimation.yaml b/robots/mini_quadrotor/config/StateEstimation.yaml
@@ -4,6 +4,7 @@ estimation:
         sensor_list:
         - sensor_plugin/imu
         - sensor_plugin/mocap
+        - sensor_plugin/vo # Fast LIO
 
         # egomotion estimation
         egomotion_list:
@@ -45,3 +46,8 @@ sensor_plugin:
                 estimate_mode: 3 # 1<<0 + 1<<1
         mocap:
                 estimate_mode: 6 # 1<<1 + 1<<2
+        vo1:
+                estimate_mode: 1 # 1<<0
+                # fusion_mode: 2 # ONLY_POS_MODE = 0, ONLY_VEL_MODE = 1, POS_VEL_MODE = 2 (default)
+                # param_verbose: true
+                throttle_rate: 120 # Hz
diff --git a/robots/mini_quadrotor/launch/includes/sensors.launch.xml b/robots/mini_quadrotor/launch/includes/sensors.launch.xml
@@ -16,11 +16,20 @@
 
         <!-- mocap -->
         <include file="$(find aerial_robot_base)/launch/external_module/mocap.launch" />
+
+        <!-- livox mid360 & fast lio -->
+        <include file="$(find livox_ros_driver2)/launch_ROS1/msg_MID360.launch" />
+        <include file="$(find fast_lio)/launch/mapping_mid360.launch" >
+          <arg name="rviz" value="false" />
+        </include>
+
       </group>
     </group>
 
     <!-- basic configuration for sensors (e.g. noise sigma) -->
     <rosparam file="$(find aerial_robot_base)/config/sensors/imu/spinal.yaml" command="load" />
     <rosparam file="$(find aerial_robot_base)/config/sensors/mocap.yaml" command="load" />
+    <rosparam file="$(find aerial_robot_base)/config/sensors/lio/livox_mid360.yaml" command="load" />
   </group>
+
 </launch>
diff --git a/robots/mini_quadrotor/urdf/mesh/main_body_livox.dae b/robots/mini_quadrotor/urdf/mesh/main_body_livox.dae
diff --git a/robots/mini_quadrotor/urdf/robot.urdf.xacro b/robots/mini_quadrotor/urdf/robot.urdf.xacro
@@ -7,8 +7,8 @@
   <thrust_link name="thrust" />
 
   <!-- kinematics [m] -->
-  <xacro:property name="rotor_x" value="0.070" />
-  <xacro:property name="rotor_y" value="0.086" />
+  <xacro:property name="rotor_x" value="0.085" />
+  <xacro:property name="rotor_y" value="0.085" />
   <xacro:property name="rotor_z" value="0.026" />
 
   <!-- dynamics -->
@@ -106,17 +106,17 @@
   <!-- main_body -->
   <link name="main_body">
     <inertial>
-      <mass value = "0.432" />
-      <origin xyz="0 0 0.017" rpy="0 0 0"/>
+      <mass value = "0.832" /> 
+      <origin xyz="0.002 0 ${0.028 - 0.002}" rpy="0 0 0"/> <!-- TODO: true cog pos -->
       <inertia
-          ixx="0.0021" ixy="0" ixz="0"
-          iyy="0.0015" iyz="0"
-          izz="0.0032"/>
+          ixx="0.0030" ixy="0" ixz="0"
+          iyy="0.0030" iyz="0"
+          izz="0.0045"/>
     </inertial>
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://mini_quadrotor/urdf/mesh/main_body.dae" />
+        <mesh filename="package://mini_quadrotor/urdf/mesh/main_body_livox.dae" />
       </geometry>
     </visual>
     <collision>
@@ -139,9 +139,21 @@
     </inertial>
   </xacro:extra_module>
 
+  <!-- livox mid360 -->
+  <xacro:extra_module name = "lidar_imu" parent = "main_body" visible = "0" > <!-- should be IMU frame -->
+    <origin xyz="${0.011} ${0.02329} ${0.0425 + 0.047 - 0.04412}" rpy="0 0 0"/> <!-- TODO: the true pos -->
+    <inertial>
+      <mass value = "00" />
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <inertia
+          ixx="0.0" ixy="0.0" ixz="0.0"
+          iyy="0.0" iyz="0.0" izz="0.0"/>
+    </inertial>
+  </xacro:extra_module>
+
   <!-- battery -->
   <xacro:extra_module name = "battery" parent = "main_body" visible = "1" model_url = "package://mini_quadrotor/urdf/mesh/turnigy_3S_2200mAh.dae" >
-    <origin xyz="0.0105 0 -0.012" rpy="0 0 0"/>
+    <origin xyz="0.0 0 -0.035" rpy="${pi/2} 0 0"/>
     <inertial>
       <mass value = "0.188" />
       <origin xyz="0 0 0" rpy="0 0 0"/>