From 93aa8ca842f5a5f79d01c1b08eda90db89eec12e Mon Sep 17 00:00:00 2001
From: Junichiro Sugihara <j-sugihara@jsk.imi.i.u-tokyo.ac.jp>
Date: Tue, 14 Nov 2023 14:39:20 +0900
Subject: [PATCH] [Beetle][Vision] workaound enabling tag detection.

---
 robots/beetle/launch/bringup.launch      | 10 +++-
 robots/beetle/robots/beetle.gazebo.xacro | 69 ++++++++++++++++++++++++
 robots/beetle/urdf/beetle.urdf.xacro     | 20 +++++++
 3 files changed, 98 insertions(+), 1 deletion(-)

diff --git a/robots/beetle/launch/bringup.launch b/robots/beetle/launch/bringup.launch
index 2dc2f61b3..6e9b8a396 100644
--- a/robots/beetle/launch/bringup.launch
+++ b/robots/beetle/launch/bringup.launch
@@ -7,7 +7,8 @@
   <arg name="estimate_mode"  default= "1" />
   <arg name="sim_estimate_mode"  default= "2" />
   <arg name="headless" default="True" />
-  <arg name="worldtype" default="$(find aerial_robot_simulation)/gazebo_model/world/empty.world" />
+  <!-- <arg name="worldtype" default="$(find aerial_robot_simulation)/gazebo_model/world/empty.world" /> -->
+  <arg name="worldtype" default="$(find mini_quadrotor)/gazebo_model/world/simple_april_tags.world" />
   <arg name="launch_gazebo" default="True" />
   <arg name="direct_model" default="False" />
   <arg name="direct_model_name" />
@@ -97,4 +98,11 @@
     <arg name="spawn_yaw" value="$(arg spawn_yaw)" />
   </include>
 
+  ######## Apriltag Detection ############
+  <include file="$(find apriltag_ros)/launch/continuous_detection.launch">
+    <arg name="camera_name" value="$(arg robot_ns)/downword_camera" />
+    <arg name="image_topic" value="image_raw" />
+  </include>
+  <rosparam command="load" file="$(find beetle)/config/apriltag/tags_bundle.yaml" ns="/apriltag_ros_continuous_node"/>
+  
 </launch>
diff --git a/robots/beetle/robots/beetle.gazebo.xacro b/robots/beetle/robots/beetle.gazebo.xacro
index 92673a5a6..da7d6d9ea 100644
--- a/robots/beetle/robots/beetle.gazebo.xacro
+++ b/robots/beetle/robots/beetle.gazebo.xacro
@@ -26,6 +26,75 @@
       <contact_topic>__default_topic__</contact_topic>
     </plugin>
   </gazebo>
+
+  <gazebo reference="base_link">
+    <mu1>0.1</mu1>
+    <mu2>0.1</mu2>
+  </gazebo>
+
+  <!-- sensors -->
+  <!-- 1. stereo camera: depth, color based on left camera -->
+  <xacro:extra_module name = "downward_camera_frame" parent = "base_link">
+    <origin xyz="0.0 0.00 0.0" rpy="0 ${pi/2} 0"/>
+    <inertial>
+      <mass value = "0.00001" />
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <inertia
+          ixx="0.000001" ixy="0.0" ixz="0.0"
+          iyy="0.000001" iyz="0.0"
+          izz="0.000002"/>
+    </inertial>
+  </xacro:extra_module>
+  <xacro:extra_module name = "downward_camera_optical_frame" parent = "downward_camera_frame">
+    <origin xyz="0 0 0" rpy="${-pi / 2} 0 ${-pi / 2}"/>
+    <inertial>
+      <mass value = "0.00001" />
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <inertia
+          ixx="0.000001" ixy="0.0" ixz="0.0"
+          iyy="0.000001" iyz="0.0"
+          izz="0.000002"/>
+    </inertial>
+  </xacro:extra_module>
+
+  <!-- gazebo plugin -->
+  <gazebo reference="downward_camera_frame">
+    <sensor type="camera" name="downward_camera">
+      <update_rate>30.0</update_rate>
+      <camera name="head">
+        <horizontal_fov>1.57</horizontal_fov>
+        <image>
+          <width>640</width>
+          <height>480</height>
+          <format>R8G8B8</format>
+        </image>
+        <clip>
+          <near>0.02</near>
+          <far>300</far>
+        </clip>
+        <noise>
+          <type>gaussian</type>
+          <mean>0.0</mean>
+          <stddev>0.007</stddev>
+        </noise>
+      </camera>
+      <plugin name="camera_controller" filename="libgazebo_ros_camera.so">
+        <robotNamespace>$(arg robot_name)</robotNamespace>
+        <alwaysOn>true</alwaysOn>
+        <updateRate>0.0</updateRate>
+        <cameraName>downword_camera</cameraName>
+        <imageTopicName>image_raw</imageTopicName>
+        <cameraInfoTopicName>camera_info</cameraInfoTopicName>
+        <frameName>downward_camera_optical_frame</frameName>
+        <hackBaseline>0.07</hackBaseline>
+        <distortionK1>0.0</distortionK1>
+        <distortionK2>0.0</distortionK2>
+        <distortionK3>0.0</distortionK3>
+        <distortionT1>0.0</distortionT1>
+        <distortionT2>0.0</distortionT2>
+      </plugin>
+    </sensor>
+  </gazebo>
   
 
 </robot>
diff --git a/robots/beetle/urdf/beetle.urdf.xacro b/robots/beetle/urdf/beetle.urdf.xacro
index 3f72eec2b..64260ff3f 100644
--- a/robots/beetle/urdf/beetle.urdf.xacro
+++ b/robots/beetle/urdf/beetle.urdf.xacro
@@ -212,6 +212,26 @@
         <mechanicalReduction>1</mechanicalReduction>
       </actuator>
     </transmission>
+  </xacro:macro>
 
+  <!--extra module-->
+  <xacro:macro name="extra_module" params="name parent *origin *inertial visible:=0 model_url:=1 scale:=1">
+    <joint name="${parent}2${name}" type="fixed">
+      <parent link="${parent}"/>
+      <child link="${name}"/>
+      <xacro:insert_block name="origin" />
+    </joint>
+    <link name="${name}">
+      <xacro:insert_block name="inertial" />
+      <xacro:if value="${visible == 1}">
+        <visual>
+          <origin rpy="0 0 0" xyz="0 0 0"/>
+          <geometry>
+            <mesh filename="${model_url}" scale= "${scale} ${scale} ${scale}"/>
+          </geometry>
+        </visual>
+      </xacro:if>
+    </link>
   </xacro:macro>
+
 </robot>