def __init__(self, node_name):
# Initialize the DTROS parent class
super(ObjectDetectionNode,
self).__init__(node_name=node_name,
node_type=NodeType.PERCEPTION)
# Construct publishers
self.pub_obj_dets = rospy.Publisher("~duckie_detected",
BoolStamped,
queue_size=1,
dt_topic_type=TopicType.PERCEPTION)
# Construct subscribers
self.sub_image = rospy.Subscriber("~image/compressed",
CompressedImage,
self.image_cb,
buff_size=10000000,
queue_size=1)
self.sub_thresholds = rospy.Subscriber("~thresholds",
AntiInstagramThresholds,
self.thresholds_cb,
queue_size=1)
self.sub_camera_info = rospy.Subscriber(
f"/{os.environ['VEHICLE_NAME']}/camera_node/camera_info",
CameraInfo,
self.cb_camera_info,
queue_size=1)
self.sub_lane_reading = rospy.Subscriber(
f"/{os.environ['VEHICLE_NAME']}/lane_filter_node/lane_pose",
LanePose,
self.cbLanePoses,
queue_size=1)
self.initialized = False
self.ai_thresholds_received = False
self.anti_instagram_thresholds = dict()
self.ai = AntiInstagram()
self.bridge = CvBridge()
self.ground_projector = None
self.rectifier = None
self.homography = self.load_extrinsics()
self.camera_info_received = False
self.log(str(self.homography))
self.lane_width = rospy.get_param('~lanewidth', None)
self.safe_distance = rospy.get_param('~safe_distance', None)
model_file = rospy.get_param('~model_file', '.')
rospack = rospkg.RosPack()
model_file_absolute = rospack.get_path('object_detection') + model_file
self.model_wrapper = Wrapper(model_file_absolute)
self.initialized = True
self.image_count = 0
self.obstacle_left_lane = False
self.obstacle_right_lane = False
self.log("Initialized!")
def __init__(self, node_name):
super(AntiInstagramNode, self).__init__(node_name=node_name, node_type=NodeType.PERCEPTION)
# Read parameters
self._interval = rospy.get_param("~interval")
self._color_balance_percentage = rospy.get_param("~color_balance_scale")
self._output_scale = rospy.get_param("~output_scale")
# Construct publisher
self.pub = rospy.Publisher(
"~thresholds", AntiInstagramThresholds, queue_size=1, dt_topic_type=TopicType.PERCEPTION
)
# Construct subscriber
self.uncorrected_image_subscriber = rospy.Subscriber(
"~uncorrected_image/compressed",
CompressedImage,
self.store_image_msg,
buff_size=10000000,
queue_size=1,
)
# Initialize Timer
rospy.Timer(rospy.Duration(self._interval), self.calculate_new_parameters)
# Initialize objects and data
self.ai = AntiInstagram()
self.bridge = CvBridge()
self.image_msg = None
self.mutex = Lock()
# ---
self.log("Initialized.")
def __init__(self, node_name):
# Initialize the DTROS parent class
super(ObjectDetectionNode, self).__init__(
node_name=node_name,
node_type=NodeType.PERCEPTION
)
self.initialized = False
self.log("Initializing!")
# Construct publishers
self.pub_obj_dets = rospy.Publisher(
"~duckie_detected",
BoolStamped,
queue_size=1,
dt_topic_type=TopicType.PERCEPTION
)
self. pub_detections_image = rospy.Publisher(
"~object_detections_img", Image, queue_size=1, dt_topic_type=TopicType.DEBUG
)
# Construct subscribers
self.sub_image = rospy.Subscriber(
"~image/compressed",
CompressedImage,
self.image_cb,
buff_size=10000000,
queue_size=1
)
self.sub_thresholds = rospy.Subscriber(
"~thresholds",
AntiInstagramThresholds,
self.thresholds_cb,
queue_size=1
)
self.ai_thresholds_received = False
self.anti_instagram_thresholds=dict()
self.ai = AntiInstagram()
self.bridge = CvBridge()
model_file = rospy.get_param('~model_file','.')
self.veh = rospy.get_namespace().strip("/")
aido_eval = rospy.get_param("~AIDO_eval", False)
self.log(f"AIDO EVAL VAR: {aido_eval}")
self.log("Starting model loading!")
self._debug = rospy.get_param("~debug", False)
self.model_wrapper = Wrapper(aido_eval)
self.log("Finished model loading!")
self.frame_id = 0
self.initialized = True
self.log("Initialized!")
def __init__(self, node_name):
# Initialize the DTROS parent class
super(ObjectDetectionNode,
self).__init__(node_name=node_name,
node_type=NodeType.PERCEPTION)
# Construct publishers
self.pub_obj_dets = rospy.Publisher("~duckie_detected",
BoolStamped,
queue_size=1,
dt_topic_type=TopicType.PERCEPTION)
# Construct subscribers
self.sub_image = rospy.Subscriber("~image/compressed",
CompressedImage,
self.image_cb_det,
buff_size=10000000,
queue_size=1)
self.sub_thresholds = rospy.Subscriber("~thresholds",
AntiInstagramThresholds,
self.thresholds_cb,
queue_size=1)
# self.pub_seglist_filtered = rospy.Publisher("~seglist_filtered",
# SegmentList,
# queue_size=1,
# dt_topic_type=TopicType.DEBUG)
self.pub_segmented_img = rospy.Publisher(
"~debug/segmented_image/compressed",
CompressedImage,
queue_size=1,
dt_topic_type=TopicType.DEBUG)
self.ai_thresholds_received = False
self.anti_instagram_thresholds = dict()
self.ai = AntiInstagram()
self.bridge = CvBridge()
model_file = rospy.get_param('~model_file', '.')
rospack = rospkg.RosPack()
model_file_absolute = rospack.get_path('object_detection') + model_file
self.model_wrapper = Wrapper(model_file_absolute)
self.homography = self.load_extrinsics()
homography = np.array(self.homography).reshape((3, 3))
self.bridge = CvBridge()
self.gpg = GroundProjectionGeometry(160, 120, homography)
# self.gpg = GroundProjectionGeometry(320, 240, homography)
self.initialized = True
self.log("Initialized!")
def __init__(self, node_name):
# Initialize the DTROS parent class
super(ObjectDetectionNode,
self).__init__(node_name=node_name,
node_type=NodeType.PERCEPTION)
self.initialized = False
# Construct publishers
self.pub_obj_dets = rospy.Publisher("~duckie_detected",
BoolStamped,
queue_size=1,
dt_topic_type=TopicType.PERCEPTION)
# Construct subscribers
self.sub_image = rospy.Subscriber("~image/compressed",
CompressedImage,
self.image_cb,
buff_size=10000000,
queue_size=1)
self.sub_thresholds = rospy.Subscriber("~thresholds",
AntiInstagramThresholds,
self.thresholds_cb,
queue_size=1)
self.ai_thresholds_received = False
self.anti_instagram_thresholds = dict()
self.ai = AntiInstagram()
self.bridge = CvBridge()
model_file = rospy.get_param('~model_file', '.')
rospack = rospkg.RosPack()
model_file_absolute = rospack.get_path('object_detection') + model_file
self.model_wrapper = Wrapper(model_file_absolute)
self.frame_id = 0
self.initialized = True
self.log("Initialized!")
class ObjectDetectionNode(DTROS):
def __init__(self, node_name):
# Initialize the DTROS parent class
super(ObjectDetectionNode,
self).__init__(node_name=node_name,
node_type=NodeType.PERCEPTION)
# Construct publishers
self.pub_obj_dets = rospy.Publisher("~duckie_detected",
BoolStamped,
queue_size=1,
dt_topic_type=TopicType.PERCEPTION)
# Construct subscribers
self.sub_image = rospy.Subscriber("~image/compressed",
CompressedImage,
self.image_cb,
buff_size=10000000,
queue_size=1)
self.sub_thresholds = rospy.Subscriber("~thresholds",
AntiInstagramThresholds,
self.thresholds_cb,
queue_size=1)
self.ai_thresholds_received = False
self.anti_instagram_thresholds = dict()
self.ai = AntiInstagram()
self.bridge = CvBridge()
model_file = rospy.get_param('~model_file', '.')
rospack = rospkg.RosPack()
model_file_absolute = rospack.get_path('object_detection') + model_file
self.model_wrapper = Wrapper(model_file_absolute)
self.initialized = True
self.log("Initialized!")
def thresholds_cb(self, thresh_msg):
self.anti_instagram_thresholds["lower"] = thresh_msg.low
self.anti_instagram_thresholds["higher"] = thresh_msg.high
self.ai_thresholds_received = True
def image_cb(self, image_msg):
if not self.initialized:
return
# TODO to get better hz, you might want to only call your wrapper's predict function only once ever 4-5 images?
# This way, you're not calling the model again for two practically identical images. Experiment to find a good number of skipped
# images.
# Decode from compressed image with OpenCV
try:
image = self.bridge.compressed_imgmsg_to_cv2(image_msg)
except ValueError as e:
self.logerr('Could not decode image: %s' % e)
return
# Perform color correction
if self.ai_thresholds_received:
image = self.ai.apply_color_balance(
self.anti_instagram_thresholds["lower"],
self.anti_instagram_thresholds["higher"], image)
image = cv2.resize(image, (224, 224))
bboxes, classes, scores = self.model_wrapper.predict(image)
msg = BoolStamped()
msg.header = image_msg.header
msg.data = self.det2bool(
bboxes[0],
classes[0]) # [0] because our batch size given to the wrapper is 1
self.pub_obj_dets.publish(msg)
def det2bool(self, bboxes, classes):
# TODO remove these debugging prints
print(bboxes)
print(classes)
# This is a dummy solution, remove this next line
return len(bboxes) > 1
# TODO filter the predictions: the environment here is a bit different versus the data collection environment, and your model might output a bit
# of noise. For example, you might see a bunch of predictions with x1=223.4 and x2=224, which makes
# no sense. You should remove these.
# TODO also filter detections which are outside of the road, or too far away from the bot. Only return True when there's a pedestrian (aka a duckie)
# in front of the bot, which you know the bot will have to avoid. A good heuristic would be "if centroid of bounding box is in the center of the image,
# assume duckie is in the road" and "if bouding box's area is more than X pixels, assume duckie is close to us"
obj_det_list = []
for i in range(len(bboxes)):
x1, y1, x2, y2 = bboxes[i]
label = classes[i]
class ObjectDetectionNode(DTROS):
def __init__(self, node_name):
# Initialize the DTROS parent class
super(ObjectDetectionNode,
self).__init__(node_name=node_name,
node_type=NodeType.PERCEPTION)
# Construct publishers
self.pub_obj_dets = rospy.Publisher("~duckie_detected",
BoolStamped,
queue_size=1,
dt_topic_type=TopicType.PERCEPTION)
# Construct subscribers
self.sub_image = rospy.Subscriber("~image/compressed",
CompressedImage,
self.image_cb,
buff_size=10000000,
queue_size=1)
self.sub_thresholds = rospy.Subscriber("~thresholds",
AntiInstagramThresholds,
self.thresholds_cb,
queue_size=1)
self.sub_camera_info = rospy.Subscriber(
f"/{os.environ['VEHICLE_NAME']}/camera_node/camera_info",
CameraInfo,
self.cb_camera_info,
queue_size=1)
self.sub_lane_reading = rospy.Subscriber(
f"/{os.environ['VEHICLE_NAME']}/lane_filter_node/lane_pose",
LanePose,
self.cbLanePoses,
queue_size=1)
self.initialized = False
self.ai_thresholds_received = False
self.anti_instagram_thresholds = dict()
self.ai = AntiInstagram()
self.bridge = CvBridge()
self.ground_projector = None
self.rectifier = None
self.homography = self.load_extrinsics()
self.camera_info_received = False
self.log(str(self.homography))
self.lane_width = rospy.get_param('~lanewidth', None)
self.safe_distance = rospy.get_param('~safe_distance', None)
model_file = rospy.get_param('~model_file', '.')
rospack = rospkg.RosPack()
model_file_absolute = rospack.get_path('object_detection') + model_file
self.model_wrapper = Wrapper(model_file_absolute)
self.initialized = True
self.image_count = 0
self.obstacle_left_lane = False
self.obstacle_right_lane = False
self.log("Initialized!")
def thresholds_cb(self, thresh_msg):
self.anti_instagram_thresholds["lower"] = thresh_msg.low
self.anti_instagram_thresholds["higher"] = thresh_msg.high
self.ai_thresholds_received = True
def cb_camera_info(self, msg):
"""
Initializes a :py:class:`image_processing.GroundProjectionGeometry` object and a
:py:class:`image_processing.Rectify` object for image rectification
Args:
msg (:obj:`sensor_msgs.msg.CameraInfo`): Intrinsic properties of the camera.
"""
if not self.camera_info_received:
self.rectifier = Rectify(msg)
self.ground_projector = GroundProjectionGeometry(
im_width=msg.width,
im_height=msg.height,
homography=np.array(self.homography).reshape((3, 3)))
self.im_width = msg.width
self.im_height = msg.height
self.camera_info_received = True
def cbLanePoses(self, input_pose_msg):
"""Callback receiving pose messages
Computes v and omega using PPController
Args:
input_pose_msg (:obj:`LanePose`): Message containing information about the current lane pose.
"""
self.pose_msg = input_pose_msg
def image_cb(self, image_msg):
if not self.initialized:
return
# TODO to get better hz, you might want to only call your wrapper's predict function only once ever 4-5 images?
# This way, you're not calling the model again for two practically identical images. Experiment to find a good number of skipped
# images.
# Decode from compressed image with OpenCV
try:
image = self.bridge.compressed_imgmsg_to_cv2(image_msg)
except ValueError as e:
self.logerr('Could not decode image: %s' % e)
return
# Perform color correction
if self.ai_thresholds_received:
image = self.ai.apply_color_balance(
self.anti_instagram_thresholds["lower"],
self.anti_instagram_thresholds["higher"], image)
image = cv2.resize(image, (224, 224))
if self.image_count != 0:
self.image_count = np.mod(self.image_count + 1, 3)
else:
bboxes, classes, scores = self.model_wrapper.predict(image)
im_boxed = self.plotWithBoundingBoxes(image, bboxes[0], classes[0],
scores[0])
cv2.imshow('detected objects', im_boxed)
cv2.waitKey(1)
self.det2bool(
bboxes[0], classes[0]
) # [0] because our batch size given to the wrapper is 1
msg = BoolStamped()
msg.header = image_msg.header
if self.obstacle_right_lane:
msg.data = True
if self.obstacle_left_lane:
pass
else:
## OVERTAKING
# msg.data = overtake
pass
self.pub_obj_dets.publish(msg)
def det2bool(self, bboxes, classes):
# TODO remove these debugging prints
# print(bboxes)
# print(classes)
# This is a dummy solution, remove this next line
# return len(bboxes) > 1
# TODO filter the predictions: the environment here is a bit different versus the data collection environment, and your model might output a bit
# of noise. For example, you might see a bunch of predictions with x1=223.4 and x2=224, which makes
# no sense. You should remove these.
# TODO also filter detections which are outside of the road, or too far away from the bot. Only return True when there's a pedestrian (aka a duckie)
# in front of the bot, which you know the bot will have to avoid. A good heuristic would be "if centroid of bounding box is in the center of the image,
# assume duckie is in the road" and "if bouding box's area is more than X pixels, assume duckie is close to us"
self.obstacle_right_lane = False
self.obstacle_left_lane = False
obj_det_list = []
for i in range(len(bboxes)):
x1, y1, x2, y2 = bboxes[i]
label = classes[i]
if label == 1:
if (x2 - x1 >= 2) and (y2 - y1 >= 2):
low_center = Point((x1 + x2) / 2, y2)
rect_pixel = self.rectifier.rectify_point(low_center)
ground_point = self.ground_projector.pixel2ground(
rect_pixel)
duckie_lane_pose = np.cos(self.pose_msg.phi) * (
ground_point.y + self.pose_msg.d) + np.sin(
self.pose_msg.phi) * ground_point.x
dist = np.sqrt(ground_point.x**2 + ground_point.y**2)
if np.abs(duckie_lane_pose
) <= self.lane_width / 2: #in our lane
if dist <= self.safe_distance:
self.obstacle_right_lane = True
elif np.abs(duckie_lane_pose
) > self.lane_width / 2: #in left lane
if dist <= self.safe_distance * 1.5:
self.obstacle_left_lane = True
# TODO if label isn't a duckie, skip
# TODO if detection is a pedestrian in front of us:
# return True
def plotWithBoundingBoxes(self, seg_im, boxes, labels, scores):
for i in range(len(labels)):
cv2.rectangle(seg_im, (boxes[i][0], boxes[i][1]),
(boxes[i][2], boxes[i][3]), (255, 255, 255), 1)
cv2.rectangle(seg_im, (boxes[i][0], boxes[i][1]),
(boxes[i][0] + 20, boxes[i][1] - 6), (255, 255, 255),
cv.FILLED)
cv2.putText(seg_im, f"{labels[i]} : {scores[i]}",
(boxes[i][0], boxes[i][1]), cv.FONT_HERSHEY_COMPLEX,
0.5, (0, 0, 0), 1)
return seg_im
def load_extrinsics(self):
"""
Loads the homography matrix from the extrinsic calibration file.
Returns:
:obj:`numpy array`: the loaded homography matrix
"""
# load intrinsic calibration
cali_file_folder = '/data/config/calibrations/camera_extrinsic/'
cali_file = cali_file_folder + rospy.get_namespace().strip(
"/") + ".yaml"
# Locate calibration yaml file or use the default otherwise
if not os.path.isfile(cali_file):
self.log(
"Can't find calibration file: %s.\n Using default calibration instead."
% cali_file, 'warn')
cali_file = (cali_file_folder + "default.yaml")
# Shutdown if no calibration file not found
if not os.path.isfile(cali_file):
msg = 'Found no calibration file ... aborting'
self.log(msg, 'err')
rospy.signal_shutdown(msg)
try:
with open(cali_file, 'r') as stream:
calib_data = yaml.load(stream)
except yaml.YAMLError:
msg = 'Error in parsing calibration file %s ... aborting' % cali_file
self.log(msg, 'err')
rospy.signal_shutdown(msg)
return calib_data['homography']
class ObjectDetectionNode(DTROS):
def __init__(self, node_name):
# Initialize the DTROS parent class
super(ObjectDetectionNode, self).__init__(
node_name=node_name,
node_type=NodeType.PERCEPTION
)
self.initialized = False
self.log("Initializing!")
# Construct publishers
self.pub_obj_dets = rospy.Publisher(
"~duckie_detected",
BoolStamped,
queue_size=1,
dt_topic_type=TopicType.PERCEPTION
)
self. pub_detections_image = rospy.Publisher(
"~object_detections_img", Image, queue_size=1, dt_topic_type=TopicType.DEBUG
)
# Construct subscribers
self.sub_image = rospy.Subscriber(
"~image/compressed",
CompressedImage,
self.image_cb,
buff_size=10000000,
queue_size=1
)
self.sub_thresholds = rospy.Subscriber(
"~thresholds",
AntiInstagramThresholds,
self.thresholds_cb,
queue_size=1
)
self.ai_thresholds_received = False
self.anti_instagram_thresholds=dict()
self.ai = AntiInstagram()
self.bridge = CvBridge()
model_file = rospy.get_param('~model_file','.')
self.veh = rospy.get_namespace().strip("/")
aido_eval = rospy.get_param("~AIDO_eval", False)
self.log(f"AIDO EVAL VAR: {aido_eval}")
self.log("Starting model loading!")
self._debug = rospy.get_param("~debug", False)
self.model_wrapper = Wrapper(aido_eval)
self.log("Finished model loading!")
self.frame_id = 0
self.initialized = True
self.log("Initialized!")
def thresholds_cb(self, thresh_msg):
self.anti_instagram_thresholds["lower"] = thresh_msg.low
self.anti_instagram_thresholds["higher"] = thresh_msg.high
self.ai_thresholds_received = True
def image_cb(self, image_msg):
if not self.initialized:
return
if self.frame_id != 0:
return
self.frame_id += 1
self.frame_id = self.frame_id % (1 + NUMBER_FRAMES_SKIPPED())
# Decode from compressed image with OpenCV
try:
image = self.bridge.compressed_imgmsg_to_cv2(image_msg)
except ValueError as e:
self.logerr('Could not decode image: %s' % e)
return
# Perform color correction
if self.ai_thresholds_received:
image = self.ai.apply_color_balance(
self.anti_instagram_thresholds["lower"],
self.anti_instagram_thresholds["higher"],
image
)
image = cv2.resize(image, (416,416))
bboxes, classes, scores = self.model_wrapper.predict(image)
msg = BoolStamped()
msg.header = image_msg.header
msg.data = self.det2bool(bboxes, classes, scores)
self.pub_obj_dets.publish(msg)
if self._debug:
colors = {0: (0, 255, 255), 1: (0, 165, 255), 2: (0, 250, 0), 3: (0, 0, 255)}
names = {0: "duckie", 1: "cone", 2: "truck", 3: "bus"}
font = cv2.FONT_HERSHEY_SIMPLEX
for clas, box in zip(classes, bboxes):
pt1 = np.array([int(box[0]), int(box[1])])
pt2 = np.array([int(box[2]), int(box[3])])
pt1 = tuple(pt1)
pt2 = tuple(pt2)
color = colors[clas.item()]
name = names[clas.item()]
image = cv2.rectangle(image, pt1, pt2, color, 2)
text_location = (pt1[0], min(416, pt1[1]+20))
image = cv2.putText(image, name, text_location, font, 1, color, thickness=3)
obj_det_img = self.bridge.cv2_to_imgmsg(image, encoding="bgr8")
self.pub_detections_image.publish(obj_det_img)
def det2bool(self, bboxes, classes, scores):
box_ids = np.array(list(map(filter_by_bboxes, bboxes))).nonzero()[0]
cla_ids = np.array(list(map(filter_by_classes, classes))).nonzero()[0]
sco_ids = np.array(list(map(filter_by_scores, scores))).nonzero()[0]
box_cla_ids = set(list(box_ids)).intersection(set(list(cla_ids)))
box_cla_sco_ids = set(list(sco_ids)).intersection(set(list(box_cla_ids)))
if len(box_cla_sco_ids) > 0:
return True
else:
return False
def run_pipeline(image, all_details=False, ground_truth=None, actual_map=None):
"""
Image: numpy (H,W,3) == BGR
Returns a dictionary, res with the following fields:
res['input_image']
ground_truth = pose
"""
print('backend: %s' % matplotlib.get_backend())
print('fname: %s' % matplotlib.matplotlib_fname())
quick = False
dtu.check_isinstance(image, np.ndarray)
res = OrderedDict()
stats = OrderedDict()
vehicle_name = dtu.get_current_robot_name()
res['Raw input image'] = image
gp = get_ground_projection(vehicle_name)
gpg = gp.get_ground_projection_geometry
line_detector = LineDetector()
lane_filter = LaneFilterHistogram(None)
print("pass lane_filter")
ai = AntiInstagram()
pts = ProcessingTimingStats()
pts.reset()
pts.received_message()
pts.decided_to_process()
with pts.phase('calculate AI transform'):
[lower, upper] = ai.calculate_color_balance_thresholds(image)
with pts.phase('apply AI transform'):
transformed = ai.apply_color_balance(lower, upper, image)
with pts.phase('edge detection'):
# note: do not apply transform twice!
segment_list2 = image_prep.process(pts,
image,
line_detector,
transform=ai.apply_color_balance)
if all_details:
res['resized and corrected'] = image_prep.image_corrected
dtu.logger.debug('segment_list2: %s' % len(segment_list2.segments))
if all_details:
res['segments_on_image_input_transformed'] = \
vs_fancy_display(image_prep.image_cv, segment_list2)
if all_details:
res['segments_on_image_input_transformed_resized'] = \
vs_fancy_display(image_prep.image_resized, segment_list2)
if all_details:
grid = get_grid(image.shape[:2])
res['grid'] = grid
res['grid_remapped'] = gpg.rectify(grid)
# res['difference between the two'] = res['image_input']*0.5 + res['image_input_rect']*0.5
with pts.phase('rectify_segments'):
segment_list2_rect = rectify_segments(gpg, segment_list2)
# Project to ground
with pts.phase('find_ground_coordinates'):
sg = find_ground_coordinates(gpg, segment_list2_rect)
lane_filter.initialize()
if all_details:
res['prior'] = lane_filter.get_plot_phi_d()
with pts.phase('lane filter update'):
print(type(lane_filter).__name__)
if type(lane_filter).__name__ == 'LaneFilterHistogram':
# XXX merging pain
_likelihood = lane_filter.update(sg.segments)
else:
_likelihood = lane_filter.update(sg)
if not quick:
with pts.phase('lane filter plot'):
res['likelihood'] = lane_filter.get_plot_phi_d(
ground_truth=ground_truth)
easy_algo_db = get_easy_algo_db()
if isinstance(lane_filter, LaneFilterMoreGeneric):
template_name = lane_filter.localization_template
else:
template_name = 'DT17_template_straight_straight'
dtu.logger.debug('Using default template %r for visualization' %
template_name)
localization_template = \
easy_algo_db.create_instance(FAMILY_LOC_TEMPLATES, template_name)
with pts.phase('lane filter get_estimate()'):
est = lane_filter.get_estimate()
# Coordinates in TILE frame
g = localization_template.pose_from_coords(est)
tinfo = TransformationsInfo()
tinfo.add_transformation(frame1=FRAME_GLOBAL, frame2=FRAME_AXLE, g=g)
if all_details:
if actual_map is not None:
# sm_axle = tinfo.transform_map_to_frame(actual_map, FRAME_AXLE)
res['real'] = plot_map_and_segments(actual_map,
tinfo,
sg.segments,
dpi=120,
ground_truth=ground_truth)
with pts.phase('rectify'):
rectified0 = gpg.rectify(image)
rectified = ai.apply_color_balance(rectified0)
if all_details:
res['image_input_rect'] = rectified
res['segments rectified on image rectified'] = \
vs_fancy_display(rectified, segment_list2_rect)
assumed = localization_template.get_map()
if not quick:
with pts.phase('plot_map_and_segments'):
res['model assumed for localization'] = plot_map_and_segments(
assumed,
tinfo,
sg.segments,
dpi=120,
ground_truth=ground_truth)
assumed_axle = tinfo.transform_map_to_frame(assumed, FRAME_AXLE)
with pts.phase('plot_map reprojected'):
res['map reprojected on image'] = plot_map(rectified,
assumed_axle,
gpg,
do_ground=False,
do_horizon=True,
do_faces=False,
do_faces_outline=True,
do_segments=False)
with pts.phase('quality computation'):
predicted_segment_list_rectified = predict_segments(sm=assumed_axle,
gpg=gpg)
quality_res, quality_stats = judge_quality(
image, segment_list2_rect, predicted_segment_list_rectified)
res.update(quality_res)
# res['blurred']= cv2.medianBlur(image, 11)
stats['estimate'] = est
stats.update(quality_stats)
dtu.logger.info(pts.get_stats())
return res, stats
class ObjectDetectionNode(DTROS):
def __init__(self, node_name):
# Initialize the DTROS parent class
super(ObjectDetectionNode,
self).__init__(node_name=node_name,
node_type=NodeType.PERCEPTION)
# Construct publishers
self.pub_obj_dets = rospy.Publisher("~duckie_detected",
BoolStamped,
queue_size=1,
dt_topic_type=TopicType.PERCEPTION)
# Construct subscribers
self.sub_image = rospy.Subscriber("~image/compressed",
CompressedImage,
self.image_cb_det,
buff_size=10000000,
queue_size=1)
self.sub_thresholds = rospy.Subscriber("~thresholds",
AntiInstagramThresholds,
self.thresholds_cb,
queue_size=1)
# self.pub_seglist_filtered = rospy.Publisher("~seglist_filtered",
# SegmentList,
# queue_size=1,
# dt_topic_type=TopicType.DEBUG)
self.pub_segmented_img = rospy.Publisher(
"~debug/segmented_image/compressed",
CompressedImage,
queue_size=1,
dt_topic_type=TopicType.DEBUG)
self.ai_thresholds_received = False
self.anti_instagram_thresholds = dict()
self.ai = AntiInstagram()
self.bridge = CvBridge()
model_file = rospy.get_param('~model_file', '.')
rospack = rospkg.RosPack()
model_file_absolute = rospack.get_path('object_detection') + model_file
self.model_wrapper = Wrapper(model_file_absolute)
self.homography = self.load_extrinsics()
homography = np.array(self.homography).reshape((3, 3))
self.bridge = CvBridge()
self.gpg = GroundProjectionGeometry(160, 120, homography)
# self.gpg = GroundProjectionGeometry(320, 240, homography)
self.initialized = True
self.log("Initialized!")
def thresholds_cb(self, thresh_msg):
self.anti_instagram_thresholds["lower"] = thresh_msg.low
self.anti_instagram_thresholds["higher"] = thresh_msg.high
self.ai_thresholds_received = True
def image_cb_det(self, image_msg):
if not self.initialized:
return
# Decode from compressed image with OpenCV
try:
image = self.bridge.compressed_imgmsg_to_cv2(image_msg)
except ValueError as e:
self.logerr('Could not decode image: %s' % e)
return
# Perform color correction
if self.ai_thresholds_received:
image = self.ai.apply_color_balance(
self.anti_instagram_thresholds["lower"],
self.anti_instagram_thresholds["higher"], image)
img_reg = cv2.resize(image, (224, 224))
img_rgb = cv2.cvtColor(img_reg, cv2.COLOR_BGR2RGB)
boxes, classes, scores = self.model_wrapper.predict(img_rgb)
boxes, classes, scores = boxes[0], classes[0], scores[0]
if type(boxes) != type(None):
img_w_boxes = add_boxes(img_reg, boxes, classes, scores)
else:
img_w_boxes = img_reg
detection_img = self.bridge.cv2_to_compressed_imgmsg(img_w_boxes)
detection_img.header.stamp = image_msg.header.stamp
self.pub_segmented_img.publish(detection_img)
msg = BoolStamped()
msg.header = image_msg.header
msg.data = self.det2bool(
boxes,
classes) # [0] because our batch size given to the wrapper is 1
self.pub_obj_dets.publish(msg)
#
# msg = BoolStamped()
# msg.header = image_msg.header
# if len(duckie_segments) == 0:
# # No duckie detection at all!
# msg.data = False
# else:
# msg.data = self.det2bool(duckie_segments, min_num_seg=3, x_lim=0.2, y_lim=0.05)
# if msg.data:
# print("A duckie is facing the bot, let's stop and wait for it to cross")
# self.pub_obj_dets.publish(msg)
def det2bool(self, boxes, classes):
if type(boxes) != type(None):
for i in range(len(boxes)):
if classes[
i] != 1: #everything except duckie is not important for now
continue
else:
x1, y1, x2, y2 = boxes[i]
centroid_x = 0.5 * (x1 + x2)
centroid_y = 0.5 * (y1 + y2)
if 224 >= centroid_x >= 0.5 * 224: # in the bottow 50% of the image
if 0.75 * 224 >= centroid_y >= 0.25 * 224: #in the middle third of the image (horizontal)
if abs((x2 - x1) * (y2 - y1)) >= 700:
print("duckie detected")
return True
return False
def load_extrinsics(self):
"""
Loads the homography matrix from the extrinsic calibration file.
Returns:
:obj:`numpy array`: the loaded homography matrix
"""
# load intrinsic calibration
cali_file_folder = '/data/config/calibrations/camera_extrinsic/'
cali_file = cali_file_folder + rospy.get_namespace().strip(
"/") + ".yaml"
# Locate calibration yaml file or use the default otherwise
if not os.path.isfile(cali_file):
self.log(
"Can't find calibration file: %s.\n Using default calibration instead."
% cali_file, 'warn')
cali_file = (cali_file_folder + "default.yaml")
# Shutdown if no calibration file not found
if not os.path.isfile(cali_file):
msg = 'Found no calibration file ... aborting'
self.log(msg, 'err')
rospy.signal_shutdown(msg)
try:
with open(cali_file, 'r') as stream:
calib_data = yaml.load(stream)
except yaml.YAMLError:
msg = 'Error in parsing calibration file %s ... aborting' % cali_file
self.log(msg, 'err')
rospy.signal_shutdown(msg)
return calib_data['homography']
class AntiInstagramNode(DTROS):
"""
Subscriber:
~uncorrected_image/compressed: The uncompressed image coming from the camera
Publisher:
~
"""
def __init__(self, node_name):
super(AntiInstagramNode, self).__init__(node_name=node_name, node_type=NodeType.PERCEPTION)
# Read parameters
self._interval = rospy.get_param("~interval")
self._color_balance_percentage = rospy.get_param("~color_balance_scale")
self._output_scale = rospy.get_param("~output_scale")
# Construct publisher
self.pub = rospy.Publisher(
"~thresholds", AntiInstagramThresholds, queue_size=1, dt_topic_type=TopicType.PERCEPTION
)
# Construct subscriber
self.uncorrected_image_subscriber = rospy.Subscriber(
"~uncorrected_image/compressed",
CompressedImage,
self.store_image_msg,
buff_size=10000000,
queue_size=1,
)
# Initialize Timer
rospy.Timer(rospy.Duration(self._interval), self.calculate_new_parameters)
# Initialize objects and data
self.ai = AntiInstagram()
self.bridge = CvBridge()
self.image_msg = None
self.mutex = Lock()
# ---
self.log("Initialized.")
def store_image_msg(self, image_msg):
with self.mutex:
self.image_msg = image_msg
def decode_image_msg(self):
with self.mutex:
try:
image = self.bridge.compressed_imgmsg_to_cv2(self.image_msg, "bgr8")
except ValueError as e:
self.log(f"Anti_instagram cannot decode image: {e}")
return image
def calculate_new_parameters(self, event):
if self.image_msg is None:
self.log("Waiting for first image!")
return
image = self.decode_image_msg()
(lower_thresholds, higher_thresholds) = self.ai.calculate_color_balance_thresholds(
image, self._output_scale, self._color_balance_percentage
)
# Publish parameters
msg = AntiInstagramThresholds()
msg.low = lower_thresholds
msg.high = higher_thresholds
self.pub.publish(msg)
def __init__(self, node_name):
# Initialize the DTROS parent class
super(LineDetectorNode, self).__init__(node_name=node_name,
node_type=NodeType.PERCEPTION)
# Define parameters
self._line_detector_parameters = rospy.get_param(
"~line_detector_parameters", None)
self._colors = rospy.get_param("~colors", None)
self._img_size = rospy.get_param("~img_size", None)
self._top_cutoff = rospy.get_param("~top_cutoff", None)
self.bridge = CvBridge()
# The thresholds to be used for AntiInstagram color correction
self.ai_thresholds_received = False
self.anti_instagram_thresholds = dict()
self.ai = AntiInstagram()
# This holds the colormaps for the debug/ranges images after they are computed once
self.colormaps = dict()
# Create a new LineDetector object with the parameters from the Parameter Server / config file
self.detector = LineDetector(**self._line_detector_parameters)
# Update the color ranges objects
self.color_ranges = {
color: ColorRange.fromDict(d)
for color, d in list(self._colors.items())
}
# Publishers
self.pub_lines = rospy.Publisher("~segment_list",
SegmentList,
queue_size=1,
dt_topic_type=TopicType.PERCEPTION)
self.pub_d_segments = rospy.Publisher("~debug/segments/compressed",
CompressedImage,
queue_size=1,
dt_topic_type=TopicType.DEBUG)
self.pub_d_edges = rospy.Publisher("~debug/edges/compressed",
CompressedImage,
queue_size=1,
dt_topic_type=TopicType.DEBUG)
self.pub_d_maps = rospy.Publisher("~debug/maps/compressed",
CompressedImage,
queue_size=1,
dt_topic_type=TopicType.DEBUG)
# these are not compressed because compression adds undesired blur
self.pub_d_ranges_HS = rospy.Publisher("~debug/ranges_HS",
Image,
queue_size=1,
dt_topic_type=TopicType.DEBUG)
self.pub_d_ranges_SV = rospy.Publisher("~debug/ranges_SV",
Image,
queue_size=1,
dt_topic_type=TopicType.DEBUG)
self.pub_d_ranges_HV = rospy.Publisher("~debug/ranges_HV",
Image,
queue_size=1,
dt_topic_type=TopicType.DEBUG)
# Subscribers
self.sub_image = rospy.Subscriber("~image/compressed",
CompressedImage,
self.image_cb,
buff_size=10000000,
queue_size=1)
self.sub_thresholds = rospy.Subscriber("~thresholds",
AntiInstagramThresholds,
self.thresholds_cb,
queue_size=1)
class LineDetectorNode(DTROS):
"""
The ``LineDetectorNode`` is responsible for detecting the line white, yellow and red line segment in an image and
is used for lane localization.
Upon receiving an image, this node reduces its resolution, cuts off the top part so that only the
road-containing part of the image is left, extracts the white, red, and yellow segments and publishes them.
The main functionality of this node is implemented in the :py:class:`line_detector.LineDetector` class.
The performance of this node can be very sensitive to its configuration parameters. Therefore, it also provides a
number of debug topics which can be used for fine-tuning these parameters. These configuration parameters can be
changed dynamically while the node is running via ``rosparam set`` commands.
Args:
node_name (:obj:`str`): a unique, descriptive name for the node that ROS will use
Configuration:
~line_detector_parameters (:obj:`dict`): A dictionary with the parameters for the detector. The full list can be found in :py:class:`line_detector.LineDetector`.
~colors (:obj:`dict`): A dictionary of colors and color ranges to be detected in the image. The keys (color names) should match the ones in the Segment message definition, otherwise an exception will be thrown! See the ``config`` directory in the node code for the default ranges.
~img_size (:obj:`list` of ``int``): The desired downsized resolution of the image. Lower resolution would result in faster detection but lower performance, default is ``[120,160]``
~top_cutoff (:obj:`int`): The number of rows to be removed from the top of the image _after_ resizing, default is 40
Subscriber:
~camera_node/image/compressed (:obj:`sensor_msgs.msg.CompressedImage`): The camera images
~anti_instagram_node/thresholds(:obj:`duckietown_msgs.msg.AntiInstagramThresholds`): The thresholds to do color correction
Publishers:
~segment_list (:obj:`duckietown_msgs.msg.SegmentList`): A list of the detected segments. Each segment is an :obj:`duckietown_msgs.msg.Segment` message
~debug/segments/compressed (:obj:`sensor_msgs.msg.CompressedImage`): Debug topic with the segments drawn on the input image
~debug/edges/compressed (:obj:`sensor_msgs.msg.CompressedImage`): Debug topic with the Canny edges drawn on the input image
~debug/maps/compressed (:obj:`sensor_msgs.msg.CompressedImage`): Debug topic with the regions falling in each color range drawn on the input image
~debug/ranges_HS (:obj:`sensor_msgs.msg.Image`): Debug topic with a histogram of the colors in the input image and the color ranges, Hue-Saturation projection
~debug/ranges_SV (:obj:`sensor_msgs.msg.Image`): Debug topic with a histogram of the colors in the input image and the color ranges, Saturation-Value projection
~debug/ranges_HV (:obj:`sensor_msgs.msg.Image`): Debug topic with a histogram of the colors in the input image and the color ranges, Hue-Value projection
"""
def __init__(self, node_name):
# Initialize the DTROS parent class
super(LineDetectorNode, self).__init__(node_name=node_name,
node_type=NodeType.PERCEPTION)
# Define parameters
self._line_detector_parameters = rospy.get_param(
"~line_detector_parameters", None)
self._colors = rospy.get_param("~colors", None)
self._img_size = rospy.get_param("~img_size", None)
self._top_cutoff = rospy.get_param("~top_cutoff", None)
self.bridge = CvBridge()
# The thresholds to be used for AntiInstagram color correction
self.ai_thresholds_received = False
self.anti_instagram_thresholds = dict()
self.ai = AntiInstagram()
# This holds the colormaps for the debug/ranges images after they are computed once
self.colormaps = dict()
# Create a new LineDetector object with the parameters from the Parameter Server / config file
self.detector = LineDetector(**self._line_detector_parameters)
# Update the color ranges objects
self.color_ranges = {
color: ColorRange.fromDict(d)
for color, d in list(self._colors.items())
}
# Publishers
self.pub_lines = rospy.Publisher("~segment_list",
SegmentList,
queue_size=1,
dt_topic_type=TopicType.PERCEPTION)
self.pub_d_segments = rospy.Publisher("~debug/segments/compressed",
CompressedImage,
queue_size=1,
dt_topic_type=TopicType.DEBUG)
self.pub_d_edges = rospy.Publisher("~debug/edges/compressed",
CompressedImage,
queue_size=1,
dt_topic_type=TopicType.DEBUG)
self.pub_d_maps = rospy.Publisher("~debug/maps/compressed",
CompressedImage,
queue_size=1,
dt_topic_type=TopicType.DEBUG)
# these are not compressed because compression adds undesired blur
self.pub_d_ranges_HS = rospy.Publisher("~debug/ranges_HS",
Image,
queue_size=1,
dt_topic_type=TopicType.DEBUG)
self.pub_d_ranges_SV = rospy.Publisher("~debug/ranges_SV",
Image,
queue_size=1,
dt_topic_type=TopicType.DEBUG)
self.pub_d_ranges_HV = rospy.Publisher("~debug/ranges_HV",
Image,
queue_size=1,
dt_topic_type=TopicType.DEBUG)
# Subscribers
self.sub_image = rospy.Subscriber("~image/compressed",
CompressedImage,
self.image_cb,
buff_size=10000000,
queue_size=1)
self.sub_thresholds = rospy.Subscriber("~thresholds",
AntiInstagramThresholds,
self.thresholds_cb,
queue_size=1)
def thresholds_cb(self, thresh_msg):
self.anti_instagram_thresholds["lower"] = thresh_msg.low
self.anti_instagram_thresholds["higher"] = thresh_msg.high
self.ai_thresholds_received = True
def image_cb(self, image_msg):
"""
Processes the incoming image messages.
Performs the following steps for each incoming image:
#. Performs color correction
#. Resizes the image to the ``~img_size`` resolution
#. Removes the top ``~top_cutoff`` rows in order to remove the part of the image that doesn't include the road
#. Extracts the line segments in the image using :py:class:`line_detector.LineDetector`
#. Converts the coordinates of detected segments to normalized ones
#. Creates and publishes the resultant :obj:`duckietown_msgs.msg.SegmentList` message
#. Creates and publishes debug images if there is a subscriber to the respective topics
Args:
image_msg (:obj:`sensor_msgs.msg.CompressedImage`): The receive image message
"""
# Decode from compressed image with OpenCV
try:
image = self.bridge.compressed_imgmsg_to_cv2(image_msg)
except ValueError as e:
self.logerr(f"Could not decode image: {e}")
return
# Perform color correction
if self.ai_thresholds_received:
image = self.ai.apply_color_balance(
self.anti_instagram_thresholds["lower"],
self.anti_instagram_thresholds["higher"], image)
# Resize the image to the desired dimensions
height_original, width_original = image.shape[0:2]
img_size = (self._img_size[1], self._img_size[0])
if img_size[0] != width_original or img_size[1] != height_original:
image = cv2.resize(image,
img_size,
interpolation=cv2.INTER_NEAREST)
image = image[self._top_cutoff:, :, :]
# Extract the line segments for every color
self.detector.setImage(image)
detections = {
color: self.detector.detectLines(ranges)
for color, ranges in list(self.color_ranges.items())
}
# Construct a SegmentList
segment_list = SegmentList()
segment_list.header.stamp = image_msg.header.stamp
# Remove the offset in coordinates coming from the removing of the top part and
arr_cutoff = np.array([0, self._top_cutoff, 0, self._top_cutoff])
arr_ratio = np.array([
1.0 / self._img_size[1],
1.0 / self._img_size[0],
1.0 / self._img_size[1],
1.0 / self._img_size[0],
])
# Fill in the segment_list with all the detected segments
for color, det in list(detections.items()):
# Get the ID for the color from the Segment msg definition
# Throw and exception otherwise
if len(det.lines) > 0 and len(det.normals) > 0:
try:
color_id = getattr(Segment, color)
lines_normalized = (det.lines + arr_cutoff) * arr_ratio
segment_list.segments.extend(
self._to_segment_msg(lines_normalized, det.normals,
color_id))
except AttributeError:
self.logerr(
f"Color name {color} is not defined in the Segment message"
)
# Publish the message
self.pub_lines.publish(segment_list)
# If there are any subscribers to the debug topics, generate a debug image and publish it
if self.pub_d_segments.get_num_connections() > 0:
colorrange_detections = {
self.color_ranges[c]: det
for c, det in list(detections.items())
}
debug_img = plotSegments(image, colorrange_detections)
debug_image_msg = self.bridge.cv2_to_compressed_imgmsg(debug_img)
debug_image_msg.header = image_msg.header
self.pub_d_segments.publish(debug_image_msg)
if self.pub_d_edges.get_num_connections() > 0:
debug_image_msg = self.bridge.cv2_to_compressed_imgmsg(
self.detector.canny_edges)
debug_image_msg.header = image_msg.header
self.pub_d_edges.publish(debug_image_msg)
if self.pub_d_maps.get_num_connections() > 0:
colorrange_detections = {
self.color_ranges[c]: det
for c, det in list(detections.items())
}
debug_img = plotMaps(image, colorrange_detections)
debug_image_msg = self.bridge.cv2_to_compressed_imgmsg(debug_img)
debug_image_msg.header = image_msg.header
self.pub_d_maps.publish(debug_image_msg)
for channels in ["HS", "SV", "HV"]:
publisher = getattr(self, f"pub_d_ranges_{channels}")
if publisher.get_num_connections() > 0:
debug_img = self._plot_ranges_histogram(channels)
debug_image_msg = self.bridge.cv2_to_imgmsg(debug_img,
encoding="bgr8")
debug_image_msg.header = image_msg.header
publisher.publish(debug_image_msg)
@staticmethod
def _to_segment_msg(lines, normals, color):
"""
Converts line detections to a list of Segment messages.
Converts the resultant line segments and normals from the line detection to a list of Segment messages.
Args:
lines (:obj:`numpy array`): An ``Nx4`` array where each row represents a line.
normals (:obj:`numpy array`): An ``Nx2`` array where each row represents the normal of a line.
color (:obj:`str`): Color name string, should be one of the pre-defined in the Segment message definition.
Returns:
:obj:`list` of :obj:`duckietown_msgs.msg.Segment`: List of Segment messages
"""
segment_msg_list = []
for x1, y1, x2, y2, norm_x, norm_y in np.hstack((lines, normals)):
segment = Segment()
segment.color = color
segment.pixels_normalized[0].x = x1
segment.pixels_normalized[0].y = y1
segment.pixels_normalized[1].x = x2
segment.pixels_normalized[1].y = y2
segment.normal.x = norm_x
segment.normal.y = norm_y
segment_msg_list.append(segment)
return segment_msg_list
def _plot_ranges_histogram(self, channels):
"""Utility method for plotting color histograms and color ranges.
Args:
channels (:obj:`str`): The desired two channels, should be one of ``['HS','SV','HV']``
Returns:
:obj:`numpy array`: The resultant plot image
"""
channel_to_axis = {"H": 0, "S": 1, "V": 2}
axis_to_range = {0: 180, 1: 256, 2: 256}
# Get which is the third channel that will not be shown in this plot
missing_channel = "HSV".replace(channels[0],
"").replace(channels[1], "")
hsv_im = self.detector.hsv
# Get the pixels as a list (flatten the horizontal and vertical dimensions)
hsv_im = hsv_im.reshape((-1, 3))
channel_idx = [
channel_to_axis[channels[0]], channel_to_axis[channels[1]]
]
# Get only the relevant channels
x_bins = np.arange(0, axis_to_range[channel_idx[1]] + 1, 2)
y_bins = np.arange(0, axis_to_range[channel_idx[0]] + 1, 2)
h, _, _ = np.histogram2d(x=hsv_im[:, channel_idx[0]],
y=hsv_im[:, channel_idx[1]],
bins=[y_bins, x_bins])
# Log-normalized histogram
np.log(h, out=h, where=(h != 0))
h = (255 * h / np.max(h)).astype(np.uint8)
# Make a color map, for the missing channel, just take the middle of the range
if channels not in self.colormaps:
colormap_1, colormap_0 = np.meshgrid(x_bins[:-1], y_bins[:-1])
colormap_2 = np.ones_like(colormap_0) * (
axis_to_range[channel_to_axis[missing_channel]] / 2)
channel_to_map = {
channels[0]: colormap_0,
channels[1]: colormap_1,
missing_channel: colormap_2
}
self.colormaps[channels] = np.stack([
channel_to_map["H"], channel_to_map["S"], channel_to_map["V"]
],
axis=-1).astype(np.uint8)
self.colormaps[channels] = cv2.cvtColor(self.colormaps[channels],
cv2.COLOR_HSV2BGR)
# resulting histogram image as a blend of the two images
im = cv2.cvtColor(h[:, :, None], cv2.COLOR_GRAY2BGR)
im = cv2.addWeighted(im, 0.5, self.colormaps[channels], 1 - 0.5, 0.0)
# now plot the color ranges on top
for _, color_range in list(self.color_ranges.items()):
# convert HSV color to BGR
c = color_range.representative
c = np.uint8([[[c[0], c[1], c[2]]]])
color = cv2.cvtColor(c, cv2.COLOR_HSV2BGR).squeeze().astype(int)
for i in range(len(color_range.low)):
cv2.rectangle(
im,
pt1=(
(color_range.high[i, channel_idx[1]] / 2).astype(
np.uint8),
(color_range.high[i, channel_idx[0]] / 2).astype(
np.uint8),
),
pt2=(
(color_range.low[i, channel_idx[1]] / 2).astype(
np.uint8),
(color_range.low[i, channel_idx[0]] / 2).astype(
np.uint8),
),
color=color,
lineType=cv2.LINE_4,
)
# ---
return im
class ObjectDetectionNode(DTROS):
def __init__(self, node_name):
# Initialize the DTROS parent class
super(ObjectDetectionNode,
self).__init__(node_name=node_name,
node_type=NodeType.PERCEPTION)
self.initialized = False
# Construct publishers
self.pub_obj_dets = rospy.Publisher("~duckie_detected",
BoolStamped,
queue_size=1,
dt_topic_type=TopicType.PERCEPTION)
# Construct subscribers
self.sub_image = rospy.Subscriber("~image/compressed",
CompressedImage,
self.image_cb,
buff_size=10000000,
queue_size=1)
self.sub_thresholds = rospy.Subscriber("~thresholds",
AntiInstagramThresholds,
self.thresholds_cb,
queue_size=1)
self.ai_thresholds_received = False
self.anti_instagram_thresholds = dict()
self.ai = AntiInstagram()
self.bridge = CvBridge()
model_file = rospy.get_param('~model_file', '.')
rospack = rospkg.RosPack()
model_file_absolute = rospack.get_path('object_detection') + model_file
self.model_wrapper = Wrapper(model_file_absolute)
self.frame_id = 0
self.initialized = True
self.log("Initialized!")
def thresholds_cb(self, thresh_msg):
self.anti_instagram_thresholds["lower"] = thresh_msg.low
self.anti_instagram_thresholds["higher"] = thresh_msg.high
self.ai_thresholds_received = True
def image_cb(self, image_msg):
if not self.initialized:
return
if self.frame_id != 0:
return
self.frame_id += 1
from integration import NUMBER_FRAMES_SKIPPED
self.frame_id = self.frame_id % (1 + NUMBER_FRAMES_SKIPPED())
# Decode from compressed image with OpenCV
try:
image = self.bridge.compressed_imgmsg_to_cv2(image_msg)
except ValueError as e:
self.logerr('Could not decode image: %s' % e)
return
# Perform color correction
if self.ai_thresholds_received:
image = self.ai.apply_color_balance(
self.anti_instagram_thresholds["lower"],
self.anti_instagram_thresholds["higher"], image)
image = cv2.resize(image, (416, 416))
bboxes, classes, scores = self.model_wrapper.predict(image)
msg = BoolStamped()
msg.header = image_msg.header
msg.data = self.det2bool(bboxes, classes, scores)
self.pub_obj_dets.publish(msg)
def det2bool(self, bboxes, classes, scores):
print(f"Before filtering: {len(bboxes)} detections")
from integration import filter_by_classes
from integration import filter_by_bboxes
from integration import filter_by_scores
box_ids = np.array(list(map(filter_by_bboxes, bboxes))).nonzero()
cla_ids = np.array(list(map(filter_by_classes, classes))).nonzero()
sco_ids = np.array(list(map(filter_by_scores, scores))).nonzero()
box_cla_ids = set(box_ids).intersection(set(cla_ids))
box_cla_sco_ids = set(sco_ids).intersection(box_cla_ids)
print(f"After filtering: {len(box_cla_sco_ids)} detections")
if len(box_cla_sco_ids) > 0:
return True
class ObjectDetectionNode(DTROS):
def __init__(self, node_name):
# Initialize the DTROS parent class
super(ObjectDetectionNode,
self).__init__(node_name=node_name,
node_type=NodeType.PERCEPTION)
# Construct publishers
self.pub_obj_dets = rospy.Publisher("~duckie_detected",
BoolStamped,
queue_size=1,
dt_topic_type=TopicType.PERCEPTION)
# Construct subscribers
self.sub_image = rospy.Subscriber("~image/compressed",
CompressedImage,
self.image_cb,
buff_size=10000000,
queue_size=1)
self.sub_thresholds = rospy.Subscriber("~thresholds",
AntiInstagramThresholds,
self.thresholds_cb,
queue_size=1)
self.ai_thresholds_received = False
self.anti_instagram_thresholds = dict()
self.ai = AntiInstagram()
self.bridge = CvBridge()
model_file = rospy.get_param('~model_file', '.')
rospack = rospkg.RosPack()
model_file_absolute = rospack.get_path('object_detection') + model_file
self.model_wrapper = Wrapper(model_file_absolute)
self.initialized = True
self.log("Initialized!")
def thresholds_cb(self, thresh_msg):
self.anti_instagram_thresholds["lower"] = thresh_msg.low
self.anti_instagram_thresholds["higher"] = thresh_msg.high
self.ai_thresholds_received = True
def image_cb(self, image_msg):
if not self.initialized:
return
# TODO to get better hz, you might want to only call your wrapper's predict function only once ever 4-5 images?
# This way, you're not calling the model again for two practically identical images. Experiment to find a good number of skipped
# images.
# Decode from compressed image with OpenCV
try:
image = self.bridge.compressed_imgmsg_to_cv2(image_msg)
except ValueError as e:
self.logerr('Could not decode image: %s' % e)
return
# Perform color correction
if self.ai_thresholds_received:
image = self.ai.apply_color_balance(
self.anti_instagram_thresholds["lower"],
self.anti_instagram_thresholds["higher"], image)
image = cv2.resize(image, (224, 224))
bboxes, classes, scores = self.model_wrapper.predict(image)
msg = BoolStamped()
msg.header = image_msg.header
msg.data = self.det2bool(
bboxes[0],
classes[0]) # [0] because our batch size given to the wrapper is 1
self.pub_obj_dets.publish(msg)
def midpoint(self, p1, p2):
return Point((p1.x + p2.x) / 2, (p1.y + p2.y) / 2)
def det2bool(self, bboxes, classes):
middle_bounds_x = (80, 160)
middle_bounds_y = (100, 224)
for i in range(len(bboxes)):
if abs(bboxes[i][0] - bboxes[i][2]) < 2 or abs(bboxes[i][1] -
bboxes[i][3]) < 2:
print("SKIP")
continue
if classes[i] == 1:
lower = Point(bboxes[i][0], bboxes[i][1])
upper = Point(bboxes[i][2], bboxes[i][3])
middle = self.midpoint(lower, upper)
print("MIDDLE")
print(f"{middle.x},{middle.y}")
if middle.x > middle_bounds_x[
0] and middle.x < middle_bounds_x[1]:
if middle.y > middle_bounds_y[
0] and middle.y < middle_bounds_y[1]:
print("DUCKIE IN FRONT")
return True
return False