add precision_landing

This commit is contained in:
robin_shaun 2021-04-03 21:34:19 +08:00
parent 6b3359a70d
commit 7637b191e5
14 changed files with 851 additions and 4 deletions

View File

@ -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()

View File

@ -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:

View File

@ -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

Binary file not shown.

After

Width:  |  Height:  |  Size: 80 KiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 1.2 KiB

View File

@ -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>

View File

@ -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>

View File

@ -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>

View File

@ -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>

View File

@ -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>

View File

@ -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()

File diff suppressed because one or more lines are too long

View File

@ -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>