in deepracer_offroad_ws/qr_detection_pkg/qr_detection_pkg/qr_detection_node.py [0:0]
def get_valid_detection_delta(self, waypoints):
"""Method to get valid detection delta to be published for navigation
from the detected waypoint information.
Args:
waypoints (Dictionary): Consisting of the waypoints decoded from QR Code
Returns:
valid_detection_delta (DetectionDeltaMsg): The delta_x, delta_y values to be published
valid_detection_point (list): List of valid detection points.
valid_waypoints (list): List of valid waypoints.
invalid_waypoints (list): List of invalid waypoints.
"""
try:
invalid_waypoints = []
valid_waypoints = []
# Continue the ongoing action delta (None / any custom action delta)
valid_detection_delta = self.ongoing_action
valid_detection_point = [0, 0]
# Keep track of last 3 waypoints detected
# This is required in case more than 3 consecutive input images dont decode a QR Code,
# we should publish a [0.0, 0.0] delta to trigger No Action in navigation.
self.last_three_waypoints.append(len(waypoints))
if len(self.last_three_waypoints) > 3:
self.last_three_waypoints.pop(0)
# Check if consecutive 3 input images did not detect a waypoint
# and valid detection is set to None
if not any(self.last_three_waypoints) and valid_detection_delta is None:
# Publish a [0.0, 0.0] delta triggering a No Action in navigation.
valid_detection_delta = self.calculate_delta(self.target_x,
self.target_y,
self.target_x,
self.target_y)
else:
# Find the average delta_x, delta_y of all the valid qr codes detected.
delta_x = 0
delta_y = 0
for wp_num, wp in sorted(waypoints.items()):
detection_delta = self.calculate_delta(self.target_x,
self.target_y,
wp[constants.QRKeys.REFERENCE][0],
wp[constants.QRKeys.REFERENCE][1])
if abs(detection_delta.delta[1]) > constants.DELTA_DISTANCE_THRESHOLD_Y:
# Detection is valid only if the detected delta in y is greater than
# the threshold value. This helps to filter out detections which are too
# close to the sensor (already navigated across) and focus on the next QRs.
delta_x += detection_delta.delta[0]
delta_y += detection_delta.delta[1]
valid_detection_point[0] += wp[constants.QRKeys.REFERENCE][0]
valid_detection_point[1] += wp[constants.QRKeys.REFERENCE][1]
valid_waypoints.append(wp)
else:
invalid_waypoints.append(wp)
if len(valid_waypoints) > 0:
# Calculate the delta to be published as an average of the delta values wrt
# the valid waypoints. This creates an average path for the car to follow when
# multiple QR codes influence the detection.
valid_detection_delta = DetectionDeltaMsg()
valid_detection_delta.delta = [delta_x / len(valid_waypoints),
delta_y / len(valid_waypoints)]
valid_detection_point = [valid_detection_point[0] // len(valid_waypoints),
valid_detection_point[1] // len(valid_waypoints)]
return valid_detection_delta, valid_detection_point, valid_waypoints, invalid_waypoints
except Exception as ex:
self.get_logger().error(f"Failed to get valid detection delta: {ex}")
# Destroy the ROS Node running in another thread as well.
self.destroy_node()
rclpy.shutdown()