forked from xtdrone/XTDrone
add precision_landing
This commit is contained in:
parent
6b3359a70d
commit
7637b191e5
|
|
@ -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()
|
||||||
|
|
||||||
|
|
@ -65,12 +65,12 @@ if __name__ == "__main__":
|
||||||
rospy.Subscriber("/uav_"+vehicle_id+"/darknet_ros/bounding_boxes", BoundingBoxes, darknet_callback)
|
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(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)
|
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)
|
cmd_pub = rospy.Publisher('/xtdrone/'+vehicle_type+'_'+vehicle_id+'/cmd', String, queue_size=2)
|
||||||
rate = rospy.Rate(60)
|
rate = rospy.Rate(60)
|
||||||
while(True):
|
while not rospy.is_shutdown():
|
||||||
rate.sleep()
|
rate.sleep()
|
||||||
vel_pub.publish(twist)
|
cmd_vel_pub.publish(twist)
|
||||||
cmd_pub.publish(cmd)
|
cmd_pub.publish(cmd)
|
||||||
if find_cnt - find_cnt_last == 0:
|
if find_cnt - find_cnt_last == 0:
|
||||||
if not get_time:
|
if not get_time:
|
||||||
|
|
|
||||||
|
|
@ -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.
Binary file not shown.
|
After Width: | Height: | Size: 1.2 KiB |
|
|
@ -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>
|
||||||
|
|
@ -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>
|
||||||
|
|
@ -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>
|
||||||
|
|
@ -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>
|
||||||
|
|
@ -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>
|
||||||
|
|
@ -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
|
|
@ -676,7 +676,7 @@ http://www.car-engineer.com/vehicle-inertia-calculation-tool/
|
||||||
<visual>
|
<visual>
|
||||||
<origin xyz="0 -0.5 0.05" rpy="0 1.57 1.57"/>
|
<origin xyz="0 -0.5 0.05" rpy="0 1.57 1.57"/>
|
||||||
<geometry>
|
<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>
|
</geometry>
|
||||||
</visual>
|
</visual>
|
||||||
|
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue