File tree Expand file tree Collapse file tree
CustomRobots/tello_phy/tello_ros/tello_gazebo/worlds Expand file tree Collapse file tree Original file line number Diff line number Diff line change 3131
3232def build_world (name , markers ):
3333 world_file = open (name , "w" )
34- world_file .write ("""<?xml version="1.0"?>
34+ world_file .write (
35+ """<?xml version="1.0"?>
3536
3637<sdf version="1.6">
3738 <world name="default">
@@ -53,41 +54,50 @@ def build_world(name, markers):
5354 </include>
5455
5556 <!-- ArUco markers -->
56- """ )
57+ """
58+ )
5759 for marker in markers :
58- world_file .write (f""" <model name="marker{ marker [0 ]} ">
60+ world_file .write (
61+ f""" <model name="marker{ marker [0 ]} ">
5962 <include>
6063 <static>true</static>
6164 <uri>model://marker_{ marker [0 ]} </uri>
6265 </include>
6366 <pose>{ marker [1 ]} { marker [2 ]} { marker [3 ]} { marker [4 ]} { marker [5 ]} { marker [6 ]} </pose>
6467 </model>
65- """ )
66- world_file .write ("""
68+ """
69+ )
70+ world_file .write (
71+ """
6772 </world>
68- </sdf>""" )
73+ </sdf>"""
74+ )
6975 world_file .close ()
7076
7177
7278def build_map (name , markers ):
7379 map_file = open (name , "w" )
74- map_file .write ("""# Map for orca.world
80+ map_file .write (
81+ """# Map for orca.world
7582# All marker locations are fixed (f: 1)
7683
7784marker_length: 0.1778
7885markers:
79- """ )
86+ """
87+ )
8088 for marker in markers :
8189 t_marker_world = xf .euler_matrix (marker [4 ], marker [5 ], marker [6 ])
8290 t_marker_map = t_marker_world @ t_world_map
8391 r_marker_map = xf .euler_from_matrix (t_marker_map )
8492
85- map_file .write (f""" - id: { marker [0 ]}
93+ map_file .write (
94+ f""" - id: { marker [0 ]}
8695 u: 1
8796 f: 1
8897 xyz: [{ marker [1 ]} , { marker [2 ]} , { marker [3 ]} ]
8998 rpy: [{ r_marker_map [0 ]} , { r_marker_map [1 ]} , { r_marker_map [2 ]} ]
90- """ )
99+ """
100+ )
91101 map_file .close ()
92102
93103
You can’t perform that action at this time.
0 commit comments