Skip to content

Commit 0dc3249

Browse files
combined self.transform_to_global_frame and self.global_frame check into one line
1 parent a976f9a commit 0dc3249

File tree

1 file changed

+34
-36
lines changed

1 file changed

+34
-36
lines changed

kf_hungarian_tracker/kf_hungarian_tracker/kf_hungarian_node.py

Lines changed: 34 additions & 36 deletions
Original file line numberDiff line numberDiff line change
@@ -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

Comments
 (0)