add precision_landing

2021-04-03 21:34:19 +08:00 · 2021-04-03 21:34:19 +08:00 · 7637b191e5
parent 6b3359a70d
commit 7637b191e5
14 changed files with 851 additions and 4 deletions
--- a/control/precision_landing.py
+++ b/control/precision_landing.py
@ -0,0 +1,37 @@
+import rospy
+from geometry_msgs.msg import Twist, PoseStamped, TransformStamped
+from tf2_ros import TransformListener, Buffer
+import sys
+
+def local_pose_callback(data):
+    global local_pose
+    local_pose = data
+
+if __name__ == '__main__':
+    vehicle_type = sys.argv[1]
+    vehicle_id = sys.argv[2]
+    rospy.init_node(vehicle_type+'_'+vehicle_id+'_precision_landing')
+    tfBuffer = Buffer()
+    tflistener = TransformListener(tfBuffer)
+    cmd_vel_enu = Twist()   
+    local_pose = PoseStamped()
+    Kp = 1.0
+    land_vel = 0.5
+    rospy.Subscriber(vehicle_type+'_'+vehicle_id+"/mavros/local_position/pose", PoseStamped, local_pose_callback)
+    cmd_vel_pub = rospy.Publisher('/xtdrone/'+vehicle_type+'_'+vehicle_id+'/cmd_vel_enu', Twist, queue_size=2)
+    rate = rospy.Rate(50)
+    while not rospy.is_shutdown():
+        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
+        # print(cmd_vel_enu)
+        cmd_vel_pub.publish(cmd_vel_enu)
+        rate.sleep()
+
--- a/control/yolo_human_tracking.py
+++ b/control/yolo_human_tracking.py
@ -65,12 +65,12 @@ if __name__ == "__main__":
    rospy.Subscriber("/uav_"+vehicle_id+"/darknet_ros/bounding_boxes", BoundingBoxes, darknet_callback)
    rospy.Subscriber(vehicle_type+'_'+vehicle_id+"/mavros/local_position/pose", PoseStamped, local_pose_callback)
    rospy.Subscriber('/xtdrone/'+vehicle_type+'_'+vehicle_id+'/cam_pose', PoseStamped, cam_pose_callback)
-    vel_pub = rospy.Publisher('/xtdrone/'+vehicle_type+'_'+vehicle_id+'/cmd_vel_flu', Twist, queue_size=2)
+    cmd_vel_pub = rospy.Publisher('/xtdrone/'+vehicle_type+'_'+vehicle_id+'/cmd_vel_flu', Twist, queue_size=2)
    cmd_pub = rospy.Publisher('/xtdrone/'+vehicle_type+'_'+vehicle_id+'/cmd', String, queue_size=2)
    rate = rospy.Rate(60) 
-    while(True):
+    while not rospy.is_shutdown():
        rate.sleep()
-        vel_pub.publish(twist)
+        cmd_vel_pub.publish(twist)
        cmd_pub.publish(cmd)
        if find_cnt - find_cnt_last == 0:
            if not get_time:
--- a/sensing/object_detection_and_tracking/apriltag/config/rviz.rviz
+++ b/sensing/object_detection_and_tracking/apriltag/config/rviz.rviz
@ -0,0 +1,207 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+      Splitter Ratio: 0.5
+    Tree Height: 355
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: Image
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Class: rviz/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+        base_link:
+          Value: true
+        base_link_frd:
+          Value: true
+        camera_link:
+          Value: true
+        map:
+          Value: true
+        map_ned:
+          Value: true
+        my_bundle:
+          Value: true
+        odom:
+          Value: true
+        odom_ned:
+          Value: true
+        tag_0:
+          Value: true
+        tag_1:
+          Value: true
+        tag_2:
+          Value: true
+        ugv_0/back_left_wheel_link:
+          Value: true
+        ugv_0/back_right_wheel_link:
+          Value: true
+        ugv_0/base_link:
+          Value: true
+        ugv_0/camera_left_link:
+          Value: true
+        ugv_0/camera_right_link:
+          Value: true
+        ugv_0/front_laser_link:
+          Value: true
+        ugv_0/front_left_steering_link:
+          Value: true
+        ugv_0/front_left_wheel_link:
+          Value: true
+        ugv_0/front_right_steering_link:
+          Value: true
+        ugv_0/front_right_wheel_link:
+          Value: true
+        ugv_0/main_mass:
+          Value: true
+        ugv_0/odom:
+          Value: true
+        ugv_0/slamodom:
+          Value: true
+        ugv_0/triclops_link:
+          Value: true
+        ugv_0/velodyne_link:
+          Value: true
+        world:
+          Value: true
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        map:
+          base_link:
+            base_link_frd:
+              {}
+          map_ned:
+            {}
+      Update Interval: 0
+      Value: true
+    - Class: rviz/Image
+      Enabled: true
+      Image Topic: /tag_detections_image
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Image
+      Normalize Range: true
+      Queue Size: 2
+      Transport Hint: raw
+      Unreliable: false
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: map
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
+      Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 13.4510498046875
+      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
+      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
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 0.785398006439209
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 846
+  Hide Left Dock: false
+  Hide Right Dock: false
+  Image:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001ee000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650100000231000000bc0000001600ffffff000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1200
+  X: 437
+  Y: 160
--- a/sensing/object_detection_and_tracking/apriltag/image/apriltag0-2.png
+++ b/sensing/object_detection_and_tracking/apriltag/image/apriltag0-2.png
--- 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/model/marker0/materials/textures/Marker0.png
+++ b/sensing/object_detection_and_tracking/apriltag/model/marker0/materials/textures/Marker0.png
--- a/sensing/object_detection_and_tracking/apriltag/model/marker0/meshes/Marker0.dae
+++ b/sensing/object_detection_and_tracking/apriltag/model/marker0/meshes/Marker0.dae
@ -0,0 +1,126 @@
+<?xml version="1.0" encoding="utf-8"?>
+<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1">
+  <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 name="meter" meter="1"/>
+    <up_axis>Z_UP</up_axis>
+  </asset>
+  <library_images>
+    <image id="Marker0_png" name="Marker0_png">
+      <init_from>Marker0.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 texture="Marker0_png-sampler" texcoord="UVMap"/>
+            </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 id="Cube-mesh-positions-array" count="24">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 source="#Cube-mesh-positions-array" count="8" 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 id="Cube-mesh-normals-array" count="36">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 source="#Cube-mesh-normals-array" count="12" 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 id="Cube-mesh-map-0-array" count="72">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 source="#Cube-mesh-map-0-array" count="36" 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 material="Marker0Mat-material" count="12">
+          <input semantic="VERTEX" source="#Cube-mesh-vertices" offset="0"/>
+          <input semantic="NORMAL" source="#Cube-mesh-normals" offset="1"/>
+          <input semantic="TEXCOORD" source="#Cube-mesh-map-0" offset="2" set="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 semantic="UVMap" input_semantic="TEXCOORD" input_set="0"/>
+              </instance_material>
+            </technique_common>
+          </bind_material>
+        </instance_geometry>
+      </node>
+    </visual_scene>
+  </library_visual_scenes>
+  <scene>
+    <instance_visual_scene url="#Scene"/>
+  </scene>
+</COLLADA>
--- a/sensing/object_detection_and_tracking/apriltag/model/marker0/model-1_4.sdf
+++ b/sensing/object_detection_and_tracking/apriltag/model/marker0/model-1_4.sdf
@ -0,0 +1,15 @@
+<?xml version="1.0" ?>
+<sdf version="1.4">
+  <model name="Marker0">
+    <static>true</static>
+    <link name="link">
+      <visual name="visual">
+        <geometry>
+          <mesh>
+            <uri>model://marker0/meshes/Marker0.dae</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+  </model>
+</sdf>
--- a/sensing/object_detection_and_tracking/apriltag/model/marker0/model-1_5.sdf
+++ b/sensing/object_detection_and_tracking/apriltag/model/marker0/model-1_5.sdf
@ -0,0 +1,15 @@
+<?xml version="1.0" ?>
+<sdf version="1.4">
+  <model name="Marker0">
+    <static>true</static>
+    <link name="link">
+      <visual name="visual">
+        <geometry>
+          <mesh>
+            <uri>model://marker0/meshes/Marker0.dae</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+  </model>
+</sdf>
--- a/sensing/object_detection_and_tracking/apriltag/model/marker0/model.config
+++ b/sensing/object_detection_and_tracking/apriltag/model/marker0/model.config
@ -0,0 +1,19 @@
+<?xml version="1.0"?>
+
+<model>
+  <name>Marker0</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/sensing/object_detection_and_tracking/apriltag/model/marker0/model.sdf
+++ b/sensing/object_detection_and_tracking/apriltag/model/marker0/model.sdf
@ -0,0 +1,15 @@
+<?xml version="1.0" ?>
+<sdf version="1.6">
+  <model name="Marker0">
+    <static>true</static>
+    <link name="link">
+      <visual name="visual">
+        <geometry>
+          <mesh>
+            <uri>model://marker0/meshes/Marker0.dae</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+  </model>
+</sdf>
--- a/sensing/object_detection_and_tracking/apriltag/script/make_gazebo_model.py
+++ b/sensing/object_detection_and_tracking/apriltag/script/make_gazebo_model.py
@ -0,0 +1,227 @@
+#! /usr/bin/env python
+# Thanks for https://github.com/mikaelarguedas/gazebo_models
+from __future__ import print_function
+
+import argparse
+from xml.dom.minidom import parse
+import os
+import sys
+
+parser = argparse.ArgumentParser(
+    usage='generate gazebo models for AR tags',
+    formatter_class=argparse.ArgumentDefaultsHelpFormatter
+)
+parser.add_argument(
+    '-i', '--images-dir',
+    default="$HOME/gazebo_models/ar_tags/images",
+    help='directory where the marker images are located')
+parser.add_argument(
+    '-g', '--gazebodir',
+    default="$HOME/.gazebo/models",
+    help='Gazebo models directory')
+parser.add_argument(
+    '-s', '--size',
+    default=500, type=int,
+    help='marker size in mm')
+parser.add_argument(
+    '-v', '--verbose',
+    dest='verbose', action='store_true',
+    help='verbose mode')
+
+parser.add_argument(
+    '-w', '--white-contour-size-mm',
+    default=0, type=int,
+    help='Add white contour around images, default to no contour')
+parser.set_defaults(verbose=False)
+
+args = parser.parse_args()
+
+args.gazebodir = os.path.expandvars(args.gazebodir)
+args.images_dir = os.path.expandvars(args.images_dir)
+script_path = os.path.dirname(os.path.realpath(__file__))
+model_dir = os.path.join(os.path.dirname(script_path), 'model')
+ORIGINAL_MARKER_SIZE_MM = 500
+ORIGINAL_IMAGE_SIZE_PX = 170
+white_contour_px = \
+    args.white_contour_size_mm * ORIGINAL_IMAGE_SIZE_PX / args.size
+
+if not os.path.isdir(args.images_dir):
+    print("provided image directory '%s' is not a directory" % args.images_dir)
+    sys.exit()
+
+# Open every collada file
+if args.verbose:
+    print(args.images_dir)
+
+# Copy marker0 directory into gazebo model directory
+cp_marker0_cmd = "cp -r " + os.path.join(model_dir, 'marker0') + \
+             " " + os.path.join(args.gazebodir, "marker0")
+if args.verbose:
+    print(cp_marker0_cmd)
+os.system(cp_marker0_cmd)
+
+file_list = sorted(os.listdir(args.images_dir))
+for image_file in file_list:
+    if not image_file.endswith('.png'):
+        continue
+    image_file_path = os.path.join(args.images_dir, image_file)
+    filename_without_ext = image_file[0:image_file.rfind('.')]
+    # ignore marker0 as it has already been copied above
+    if not filename_without_ext.lower() == 'marker0':
+        cmd = "cp -r " + os.path.join(args.gazebodir, "marker0") + \
+              " " + os.path.join(args.gazebodir, filename_without_ext.lower())
+        if args.verbose:
+            print(cmd)
+        os.system(cmd)
+
+    cmd = "rm " + os.path.join(
+        args.gazebodir, filename_without_ext.lower(),
+        "materials", "textures", "Marker0.png")
+    if args.verbose:
+        print(cmd)
+    os.system(cmd)
+    image_dest_path = os.path.join(
+        args.gazebodir,
+        filename_without_ext.lower(),
+        "materials", "textures", image_file)
+    cmd = "cp " + image_file_path + " " + \
+          image_dest_path
+    if args.verbose:
+        print(cmd)
+    os.system(cmd)
+
+    # add white contour if applicable:
+    if white_contour_px > 0:
+        convert_cmd = "convert %s -bordercolor white -border %dx%d %s" % (
+            image_dest_path, white_contour_px,
+            white_contour_px, image_dest_path)
+        if args.verbose:
+            print(convert_cmd)
+        os.system(convert_cmd)
+
+    model_config_path = os.path.join(
+        args.gazebodir, filename_without_ext.lower(), "model.config")
+    if args.verbose:
+        print(model_config_path)
+    dom = parse(model_config_path)
+    # modify model.config
+    for node in dom.getElementsByTagName('name'):
+        node.firstChild.nodeValue = filename_without_ext
+        if args.verbose:
+            print(node.firstChild.nodeValue)
+        break
+    f = open(os.path.join(
+        args.gazebodir, filename_without_ext.lower(), "model.config"), 'w+')
+    # Write the modified xml file
+    f.write(dom.toxml())
+    f.close()
+
+    # modify model.sdf
+    model_noversion_sdf_path = os.path.join(
+        args.gazebodir, filename_without_ext.lower(), "model.sdf")
+    if args.verbose:
+        print("open model.sdf")
+        print(model_noversion_sdf_path)
+    dom = parse(model_noversion_sdf_path)
+    for node in dom.getElementsByTagName('model'):
+        node.attributes["name"].value = filename_without_ext
+        if args.verbose:
+            print("model tag found")
+        break
+
+    scaleModified = False
+    scale = (args.size + 2 * args.white_contour_size_mm) / \
+        float(ORIGINAL_MARKER_SIZE_MM)
+    for node in dom.getElementsByTagName('mesh'):
+        for child in node.childNodes:
+            if child.nodeName == "scale":
+                child.firstChild.nodeValue = \
+                    "{} {} {}".format(scale, scale, scale)
+                scaleModified = True
+            if child.nodeName == "uri":
+                child.firstChild.nodeValue = "model://" + os.path.join(
+                    filename_without_ext.lower(), "meshes",
+                    filename_without_ext + ".dae")
+                if args.verbose:
+                    print("uri tag found")
+                    print(node.firstChild.nodeValue)
+        if not scaleModified:
+            if args.verbose:
+                print("creating scale tag")
+            x = dom.createElement("scale")
+            y = dom.createTextNode("{} {} {}".format(scale, scale, scale))
+            x.appendChild(y)
+            node.appendChild(x)
+
+    f = open(model_noversion_sdf_path, 'w+')
+    # Write the modified xml file
+    f.write(dom.toxml())
+    f.close()
+
+    # modify model-1_4.sdf
+    model_sdf_path = os.path.join(
+        args.gazebodir, filename_without_ext.lower(), "model-1_4.sdf")
+    cmd = "cp " + model_noversion_sdf_path + " " + model_sdf_path
+    if args.verbose:
+        print(cmd)
+    os.system(cmd)
+
+    if args.verbose:
+        print("open model-1_4.sdf")
+        print(model_sdf_path)
+    dom = parse(model_sdf_path)
+    for node in dom.getElementsByTagName('sdf'):
+        node.attributes["version"].value = "1.4"
+        break
+    f = open(model_sdf_path, 'w+')
+    # Write the modified xml file
+    f.write(dom.toxml())
+    f.close()
+
+    # modify model-1_5.sdf
+    model_sdf_path = os.path.join(
+        args.gazebodir, filename_without_ext.lower(), "model-1_5.sdf")
+    cmd = "cp " + model_noversion_sdf_path + \
+          " " + model_sdf_path
+    if args.verbose:
+        print(cmd)
+    os.system(cmd)
+
+    if args.verbose:
+        print("open model-1_5.sdf")
+        print(model_sdf_path)
+    dom = parse(model_sdf_path)
+    for node in dom.getElementsByTagName('sdf'):
+        node.attributes["version"].value = "1.5"
+        break
+    f = open(model_sdf_path, 'w+')
+    # Write the modified xml file
+    f.write(dom.toxml())
+    f.close()
+
+    meshes_dir = os.path.join(
+        args.gazebodir, filename_without_ext.lower(), "meshes")
+    if args.verbose:
+        print(os.path.join(meshes_dir, "Marker0.dae") +
+              "  newname " + os.path.join(
+                  meshes_dir, filename_without_ext + ".dae"))
+    os.rename(
+        os.path.join(meshes_dir, "Marker0.dae"),
+        os.path.join(meshes_dir, filename_without_ext + ".dae"))
+
+    # modify ModelX.dae
+    if args.verbose:
+        print("open ModelX.dae")
+        print(os.path.join(meshes_dir, filename_without_ext + ".dae"))
+    dom = parse(os.path.join(meshes_dir, filename_without_ext + ".dae"))
+    for node in dom.getElementsByTagName('init_from'):
+        node.firstChild.nodeValue = image_file
+        if args.verbose:
+            print("init_from tag found")
+        break
+
+    f = open(os.path.join(
+        meshes_dir, filename_without_ext + ".dae"), 'w+')
+    # Write the modified xml file
+    f.write(dom.toxml())
+    f.close()
--- a/sensing/object_detection_and_tracking/apriltag/script/make_tag.py
+++ b/sensing/object_detection_and_tracking/apriltag/script/make_tag.py
--- a/sitl_config/ugv/catvehicle/urdf/catvehicle1-3.xacro
+++ b/sitl_config/ugv/catvehicle/urdf/catvehicle1-3.xacro
@ -676,7 +676,7 @@ http://www.car-engineer.com/vehicle-inertia-calculation-tool/
            <visual>
              <origin xyz="0 -0.5 0.05" rpy="0 1.57 1.57"/>
              <geometry>
-                <mesh filename="model://qrcode1-3/meshes/qrcode1-3.dae" scale="2 5 2"/>
+                <mesh filename="model://apriltag0-2/meshes/apriltag0-2.dae" scale="2 5 2"/>
              </geometry>
            </visual>