update apriltag

2021-04-04 10:07:24 +08:00 · 2021-04-04 10:07:24 +08:00 · 23f966dab1
parent 7637b191e5
commit 23f966dab1
21 changed files with 497 additions and 33 deletions
--- a/communication/multi_vehicle_communication.sh
+++ b/communication/multi_vehicle_communication.sh
@ -1,6 +1,6 @@
 #!/bin/bash
 iris_num=3
-typhoon_h480_num=2
+typhoon_h480_num=0
 solo_num=0
 plane_num=0
 rover_num=0
--- a/control/multi_precision_landing.sh
+++ b/control/multi_precision_landing.sh
@ -0,0 +1,3 @@
+python precision_landing.py iris 0&
+python precision_landing.py iris 1&
+python precision_landing.py iris 2
--- a/control/precision_landing.py
+++ b/control/precision_landing.py
@ -24,13 +24,12 @@ if __name__ == '__main__':
        try:
            tfstamped = tfBuffer.lookup_transform('tag_'+vehicle_id, 'map', rospy.Time(0))
        except:
-            cmd_vel_enu.linear.z = 0.0
            continue
        # print('tf:',tfstamped.transform.translation.x)
        # print(local_pose.pose.position.x)
        cmd_vel_enu.linear.x = Kp * (tfstamped.transform.translation.x - local_pose.pose.position.x)
        cmd_vel_enu.linear.y = Kp * (tfstamped.transform.translation.y - local_pose.pose.position.y)
-        cmd_vel_enu.linear.z = - land_vel
+        cmd_vel_enu.linear.z = -land_vel
        # print(cmd_vel_enu)
        cmd_vel_pub.publish(cmd_vel_enu)
        rate.sleep()
--- a/sensing/object_detection_and_tracking/apriltag/config/rviz.rviz
+++ b/sensing/object_detection_and_tracking/apriltag/config/rviz.rviz
@ -1,13 +1,14 @@
 Panels:
  - Class: rviz/Displays
-    Help Height: 78
+    Help Height: 70
    Name: Displays
    Property Tree Widget:
      Expanded:
        - /Global Options1
        - /Status1
+        - /Image1
      Splitter Ratio: 0.5
-    Tree Height: 355
+    Tree Height: 107
  - Class: rviz/Selection
    Name: Selection
  - Class: rviz/Tool Properties
@ -61,7 +62,19 @@ Visualization Manager:
          Value: true
        base_link_frd:
          Value: true
-        camera_link:
+        iris_0/base_link:
+          Value: true
+        iris_0/camera_link:
+          Value: true
+        iris_1/base_link:
+          Value: true
+        iris_1/camera_link:
+          Value: true
+        iris_1/camera_link):
+          Value: true
+        iris_2/base_link:
+          Value: true
+        iris_2/camera_link:
          Value: true
        map:
          Value: true
@ -118,8 +131,21 @@ Visualization Manager:
      Show Names: true
      Tree:
        map:
-          base_link:
-            base_link_frd:
+          iris_0/base_link:
+            iris_0/camera_link:
+              my_bundle:
+                {}
+              tag_0:
+                {}
+              tag_1:
+                {}
+              tag_2:
+                {}
+          iris_1/base_link:
+            iris_1/camera_link:
+              {}
+          iris_2/base_link:
+            iris_2/camera_link:
              {}
          map_ned:
            {}
@ -127,7 +153,7 @@ Visualization Manager:
      Value: true
    - Class: rviz/Image
      Enabled: true
-      Image Topic: /tag_detections_image
+      Image Topic: /iris_0/tag_detections_image
      Max Value: 1
      Median window: 5
      Min Value: 0
@ -165,35 +191,35 @@ Visualization Manager:
  Views:
    Current:
      Class: rviz/Orbit
-      Distance: 13.4510498046875
+      Distance: 10.266495704650879
      Enable Stereo Rendering:
        Stereo Eye Separation: 0.05999999865889549
        Stereo Focal Distance: 1
        Swap Stereo Eyes: false
        Value: false
      Focal Point:
-        X: 0
-        Y: 0
-        Z: 0
+        X: -0.0488688126206398
+        Y: -1.6133887767791748
+        Z: 2.466456890106201
      Focal Shape Fixed Size: true
      Focal Shape Size: 0.05000000074505806
      Invert Z Axis: false
      Name: Current View
      Near Clip Distance: 0.009999999776482582
-      Pitch: 0.785398006439209
+      Pitch: 0.5503982305526733
      Target Frame: <Fixed Frame>
      Value: Orbit (rviz)
-      Yaw: 0.785398006439209
+      Yaw: 1.8953969478607178
    Saved: ~
 Window Geometry:
  Displays:
    collapsed: false
-  Height: 846
+  Height: 1025
  Hide Left Dock: false
-  Hide Right Dock: false
+  Hide Right Dock: true
  Image:
    collapsed: false
-  QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001ee000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650100000231000000bc0000001600ffffff000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000004000000000000037200000363fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000000ee000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000001310000026f0000001600ffffff000000010000010000000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000363000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000039f0000003efc0100000002fb0000000800540069006d006501000000000000039f000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000000270000036300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
  Selection:
    collapsed: false
  Time:
@ -201,7 +227,7 @@ Window Geometry:
  Tool Properties:
    collapsed: false
  Views:
-    collapsed: false
-  Width: 1200
-  X: 437
-  Y: 160
+    collapsed: true
+  Width: 927
+  X: 993
+  Y: 27
--- a/sensing/object_detection_and_tracking/apriltag/config/settings.yaml
+++ b/sensing/object_detection_and_tracking/apriltag/config/settings.yaml
@ -0,0 +1,11 @@
+# AprilTag 3 code parameters
+# Find descriptions in apriltag/include/apriltag.h:struct apriltag_detector
+#                      apriltag/include/apriltag.h:struct apriltag_family
+tag_family:        'tag36h11' # options: tagStandard52h13, tagStandard41h12, tag36h11, tag25h9, tag16h5, tagCustom48h12, tagCircle21h7, tagCircle49h12
+tag_threads:       2          # default: 2
+tag_decimate:      1.0        # default: 1.0
+tag_blur:          0.0        # default: 0.0
+tag_refine_edges:  1          # default: 1
+tag_debug:         0          # default: 0
+# Other parameters
+publish_tf:        true       # default: true
--- a/sensing/object_detection_and_tracking/apriltag/config/tags.yaml
+++ b/sensing/object_detection_and_tracking/apriltag/config/tags.yaml
@ -0,0 +1,58 @@
+# # Definitions of tags to detect
+#
+# ## General remarks
+#
+# - All length in meters
+# - Ellipsis (...) signifies that the previous element can be repeated multiple times.
+#
+# ## Standalone tag definitions
+# ### Remarks
+#
+# - name is optional
+#
+# ### Syntax
+#
+# standalone_tags:
+#   [
+#     {id: ID, size: SIZE, name: NAME},
+#     ...
+#   ]
+standalone_tags:
+  [
+    {id: 0, size: 0.5},
+    {id: 1, size: 0.5},
+    {id: 2, size: 0.5}
+  ]
+# ## Tag bundle definitions
+# ### Remarks
+#
+# - name is optional
+# - x, y, z have default values of 0 thus they are optional
+# - qw has default value of 1 and qx, qy, qz have default values of 0 thus they are optional
+#
+# ### Syntax
+#
+# tag_bundles:
+#   [
+#     {
+#       name: 'CUSTOM_BUNDLE_NAME',
+#       layout:
+#         [
+#           {id: ID, size: SIZE, x: X_POS, y: Y_POS, z: Z_POS, qw: QUAT_W_VAL, qx: QUAT_X_VAL, qy: QUAT_Y_VAL, qz: QUAT_Z_VAL},
+#           ...
+#         ]
+#     },
+#     ...
+#   ]
+tag_bundles:
+  [
+    {
+      name: 'my_bundle',
+      layout:
+        [
+          {id: 0, size: 0.5, x: -0.75, y: 0.0, z: 0.0, qw: 1.0, qx: 0.0, qy: 0.0, qz: 0.0},
+          {id: 1, size: 0.5, x: 0.0, y: 0.0, z: 0.0, qw: 1.0, qx: 0.0, qy: 0.0, qz: 0.0},
+          {id: 2, size: 0.5, x: 0.75, y: 0.0, z: 0.0, qw: 1.0, qx: 0.0, qy: 0.0, qz: 0.0}
+        ]
+    }
+  ]
--- a/sensing/object_detection_and_tracking/apriltag/image/target.pdf
+++ b/sensing/object_detection_and_tracking/apriltag/image/target.pdf
--- a/sensing/object_detection_and_tracking/apriltag/launch/xtdrone_detection.launch
+++ b/sensing/object_detection_and_tracking/apriltag/launch/xtdrone_detection.launch
@ -0,0 +1,50 @@
+<launch>
+  <arg name="launch_prefix" default="" /> <!-- set to value="gdbserver localhost:10000" for remote debugging -->
+  <!-- Set parameters -->
+  <rosparam command="load" file="$(find apriltag_ros)/config/settings.yaml" ns="iris_0/apriltag_ros_continuous_node" />
+  <rosparam command="load" file="$(find apriltag_ros)/config/tags.yaml" ns="iris_0/apriltag_ros_continuous_node" />
+  <rosparam command="load" file="$(find apriltag_ros)/config/settings.yaml" ns="iris_1/apriltag_ros_continuous_node" />
+  <rosparam command="load" file="$(find apriltag_ros)/config/tags.yaml" ns="iris_1/apriltag_ros_continuous_node" />
+  <rosparam command="load" file="$(find apriltag_ros)/config/settings.yaml" ns="iris_2/apriltag_ros_continuous_node" />
+  <rosparam command="load" file="$(find apriltag_ros)/config/tags.yaml" ns="iris_2/apriltag_ros_continuous_node" />
+
+  <group ns='iris_0'>
+  <node pkg="apriltag_ros" type="apriltag_ros_continuous_node" name="apriltag_ros_continuous_node" clear_params="true" output="screen" launch-prefix="$(arg launch_prefix)" >
+    <!-- Remap topics from those used in code to those on the ROS network -->
+    <remap from="image_rect" to="/iris_0/camera/image_raw" />
+    <remap from="camera_info" to="/iris_0/camera/camera_info" />
+
+    <param name="camera_frame" type="str" value="iris_0/camera_link" />
+    <param name="publish_tag_detections_image" type="bool" value="true" />      <!-- default: false -->
+  </node>
+  <node pkg="tf" type="static_transform_publisher" name="base_link_frd_to_camera_link"
+    args="0 0 -0.05 1.5707963 3.1415927 0 iris_0/base_link iris_0/camera_link 100" />
+  </group>
+
+  <group ns='iris_1'>
+  <node pkg="apriltag_ros" type="apriltag_ros_continuous_node" name="apriltag_ros_continuous_node" clear_params="true" output="screen" launch-prefix="$(arg launch_prefix)" >
+    <!-- Remap topics from those used in code to those on the ROS network -->
+    <remap from="image_rect" to="/iris_1/camera/image_raw" />
+    <remap from="camera_info" to="/iris_1/camera/camera_info" />
+
+    <param name="camera_frame" type="str" value="iris_1/camera_link)" />
+    <param name="publish_tag_detections_image" type="bool" value="true" />      <!-- default: false -->
+  </node>
+  <node pkg="tf" type="static_transform_publisher" name="base_link_frd_to_camera_link"
+    args="0 0 -0.05 1.5707963 3.1415927 0 iris_1/base_link iris_1/camera_link 100" />
+  </group>
+
+  <group ns='iris_2'>
+  <node pkg="apriltag_ros" type="apriltag_ros_continuous_node" name="apriltag_ros_continuous_node" clear_params="true" output="screen" launch-prefix="$(arg launch_prefix)" >
+    <!-- Remap topics from those used in code to those on the ROS network -->
+    <remap from="image_rect" to="/iris_2/camera/image_raw" />
+    <remap from="camera_info" to="/iris_2/camera/camera_info" />
+
+    <param name="camera_frame" type="str" value="iris_2/camera_link" />
+    <param name="publish_tag_detections_image" type="bool" value="true" />      <!-- default: false -->
+  </node>
+  <node pkg="tf" type="static_transform_publisher" name="base_link_frd_to_camera_link"
+    args="0 0 -0.05 1.5707963 3.1415927 0 iris_2/base_link iris_2/camera_link 100" />
+  </group>
+
+</launch>
--- a/sensing/object_detection_and_tracking/qrcode/qrcode1-3.png
+++ b/sensing/object_detection_and_tracking/qrcode/qrcode1-3.png
--- a/sensing/object_detection_and_tracking/qrcode/qrcode1-3_medium.png
+++ b/sensing/object_detection_and_tracking/qrcode/qrcode1-3_medium.png
--- a/sensing/object_detection_and_tracking/qrcode/qrcode1-3_small.png
+++ b/sensing/object_detection_and_tracking/qrcode/qrcode1-3_small.png
--- a/sensing/object_detection_and_tracking/qrcode/recognization.py
+++ b/sensing/object_detection_and_tracking/qrcode/recognization.py
@ -1,6 +0,0 @@
-import cv2 as cv
-import numpy as np
-im = cv.imread('up.jpg')
-det = cv.QRCodeDetector()
-retval, decoded_info, points, straight_qrcode = det.detectAndDecodeMulti(im)
-print(retval)
--- a/sensing/object_detection_and_tracking/qrcode/up.jpg
+++ b/sensing/object_detection_and_tracking/qrcode/up.jpg
--- a/sitl_config/launch/outdoor2_ugv.launch
+++ b/sitl_config/launch/outdoor2_ugv.launch
@ -0,0 +1,139 @@
+<launch>
+
+    <!-- MAVROS posix SITL environment launch script -->
+    <!-- launches Gazebo environment and 2x: MAVROS, PX4 SITL, and spawns vehicle -->
+    <!-- vehicle model and world -->
+    <arg name="est" default="ekf2"/>
+    <arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/outdoor2.world"/>
+    <!-- gazebo configs -->
+    <arg name="gui" default="true"/>
+    <arg name="debug" default="false"/>
+    <arg name="verbose" default="false"/>
+    <arg name="paused" default="false"/>
+    <!-- Gazebo sim -->
+    <include file="$(find gazebo_ros)/launch/empty_world.launch">
+        <arg name="gui" value="$(arg gui)"/>
+        <arg name="world_name" value="$(arg world)"/>
+        <arg name="debug" value="$(arg debug)"/>
+        <arg name="verbose" value="$(arg verbose)"/>
+        <arg name="paused" value="$(arg paused)"/>
+    </include>
+
+    <arg name="obstaclestopper" default="false"/>
+    <group ns="ugv_0">
+        <param name="robot_description"
+                command="$(find xacro)/xacro.py '$(find catvehicle)/urdf/catvehicle1-3.xacro' roboname:='ugv_0'" />
+        <include file="$(find catvehicle)/launch/catvehicle.launch">
+            <arg name="robot_name" value="ugv_0"/>
+            <arg name="init_pose" value="-x 0 -y 0 -z 0 -R 0 -P 0 -Y 0"/>
+            <arg name="config_file" value="catvehicle_control.yaml"/>
+            <arg name="obstaclestopper" value="$(arg obstaclestopper)"/>
+        </include>
+
+        <!-- Uncomment this to get immediate motion from the car
+        <node name="openLoopCircle" pkg="safeopenloopcircle" type="safeopenloopcircle_node"/>
+        -->
+    </group>
+
+    <!-- iris_0 -->
+    <group ns="iris_0">
+        <!-- MAVROS and vehicle configs -->
+            <arg name="ID" value="0"/>
+            <arg name="ID_in_group" value="0"/>
+            <arg name="fcu_url" default="udp://:24540@localhost:34580"/>
+        <!-- PX4 SITL and vehicle spawn -->
+        <include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
+            <arg name="x" value="-3"/>
+            <arg name="y" value="0"/>
+            <arg name="z" value="0"/>
+            <arg name="R" value="0"/>
+            <arg name="P" value="0"/>
+            <arg name="Y" value="0"/>
+            <arg name="vehicle" value="iris"/>
+            <arg name="sdf" value="iris_downward_camera"/>
+            <arg name="mavlink_udp_port" value="18570"/>
+            <arg name="mavlink_tcp_port" value="4560"/>
+            <arg name="ID" value="$(arg ID)"/>
+            <arg name="ID_in_group" value="$(arg ID_in_group)"/>
+        </include>
+        <!-- MAVROS -->
+        <include file="$(find mavros)/launch/px4.launch">
+            <arg name="fcu_url" value="$(arg fcu_url)"/>
+            <arg name="gcs_url" value=""/>
+            <arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
+            <arg name="tgt_component" value="1"/>
+            <arg name="config_yaml" value="$(find mavros)/launch/px4_config_0.yaml"/>
+        </include>
+    </group>
+
+    <!-- iris_1 -->
+     <group ns="iris_1">
+        <!-- MAVROS and vehicle configs -->
+            <arg name="ID" value="1"/>
+            <arg name="ID_in_group" value="1"/>
+            <arg name="fcu_url" default="udp://:24541@localhost:34581"/>
+        <!-- PX4 SITL and vehicle spawn -->
+        <include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
+            <arg name="x" value="-4.5"/>
+            <arg name="y" value="0"/>
+            <arg name="z" value="0"/>
+            <arg name="R" value="0"/>
+            <arg name="P" value="0"/>
+            <arg name="Y" value="0"/>
+            <arg name="vehicle" value="iris"/>
+            <arg name="sdf" value="iris_downward_camera"/>
+            <arg name="mavlink_udp_port" value="18571"/>
+            <arg name="mavlink_tcp_port" value="4561"/>
+            <arg name="ID" value="$(arg ID)"/>
+            <arg name="ID_in_group" value="$(arg ID_in_group)"/>
+        </include>
+        <!-- MAVROS -->
+        <include file="$(find mavros)/launch/px4.launch">
+            <arg name="fcu_url" value="$(arg fcu_url)"/>
+            <arg name="gcs_url" value=""/>
+            <arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
+            <arg name="tgt_component" value="1"/>
+            <arg name="config_yaml" value="$(find mavros)/launch/px4_config_1.yaml"/>
+        </include>
+    </group>
+
+     <!-- iris_2 -->
+     <group ns="iris_2">
+        <!-- MAVROS and vehicle configs -->
+            <arg name="ID" value="2"/>
+            <arg name="ID_in_group" value="2"/>
+            <arg name="fcu_url" default="udp://:24542@localhost:34582"/>
+        <!-- PX4 SITL and vehicle spawn -->
+        <include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
+            <arg name="x" value="-6"/>
+            <arg name="y" value="0"/>
+            <arg name="z" value="0"/>
+            <arg name="R" value="0"/>
+            <arg name="P" value="0"/>
+            <arg name="Y" value="0"/>
+            <arg name="vehicle" value="iris"/>
+            <arg name="sdf" value="iris_downward_camera"/>
+            <arg name="mavlink_udp_port" value="18572"/>
+            <arg name="mavlink_tcp_port" value="4562"/>
+            <arg name="ID" value="$(arg ID)"/>
+            <arg name="ID_in_group" value="$(arg ID_in_group)"/>
+        </include>
+        <!-- MAVROS -->
+        <include file="$(find mavros)/launch/px4.launch">
+            <arg name="fcu_url" value="$(arg fcu_url)"/>
+            <arg name="gcs_url" value=""/>
+            <arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
+            <arg name="tgt_component" value="1"/>
+            <arg name="config_yaml" value="$(find mavros)/launch/px4_config_2.yaml"/>
+        </include>
+    </group>
+
+    <!-- <node pkg="tf" type="static_transform_publisher" name="0_base_link_to_base_link_frd"
+    args="0 0 -0.05 3.1415927 0 0 /base_link_0 /base_link_frd_0 100" />
+    <node pkg="tf" type="static_transform_publisher" name="1_base_link_to_base_link_frd"
+    args="0 0 -0.05 3.1415927 0 0 /base_link_1 /base_link_frd_1 100" />
+    <node pkg="tf" type="static_transform_publisher" name="2_base_link_to_base_link_frd"
+    args="0 0 -0.05 3.1415927 0 0 /base_link_2 /base_link_frd_2 100" /> -->
+
+
+</launch>
--- a/sitl_config/models/apriltag0-2/materials/textures/apriltag0-2.png
+++ b/sitl_config/models/apriltag0-2/materials/textures/apriltag0-2.png
--- a/sitl_config/models/apriltag0-2/meshes/apriltag0-2.dae
+++ b/sitl_config/models/apriltag0-2/meshes/apriltag0-2.dae
@ -0,0 +1,125 @@
+<?xml version="1.0" ?><COLLADA version="1.4.1" xmlns="http://www.collada.org/2005/11/COLLADASchema">
+  <asset>
+    <contributor>
+      <author>Blender User</author>
+      <authoring_tool>Blender 2.74.0 commit date:2015-03-31, commit time:13:39, hash:000dfc0</authoring_tool>
+    </contributor>
+    <created>2015-04-05T02:03:25</created>
+    <modified>2015-04-05T02:03:25</modified>
+    <unit meter="1" name="meter"/>
+    <up_axis>Z_UP</up_axis>
+  </asset>
+  <library_images>
+    <image id="Marker0_png" name="Marker0_png">
+      <init_from>apriltag0-2.png</init_from>
+    </image>
+  </library_images>
+  <library_effects>
+    <effect id="Marker0Mat-effect">
+      <profile_COMMON>
+        <newparam sid="Marker0_png-surface">
+          <surface type="2D">
+            <init_from>Marker0_png</init_from>
+          </surface>
+        </newparam>
+        <newparam sid="Marker0_png-sampler">
+          <sampler2D>
+            <source>Marker0_png-surface</source>
+          </sampler2D>
+        </newparam>
+        <technique sid="common">
+          <phong>
+            <emission>
+              <color sid="emission">0 0 0 1</color>
+            </emission>
+            <ambient>
+              <color sid="ambient">0.9 0.9 0.9 1</color>
+            </ambient>
+            <diffuse>
+              <texture texcoord="UVMap" texture="Marker0_png-sampler"/>
+            </diffuse>
+            <specular>
+              <color sid="specular">0.5 0.5 0.5 1</color>
+            </specular>
+            <shininess>
+              <float sid="shininess">50</float>
+            </shininess>
+            <index_of_refraction>
+              <float sid="index_of_refraction">1</float>
+            </index_of_refraction>
+          </phong>
+        </technique>
+      </profile_COMMON>
+    </effect>
+  </library_effects>
+  <library_materials>
+    <material id="Marker0Mat-material" name="Marker0Mat">
+      <instance_effect url="#Marker0Mat-effect"/>
+    </material>
+  </library_materials>
+  <library_geometries>
+    <geometry id="Cube-mesh" name="Cube">
+      <mesh>
+        <source id="Cube-mesh-positions">
+          <float_array count="24" id="Cube-mesh-positions-array">1 0.9999999 -9.41753e-6 1 -1 -9.41753e-6 -1 -0.9999998 -9.41753e-6 -0.9999997 1 -9.41753e-6 1 0.9999994 1.999991 0.9999994 -1.000001 1.999991 -1 -0.9999997 1.999991 -0.9999999 1 1.999991</float_array>
+          <technique_common>
+            <accessor count="8" source="#Cube-mesh-positions-array" stride="3">
+              <param name="X" type="float"/>
+              <param name="Y" type="float"/>
+              <param name="Z" type="float"/>
+            </accessor>
+          </technique_common>
+        </source>
+        <source id="Cube-mesh-normals">
+          <float_array count="36" id="Cube-mesh-normals-array">0 0 -1 0 0 1 1 -5.66244e-7 3.27825e-7 -4.76837e-7 -1 0 -1 2.38419e-7 -1.19209e-7 2.38419e-7 1 1.78814e-7 0 0 -1 0 0 1 1 0 -2.38419e-7 0 -1 -2.98023e-7 -1 2.38419e-7 0 2.98023e-7 1 2.38418e-7</float_array>
+          <technique_common>
+            <accessor count="12" source="#Cube-mesh-normals-array" stride="3">
+              <param name="X" type="float"/>
+              <param name="Y" type="float"/>
+              <param name="Z" type="float"/>
+            </accessor>
+          </technique_common>
+        </source>
+        <source id="Cube-mesh-map-0">
+          <float_array count="72" id="Cube-mesh-map-0-array">0 0 0 0 0 0 0 0 0 0 0 0 0.9999 0.9940189 9.96856e-5 0.9940189 1.00079e-4 9.97642e-5 0 0 0 0 0 0 9.96856e-5 0.9940191 9.98823e-5 9.96856e-5 0.9999004 9.98429e-5 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0.9999004 9.96856e-5 0.9999 0.9940189 1.00079e-4 9.97642e-5 0 0 0 0 0 0 0.9999004 0.9940191 9.96856e-5 0.9940191 0.9999004 9.98429e-5 0 0 0 0 0 0</float_array>
+          <technique_common>
+            <accessor count="36" source="#Cube-mesh-map-0-array" stride="2">
+              <param name="S" type="float"/>
+              <param name="T" type="float"/>
+            </accessor>
+          </technique_common>
+        </source>
+        <vertices id="Cube-mesh-vertices">
+          <input semantic="POSITION" source="#Cube-mesh-positions"/>
+        </vertices>
+        <polylist count="12" material="Marker0Mat-material">
+          <input offset="0" semantic="VERTEX" source="#Cube-mesh-vertices"/>
+          <input offset="1" semantic="NORMAL" source="#Cube-mesh-normals"/>
+          <input offset="2" semantic="TEXCOORD" set="0" source="#Cube-mesh-map-0"/>
+          <vcount>3 3 3 3 3 3 3 3 3 3 3 3 </vcount>
+          <p>0 0 0 1 0 1 2 0 2 7 1 3 6 1 4 5 1 5 4 2 6 5 2 7 1 2 8 5 3 9 6 3 10 2 3 11 2 4 12 6 4 13 7 4 14 0 5 15 3 5 16 7 5 17 3 6 18 0 6 19 2 6 20 4 7 21 7 7 22 5 7 23 0 8 24 4 8 25 1 8 26 1 9 27 5 9 28 2 9 29 3 10 30 2 10 31 7 10 32 4 11 33 0 11 34 7 11 35</p>
+        </polylist>
+      </mesh>
+    </geometry>
+  </library_geometries>
+  <library_controllers/>
+  <library_visual_scenes>
+    <visual_scene id="Scene" name="Scene">
+      <node id="Marker0" name="Marker0" type="NODE">
+        <matrix sid="transform">0.004999998 0 0 0 0 0.2499999 0 0 0 0 0.25 0 0 0 0 1</matrix>
+        <instance_geometry url="#Cube-mesh">
+          <bind_material>
+            <technique_common>
+              <instance_material symbol="Marker0Mat-material" target="#Marker0Mat-material">
+                <bind_vertex_input input_semantic="TEXCOORD" input_set="0" semantic="UVMap"/>
+              </instance_material>
+            </technique_common>
+          </bind_material>
+        </instance_geometry>
+      </node>
+    </visual_scene>
+  </library_visual_scenes>
+  <scene>
+    <instance_visual_scene url="#Scene"/>
+  </scene>
+</COLLADA>
--- a/sitl_config/models/apriltag0-2/model-1_4.sdf
+++ b/sitl_config/models/apriltag0-2/model-1_4.sdf
@ -0,0 +1,14 @@
+<?xml version="1.0" ?><sdf version="1.4">
+  <model name="apriltag0-2">
+    <static>true</static>
+    <link name="link">
+      <visual name="visual">
+        <geometry>
+          <mesh>
+            <uri>model://apriltag0-2/meshes/apriltag0-2.dae</uri>
+          <scale>1.0 1.0 1.0</scale></mesh>
+        </geometry>
+      </visual>
+    </link>
+  </model>
+</sdf>
--- a/sitl_config/models/apriltag0-2/model-1_5.sdf
+++ b/sitl_config/models/apriltag0-2/model-1_5.sdf
@ -0,0 +1,14 @@
+<?xml version="1.0" ?><sdf version="1.5">
+  <model name="apriltag0-2">
+    <static>true</static>
+    <link name="link">
+      <visual name="visual">
+        <geometry>
+          <mesh>
+            <uri>model://apriltag0-2/meshes/apriltag0-2.dae</uri>
+          <scale>1.0 1.0 1.0</scale></mesh>
+        </geometry>
+      </visual>
+    </link>
+  </model>
+</sdf>
--- a/sitl_config/models/apriltag0-2/model.config
+++ b/sitl_config/models/apriltag0-2/model.config
@ -0,0 +1,17 @@
+<?xml version="1.0" ?><model>
+  <name>apriltag0-2</name>
+  <version>1.0</version>
+  <sdf version="1.6">model.sdf</sdf>
+  <sdf version="1.5">model-1_5.sdf</sdf>
+  <sdf version="1.4">model-1_4.sdf</sdf>
+
+  <author>
+    <name>Mikael Arguedas</name>
+    <email>mikael.arguedas@gmail.com</email>
+  </author>
+
+  <description>
+    A model of AR marker
+  </description>
+
+</model>
--- a/sitl_config/models/apriltag0-2/model.sdf
+++ b/sitl_config/models/apriltag0-2/model.sdf
@ -0,0 +1,14 @@
+<?xml version="1.0" ?><sdf version="1.6">
+  <model name="apriltag0-2">
+    <static>true</static>
+    <link name="link">
+      <visual name="visual">
+        <geometry>
+          <mesh>
+            <uri>model://apriltag0-2/meshes/apriltag0-2.dae</uri>
+          <scale>1.0 1.0 1.0</scale></mesh>
+        </geometry>
+      </visual>
+    </link>
+  </model>
+</sdf>
--- a/sitl_config/models/monocular_camera/monocular_camera.sdf
+++ b/sitl_config/models/monocular_camera/monocular_camera.sdf
@ -1,12 +1,12 @@
 <?xml version='1.0'?>
 <sdf version='1.0'>
    <model name='monocular_camera'>
-    <pose>0.0 0.0 -0.05 0 0 0</pose>
+    <pose>0.0 0.0 0 0 0 0</pose>
      <link name='link'>
        <inertial>
          <mass>0.001</mass>
          <inertia>
-            <ixx>4.15e-6</ixx>
+          <ixx>4.15e-6</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>2.407e-6</iyy>
@ -51,15 +51,15 @@
          </camera>
          <always_on>1</always_on>
          <update_rate>30</update_rate>
-          <visualize>1</visualize>
+          <visualize>0</visualize>
          <plugin name='camera_plugin' filename='libgazebo_ros_camera.so'>
-              <robotNamespace>/xtdrone</robotNamespace>
+              <robotNamespace></robotNamespace>
              <alwaysOn>true</alwaysOn>
              <imageTopicName>image_raw</imageTopicName>
              <cameraInfoTopicName>camera_info</cameraInfoTopicName>
              <updateRate>30.0</updateRate>
              <cameraName>camera</cameraName>
-              <frameName>/monocular_link</frameName>
+              <frameName>/camera_link</frameName>
              <CxPrime>640</CxPrime>
              <Cx>640</Cx>
              <Cy>360</Cy>