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): # 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
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 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