@@ -103,43 +103,41 @@ def callback(self, msg):
103103 for obj in self .obstacle_list :
104104 obj .predict (dt )
105105
106- if self .transform_to_global_frame :
107- # transform to global frame
108- if self .global_frame is not None :
109- try :
110- trans = self .tf_buffer .lookup_transform (
111- self .global_frame , msg .header .frame_id , rclpy .time .Time ()
112- )
113- msg .header .frame_id = self .global_frame
114- # do_transform_vector3(vector, trans) resets trans.transform.translation
115- # values to 0.0, so we need to preserve them for future usage in the loop below
116- translation_backup_x = trans .transform .translation .x
117- translation_backup_y = trans .transform .translation .y
118- translation_backup_z = trans .transform .translation .z
119- for i in range (len (detections )):
120- trans .transform .translation .x = translation_backup_x
121- trans .transform .translation .y = translation_backup_y
122- trans .transform .translation .z = translation_backup_z
123- # transform position (point)
124- p = PointStamped ()
125- p .point = detections [i ].position
126- detections [i ].position = do_transform_point (p , trans ).point
127- # transform velocity (vector3)
128- v = Vector3Stamped ()
129- v .vector = detections [i ].velocity
130- detections [i ].velocity = do_transform_vector3 (v , trans ).vector
131- # transform size (vector3)
132- s = Vector3Stamped ()
133- s .vector = detections [i ].size
134- detections [i ].size = do_transform_vector3 (s , trans ).vector
135-
136- except TransformException as ex :
137- self .get_logger ().error (
138- "fail to get tf from {} to {}: {}" .format (
139- msg .header .frame_id , self .global_frame , ex
140- )
106+ if (self .transform_to_global_frame ) and (self .global_frame is not None ):
107+ try :
108+ trans = self .tf_buffer .lookup_transform (
109+ self .global_frame , msg .header .frame_id , rclpy .time .Time ()
110+ )
111+ msg .header .frame_id = self .global_frame
112+ # do_transform_vector3(vector, trans) resets trans.transform.translation
113+ # values to 0.0, so we need to preserve them for future usage in the loop below
114+ translation_backup_x = trans .transform .translation .x
115+ translation_backup_y = trans .transform .translation .y
116+ translation_backup_z = trans .transform .translation .z
117+ for i in range (len (detections )):
118+ trans .transform .translation .x = translation_backup_x
119+ trans .transform .translation .y = translation_backup_y
120+ trans .transform .translation .z = translation_backup_z
121+ # transform position (point)
122+ p = PointStamped ()
123+ p .point = detections [i ].position
124+ detections [i ].position = do_transform_point (p , trans ).point
125+ # transform velocity (vector3)
126+ v = Vector3Stamped ()
127+ v .vector = detections [i ].velocity
128+ detections [i ].velocity = do_transform_vector3 (v , trans ).vector
129+ # transform size (vector3)
130+ s = Vector3Stamped ()
131+ s .vector = detections [i ].size
132+ detections [i ].size = do_transform_vector3 (s , trans ).vector
133+
134+ except TransformException as ex :
135+ self .get_logger ().error (
136+ "fail to get tf from {} to {}: {}" .format (
137+ msg .header .frame_id , self .global_frame , ex
141138 )
142- return
139+ )
140+ return
143141
144142 # hungarian matching
145143 cost = np .zeros ((num_of_obstacle , num_of_detect ))
0 commit comments