Skip to content

Commit f238edf

Browse files
author
lamprecht
committed
added create_map py node
1 parent 6b3a9d3 commit f238edf

File tree

9 files changed

+144
-94
lines changed

9 files changed

+144
-94
lines changed

‎README.md‎

Lines changed: 9 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,16 +1,24 @@
11
# tf_mapping
22

3+
Fixed marker moving camera!
34

45
Stelle so auf, dass zuerst nur Marker_Id = 1 footprint sieht. Dadurch werden folgende Marker erstellt:
56
* Dort wo die Kamera aktuell ist: marker_globe_0
6-
* Dort wo man den Ursprung haben möchte: marker_globe_1 = *footprint of origin* -> set fixed frame to marker_globe_1
7+
* Dort wo man den Ursprung haben möchte: marker_globe_1 = *footprint of origin* -> set fixed frame to marker_globe_1 (Die Position und Orientierung unter tf in RVIZ von Bsp. marker_globe_2 ist dann relativ zu marker_globe_1 (die dort angegebene relative position ist bezüglich des world frames))
78
* Bewege dann Kamera so dass Marker_Id = 1 und Marker_Id = 2 auf einem Bild zu sehen sind dadurch wird dann marker_globe_2 erstellt.
89
* Fahre dann so weiter fort um map aufzubauen.
910
* starte python skript um map abzuspeichern
1011
* starte roslaunch die automatisch fake_marker published.
1112
* diese fake_marker können dann zur navigation genutzt werden.
1213

1314

15+
// rostopic echo /scan |grep frame_id
16+
// rosrun tf tf_monitor /marker_globe_1 /marker_globe_2
17+
// rosrun tf tf_echo /marker_globe_1 /marker_globe_2
18+
- Translation: [-0.043, -0.506, 0.181]
19+
- Rotation: in Quaternion [0.016, 0.690, 0.722, 0.056]
20+
21+
1422

1523
TODO
1624
![aruco_detector_example](https://github.com/CesMak/aruco_detector_ocv/blob/master/data/rviz_example.png)

‎config/debug_rosconsole.conf‎

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,9 @@
1+
# More Info see: http://wiki.ros.org/rosconsole#Configuration
2+
# rosconsole will find this file by default at $ROS_ROOT/config/rosconsole.config
3+
#
4+
# You can define your own by e.g. copying this file and setting
5+
# ROSCONSOLE_CONFIG_FILE (in your environment) to point to the new file
6+
#
7+
log4j.logger.ros=DEBUG
8+
log4j.logger.ros.roscpp.superdebug=WARN
9+
log4j.logger.ros.tf_mapping=DEBUG # add line to $ROS_ROOT/config/rosconsole.config then it works

‎config/tf_mapping.rviz‎

Lines changed: 13 additions & 75 deletions
Original file line numberDiff line numberDiff line change
@@ -9,7 +9,7 @@ Panels:
99
- /TF1
1010
- /TF1/Frames1
1111
Splitter Ratio: 0.39250493
12-
Tree Height: 400
12+
Tree Height: 215
1313
- Class: rviz/Selection
1414
Name: Selection
1515
- Class: rviz/Tool Properties
@@ -88,16 +88,6 @@ Visualization Manager:
8888
Value: true
8989
camera_2:
9090
Value: true
91-
camera_3:
92-
Value: true
93-
camera_4:
94-
Value: true
95-
camera_5:
96-
Value: true
97-
camera_6:
98-
Value: true
99-
camera_7:
100-
Value: true
10191
camera_position:
10292
Value: true
10393
marker_0:
@@ -106,46 +96,18 @@ Visualization Manager:
10696
Value: false
10797
marker_2:
10898
Value: false
109-
marker_3:
110-
Value: false
111-
marker_4:
112-
Value: false
113-
marker_5:
114-
Value: false
115-
marker_6:
116-
Value: true
117-
marker_7:
118-
Value: true
11999
marker_globe_0:
120100
Value: true
121101
marker_globe_1:
122102
Value: true
123103
marker_globe_2:
124104
Value: true
125-
marker_globe_3:
126-
Value: true
127-
marker_globe_4:
128-
Value: true
129-
marker_globe_5:
130-
Value: true
131-
marker_globe_6:
132-
Value: true
133-
marker_globe_7:
105+
marker_globe_2_map:
134106
Value: true
135107
marker_id0:
136108
Value: true
137109
marker_id1:
138110
Value: false
139-
marker_id2:
140-
Value: true
141-
marker_id3:
142-
Value: true
143-
marker_id335:
144-
Value: true
145-
marker_id4:
146-
Value: true
147-
marker_id5:
148-
Value: true
149111
usb_cam:
150112
Value: false
151113
world:
@@ -156,6 +118,9 @@ Visualization Manager:
156118
Show Axes: true
157119
Show Names: true
158120
Tree:
121+
marker_globe_1:
122+
marker_globe_2_map:
123+
{}
159124
world:
160125
camera_position:
161126
{}
@@ -168,37 +133,10 @@ Visualization Manager:
168133
marker_2:
169134
camera_2:
170135
{}
171-
marker_3:
172-
camera_3:
173-
{}
174-
marker_4:
175-
camera_4:
176-
{}
177-
marker_5:
178-
camera_5:
179-
{}
180-
marker_6:
181-
camera_6:
182-
{}
183-
marker_7:
184-
camera_7:
185-
{}
186136
marker_globe_0:
187137
{}
188-
marker_globe_1:
189-
{}
190138
marker_globe_2:
191139
{}
192-
marker_globe_3:
193-
{}
194-
marker_globe_4:
195-
{}
196-
marker_globe_5:
197-
{}
198-
marker_globe_6:
199-
{}
200-
marker_globe_7:
201-
{}
202140
Update Interval: 0
203141
Value: true
204142
Enabled: true
@@ -226,7 +164,7 @@ Visualization Manager:
226164
Views:
227165
Current:
228166
Class: rviz/Orbit
229-
Distance: 1.34019113
167+
Distance: 1.03784406
230168
Enable Stereo Rendering:
231169
Stereo Eye Separation: 0.0599999987
232170
Stereo Focal Distance: 1
@@ -241,18 +179,18 @@ Visualization Manager:
241179
Invert Z Axis: false
242180
Name: Current View
243181
Near Clip Distance: 0.00999999978
244-
Pitch: 0.685399354
182+
Pitch: 0.370399594
245183
Target Frame: <Fixed Frame>
246184
Value: Orbit (rviz)
247-
Yaw: 1.16541147
185+
Yaw: 1.36541617
248186
Saved: ~
249187
Window Geometry:
250188
Displays:
251189
collapsed: false
252-
Height: 1056
190+
Height: 744
253191
Hide Left Dock: false
254192
Hide Right Dock: true
255-
QMainWindow State: 000000ff00000000fd00000004000000000000025700000396fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000280000021c000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000120052006500730075006c00740049006d0067010000024a000001740000001600ffffff000000010000010f0000025efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000280000025e000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000075f0000003efc0100000002fb0000000800540069006d006501000000000000075f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000005020000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
193+
QMainWindow State: 000000ff00000000fd0000000400000000000002570000025efc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000163000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000120052006500730075006c00740049006d00670100000191000000f50000001600ffffff000000010000010f0000025efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000280000025e000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005350000003efc0100000002fb0000000800540069006d00650100000000000005350000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000002d80000025e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
256194
ResultImg:
257195
collapsed: false
258196
Selection:
@@ -263,6 +201,6 @@ Window Geometry:
263201
collapsed: false
264202
Views:
265203
collapsed: true
266-
Width: 1887
267-
X: 33
268-
Y: 24
204+
Width: 1333
205+
X: 1953
206+
Y: 336

‎launch/start_tf_mapping.launch‎

Lines changed: 17 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,23 @@
55
<arg name="use_bag" default="False" /> <!-- use tf bag -->
66
<arg name="bag_name" default="tf_example2_img" />
77
<arg name="start_rviz" default="True" />
8+
<arg name="use_map" default="True" />
9+
<arg name="use_DEBUG_OUTPUT" default="False" /> <!-- currently not working.... -->
10+
11+
<arg name="create_map" default="False" /> <!-- start in extra terminal using rosrun tf_mapping tf_create_map.py -->
12+
13+
14+
<group if="$(arg use_DEBUG_OUTPUT)" >
15+
<node pkg="rosservice" type="rosservice" name="set_logger_level" args="call /tf_mapping_node/set_logger_level '{logger: 'rosout', level: 'ERROR'}'"/>
16+
</group>
17+
18+
<group if="$(arg use_map)" >
19+
<include file="$(find tf_mapping)/launch/use_map.launch"> </include>
20+
</group>
21+
22+
<group if="$(arg create_map)" >
23+
<node pkg="tf_mapping" type="tf_create_map.py" name="tf_create_map" />
24+
</group>
825

926
<!-- start bag file in a loop -->
1027
<group if="$(arg use_bag)" >

‎launch/use_map.launch‎

Lines changed: 16 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,16 @@
1+
<?xml version="1.0"?>
2+
3+
<launch>
4+
5+
6+
<node name="initpos_to_globe_2_map_tf_pub" pkg="tf" type="static_transform_publisher" args="-0.0433303037382, -0.506249892749, 0.180293508012, -0.0182341336922, -0.691375785218, -0.72008267916, -0.0561068191651 /marker_globe_1 /marker_globe_2_map 100 "/>
7+
8+
<node name="initpos_to_globe_3_map_tf_pub" pkg="tf" type="static_transform_publisher" args="0.323812080453, -0.545785709997, 0.185339789778, -0.043425689039, -0.716393339329, -0.695127799002, -0.0411355801089 /marker_globe_1 /marker_globe_3_map 100 "/>
9+
10+
<node name="initpos_to_globe_4_map_tf_pub" pkg="tf" type="static_transform_publisher" args="0.672489815871, -0.632801185369, 0.13629940064, -0.091751399016, -0.720864109193, -0.68381679032, -0.0658119604066 /marker_globe_1 /marker_globe_4_map 100 "/>
11+
12+
<node name="initpos_to_globe_5_map_tf_pub" pkg="tf" type="static_transform_publisher" args="0.728318412087, -0.391433824343, -0.0299443758018, -0.0274085141937, -0.0207744652004, -0.795763861448, 0.604629698045 /marker_globe_1 /marker_globe_5_map 100 "/>
13+
14+
<node name="initpos_to_globe_6_map_tf_pub" pkg="tf" type="static_transform_publisher" args="0.965603789736, -0.155192234575, -0.0203005085124, 0.0191483961819, -0.0454211354031, -0.783110062628, 0.619926519189 /marker_globe_1 /marker_globe_6_map 100 "/>
15+
16+
</launch>

‎launch/use_map1.launch‎

Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,12 @@
1+
<?xml version="1.0"?>
2+
3+
<launch>
4+
5+
6+
<node name="initpos_to_globe_2_map_tf_pub" pkg="tf" type="static_transform_publisher" args="0.42279622257, 0.0386389601821, 0.0126430521968, 0.0138408059436, 0.000143898951555, -0.0177199613191, 0.999747175217 /marker_globe_1 /marker_globe_2_map 100 "/>
7+
8+
<node name="initpos_to_globe_3_map_tf_pub" pkg="tf" type="static_transform_publisher" args="0.734958967542, 0.0186882239643, -0.0448340113008, 0.0239195019051, 0.106593775574, -0.0277574998272, 0.993627266957 /marker_globe_1 /marker_globe_3_map 100 "/>
9+
10+
<node name="initpos_to_globe_4_map_tf_pub" pkg="tf" type="static_transform_publisher" args="0.736215728442, -0.202443016388, 0.387410589174, 0.216495618954, 0.636196382255, 0.699353599773, -0.243491997126 /marker_globe_1 /marker_globe_4_map 100 "/>
11+
12+
</launch>

‎launch/use_map_ex.launch‎

Lines changed: 11 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,11 @@
1+
<?xml version="1.0"?>
2+
3+
<launch>
4+
5+
6+
<!--<node name="initpos_to_globe_2_map_tf_pub" pkg="tf" type="static_transform_publisher"
7+
args="-0.043, -0.506, 0.181 0.016, 0.690, 0.722, 0.056 /marker_globe_1 /marker_globe_2_map 100" />-->
8+
9+
<node name="initpos_to_globe_2_map_tf_pub" pkg="tf" type="static_transform_publisher" args="-0.0439078070927, -0.509405648389, 0.181581539121, -0.0211379183288, -0.69107301685,
10+
-0.719884320888, -0.0611378633042 /marker_globe_1 /marker_globe_2_map 100 "/>
11+
</launch>

‎scripts/tf_create_map.py‎

Lines changed: 41 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,41 @@
1+
#!/usr/bin/env python
2+
import roslib
3+
roslib.load_manifest('tf_mapping')
4+
import rospy
5+
import tf
6+
import rospkg
7+
8+
if __name__ == '__main__':
9+
rospy.init_node('tf_create_map')
10+
rospy.loginfo("Started tf_create_map %s", '')
11+
12+
listener = tf.TransformListener()
13+
14+
rate = rospy.Rate(10.0)
15+
printed_number = 2
16+
max_number = 6 # if this number is reached program is closed file is written.
17+
result_file ="<?xml version=\"1.0\"?> \n \n <launch> \n"
18+
while not rospy.is_shutdown():
19+
tf_to_str = '/marker_globe_'+str(printed_number)
20+
if (printed_number<=max_number):
21+
#print ('Calculate tf from /marker_globe_1 --> '+tf_to_str+" :")
22+
try:
23+
#listener.waitForTransform('/marker_globe_1', tf_to_str, rospy.Time(), rospy.Duration(2.0))
24+
(trans, rot) = listener.lookupTransform('/marker_globe_1', tf_to_str, rospy.Time(0))
25+
str_output = (str(trans[0])+", "+str(trans[1])+", "+str(trans[2])+", "+str(rot[0])+", "+str(rot[1])+", "+str(rot[2])+", "+str(rot[3])+" ")
26+
str_output = "<node name=\"initpos_to_globe_"+str(printed_number)+"_map_tf_pub\" pkg=\"tf\" type=\"static_transform_publisher\" args=\""+str_output+" /marker_globe_1 "+tf_to_str+"_map 100 \"/>"
27+
result_file = result_file+"\n \n"+str_output
28+
printed_number = printed_number+1
29+
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
30+
#print exception
31+
continue
32+
rate.sleep()
33+
else:
34+
result_file = result_file+"\n \n </launch>"
35+
rospack = rospkg.RosPack()
36+
path = rospack.get_path('tf_mapping')+"/launch/use_map.launch"
37+
38+
f = open(path, "w")
39+
f.write(result_file)
40+
print result_file
41+
rospy.signal_shutdown("Finish file is written to: tf_mapping/launch/use_map.launch")

0 commit comments

Comments
 (0)