in deepracer_offroad_ws/qr_detection_pkg/qr_detection_pkg/qr_detection_node.py [0:0]
def publish_qr_detection_visualization(self,
display_image,
valid_detection_point,
valid_waypoints,
invalid_waypoints):
"""Method to add visualization on top of received image
after inference.
Args:
display_image (Image): The input image.
valid_detection_point (list): List of valid detection points.
valid_waypoints (list): List of valid waypoints.
invalid_waypoints (list): List of invalid waypoints.
"""
try:
# Add visualization for valid waypoints
if len(valid_waypoints) > 0:
waypoint_connection_path_points = []
for valid_waypoint in valid_waypoints:
valid_waypoint_ref = valid_waypoint[constants.QRKeys.REFERENCE]
valid_waypoint_bb = valid_waypoint[constants.QRKeys.BOUNDING_BOX]
valid_waypoint_num = valid_waypoint[constants.QRKeys.WAYPOINT]
valid_waypoint_pos = valid_waypoint[constants.QRKeys.POSITION]
# Drawing bounding boxes with green on the image for valid waypoint.
cv2.rectangle(display_image,
(valid_waypoint_bb[0], valid_waypoint_bb[1]),
(valid_waypoint_bb[2], valid_waypoint_bb[3]),
constants.CVConstants.GREEN,
2)
# Printing reference center on the image for valid waypoint.
cv2.circle(display_image,
(int(valid_waypoint_ref[0]),
int(valid_waypoint_ref[1])),
5,
constants.CVConstants.GREEN,
-1)
cv2.putText(display_image,
f"Valid: {valid_waypoint_num}; Pos: {valid_waypoint_pos}",
(int(valid_waypoint_bb[0]), int(valid_waypoint_bb[3]) + 10),
cv2.FONT_HERSHEY_SIMPLEX,
1,
constants.CVConstants.GREEN,
3)
waypoint_connection_path_points.append((int(valid_waypoint_ref[0]),
int(valid_waypoint_ref[1])))
# Draw path points
start_point = (int(self.target_x),
int(self.target_y))
for end_point in waypoint_connection_path_points:
cv2.line(display_image,
start_point,
end_point,
constants.CVConstants.GREEN,
2,
cv2.LINE_8)
start_point = end_point
# Add visualization for invalid waypoints
if len(invalid_waypoints) > 0:
for invalid_waypoint in invalid_waypoints:
invalid_waypoint_ref = invalid_waypoint[constants.QRKeys.REFERENCE]
invalid_waypoint_bb = invalid_waypoint[constants.QRKeys.BOUNDING_BOX]
invalid_waypoint_num = invalid_waypoint[constants.QRKeys.WAYPOINT]
invalid_waypoint_pos = invalid_waypoint[constants.QRKeys.POSITION]
# Drawing bounding boxes with blue on the image for invalid waypoint.
cv2.rectangle(display_image,
(invalid_waypoint_bb[0], invalid_waypoint_bb[1]),
(invalid_waypoint_bb[2], invalid_waypoint_bb[3]),
constants.CVConstants.BLUE,
2)
# Printing reference center on the image for invalid waypoint.
cv2.circle(display_image,
(int(invalid_waypoint_ref[0]),
int(invalid_waypoint_ref[1])),
5,
constants.CVConstants.BLUE,
-1)
cv2.putText(display_image,
f"Invalid: {invalid_waypoint_num}; Pos: {invalid_waypoint_pos}",
(int(invalid_waypoint_bb[0]), int(invalid_waypoint_bb[3]) + 10),
cv2.FONT_HERSHEY_SIMPLEX,
1,
constants.CVConstants.BLUE,
3)
# Printing target center on the image.
cv2.circle(display_image,
(int(self.target_x),
int(self.target_y)),
5,
constants.CVConstants.LIGHT_GREEN,
-1)
# Plot the ideal trajectory of travel
if valid_detection_point != [0, 0]:
cv2.line(display_image,
(int(self.target_x), int(self.target_y)),
(valid_detection_point[0], valid_detection_point[1]),
constants.CVConstants.LIGHT_GREEN,
2,
cv2.LINE_8)
display_image = self.bridge.cv2_to_imgmsg(np.array(display_image), "bgr8")
# Publish to display topic (Can be viewed on localhost:8080).
self.display_image_publisher.publish(display_image)
except Exception as ex:
self.get_logger().error(f"Failed publish qr detection visualization step: {ex}")
# Destroy the ROS Node running in another thread as well.
self.destroy_node()
rclpy.shutdown()