6
6
# through dynamic_graph_bridge.
7
7
#
8
8
9
+ import logging
10
+
9
11
import rospy
10
12
11
13
import tf
12
14
import geometry_msgs .msg
13
15
14
16
15
17
def main ():
16
- rospy .init_node (' tf_publisher' , anonymous = True )
18
+ rospy .init_node (" tf_publisher" , anonymous = True )
17
19
18
- frame = rospy .get_param (' ~frame' , '' )
19
- childFrame = rospy .get_param (' ~child_frame' , '' )
20
- topic = rospy .get_param (' ~topic' , '' )
21
- rateSeconds = rospy .get_param (' ~rate' , 5 )
20
+ frame = rospy .get_param (" ~frame" , "" )
21
+ childFrame = rospy .get_param (" ~child_frame" , "" )
22
+ topic = rospy .get_param (" ~topic" , "" )
23
+ rateSeconds = rospy .get_param (" ~rate" , 5 )
22
24
23
25
if not frame or not childFrame or not topic :
24
- logpy .error ("frame, childFrame and topic are required parameters" )
26
+ logging .error ("frame, childFrame and topic are required parameters" )
25
27
return
26
28
27
29
rate = rospy .Rate (rateSeconds )
@@ -35,11 +37,10 @@ def main():
35
37
ok = False
36
38
while not rospy .is_shutdown () and not ok :
37
39
try :
38
- tl .waitForTransform (childFrame , frame ,
39
- rospy .Time (), rospy .Duration (0.1 ))
40
+ tl .waitForTransform (childFrame , frame , rospy .Time (), rospy .Duration (0.1 ))
40
41
ok = True
41
- except tf .Exception , e :
42
- rospy . logwarn ("waiting for tf transform" )
42
+ except tf .Exception :
43
+ logging . warning ("waiting for tf transform" )
43
44
ok = False
44
45
45
46
while not rospy .is_shutdown ():
@@ -60,4 +61,5 @@ def main():
60
61
pub .publish (transform )
61
62
rate .sleep ()
62
63
64
+
63
65
main ()
0 commit comments