def object_msg(self): object_publisher0 = rospy.Publisher('/detector/cat', DetectedObject, queue_size=10) object_publisher1 = rospy.Publisher('/detector/dog', DetectedObject, queue_size=10) object_publisher2 = rospy.Publisher('/detector/stop_sign', DetectedObject, queue_size=10) object_publisher3 = rospy.Publisher('/detector/elephant', DetectedObject, queue_size=10) object_publisher4 = rospy.Publisher('/detector/bicycle', DetectedObject, queue_size=10) publishers = [ object_publisher0, object_publisher1, object_publisher2, object_publisher3, object_publisher4 ] names = ['cat', 'dog', 'stop_sign', 'elephant', 'bicycle'] rate = rospy.Rate(1) # 1 Hz count = 0 while 1: i = count % len(publishers) # publishes the detected object and its location object_msg = DetectedObject() object_msg.id = 0 object_msg.name = names[i] object_msg.confidence = 0 object_msg.distance = 0 object_msg.thetaleft = 0 object_msg.thetaright = 0 object_msg.corners = [0, 0, 0, 0] loc = np.random.rand(2) * 3 object_msg.location_W = loc.tolist() + [1, 0] print('publishing DetectedObject ' + names[i]) publishers[i].publish(object_msg) count += 1 rate.sleep()
def camera_common(self, img_laser_ranges, img, img_bgr8): try: (translation, rotation) = self.tf_listener.lookupTransform( '/map', '/base_footprint', rospy.Time(0)) x_w2b_W = translation[0] y_w2b_W = translation[1] euler = tf.transformations.euler_from_quaternion(rotation) theta = euler[2] pose_w2b_W = np.vstack((x_w2b_W, y_w2b_W, theta)) print("Bacons world nav: " + str(pose_w2b_W[:2].flatten())) nav_flag = True except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): nav_flag = False (img_h, img_w, img_c) = img.shape # runs object detection in the image (boxes, scores, classes, num) = self.run_detection(img) if num > 0: # some objects were detected for (box, sc, cl) in zip(boxes, scores, classes): ymin = int(box[0] * img_h) xmin = int(box[1] * img_w) ymax = int(box[2] * img_h) xmax = int(box[3] * img_w) xcen = int(0.5 * (xmax - xmin) + xmin) ycen = int(0.5 * (ymax - ymin) + ymin) draw_color = (255, 0, 0) if self.object_labels[cl] in ANIMAL_LABELS: draw_color = (0, 255, 0) elif self.object_labels[cl] == 'stop_sign': draw_color = (0, 0, 255) cv2.rectangle(img_bgr8, (xmin, ymin), (xmax, ymax), draw_color, 2) cv2.putText(img_bgr8, self.object_labels[cl], (xmin, ymin - 10), CV2_FONT, .5, draw_color) # computes the vectors in camera frame corresponding to each sides of the box rayleft = self.project_pixel_to_ray(xmin, ycen) rayright = self.project_pixel_to_ray(xmax, ycen) # convert the rays to angles (with 0 poiting forward for the robot) thetaleft = math.atan2(-rayleft[0], rayleft[2]) thetaright = math.atan2(-rayright[0], rayright[2]) if thetaleft < 0: thetaleft += 2. * math.pi if thetaright < 0: thetaright += 2. * math.pi box_height = np.abs(ymax - ymin) animal_labels = range(16, 26) animal_heights = [ 113, # bird 113, # cat 113, # dog 113, # horse 113, # sheep 113, # cow 113, # elephant 113, # bear 113, # zebra 113 # giraffe ] if cl == 13: # Detected a stop sign obj_height = 64 #mm est_dist_flag = True elif cl in animal_labels: # Detected an animal obj_height = animal_heights[cl - 16] est_dist_flag = True else: # Detected something else obj_height = None est_dist_flag = False # if cl == 'stop sign': dist = self.estimate_distance_from_image( box_height, obj_height, est_dist_flag) if nav_flag: pos_obj_W, theta_g = self.estimate_obj_pos_in_world( dist, xcen, ycen, pose_w2b_W) pos_obj_W_wflag = np.vstack( (pos_obj_W.reshape(2, 1), 1.0, theta_g)).flatten() else: pos_obj_W = np.array([0, 0]) pos_obj_W_wflag = np.vstack((pos_obj_W.reshape( (2, 1)), 0.0, 0.0)).flatten() print("Object world pos: " + str(pos_obj_W)) if not self.object_publishers.has_key(cl): self.object_publishers[cl] = rospy.Publisher( '/detector/' + self.object_labels[cl], DetectedObject, queue_size=10) # publishes the detected object and its location object_msg = DetectedObject() object_msg.id = cl object_msg.name = self.object_labels[cl] object_msg.confidence = sc object_msg.distance = dist object_msg.thetaleft = thetaleft object_msg.thetaright = thetaright object_msg.corners = [ymin, xmin, ymax, xmax] object_msg.location_W = pos_obj_W_wflag.tolist() self.object_publishers[cl].publish(object_msg) # displays the camera image cv2.imshow("Camera", img_bgr8) cv2.waitKey(1)