def test():
rospy.init_node('localizer_client')
l = Localizer()
# Fake data
bb = BoundingBoxes()
bb.bounding_boxes = []
bb.image = Image()
b1 = BoundingBox()
b1.Class = "start_gate_pole"
b2 = BoundingBox()
b2.Class = "dice1"
b3 = BoundingBox()
b3.Class = "dice2"
b4 = BoundingBox()
b4.Class = "random"
bb.bounding_boxes.append(b1)
bb.bounding_boxes.append(b2)
bb.bounding_boxes.append(b3)
bb.bounding_boxes.append(b4)
l.boxes_received(bb)
try:
rospy.spin()
except rospy.ROSInterruptException:
sys.exit()
def camera_image_callback(self, image, camera):
"""Gets images from camera to generate detections on
Computes where the bounding boxes should be in the image, and
fakes YOLO bounding boxes output as well as publishing a debug
image showing where the detections are if turned on
Parameters
----------
image : sensor_msgs.Image
The image from the camera to create detections for
camera : Camera
Holds camera parameters for projecting points
"""
cv_image = self.bridge.imgmsg_to_cv2(image, desired_encoding="rgb8")
if camera.is_ready():
# Fake darknet yolo detections message
bounding_boxes = BoundingBoxes()
bounding_boxes.header = image.header
bounding_boxes.image_header = image.header
bounding_boxes.image = image
bounding_boxes.bounding_boxes = []
for _, obj in self.objects.iteritems():
detections = self.get_detections(obj, camera, image.header.stamp)
if detections is not None:
if camera.debug_image_pub:
self.render_debug_context(cv_image, detections, camera)
for det in detections:
bounding_box = BoundingBox()
bounding_box.Class = det[2]
bounding_box.probability = 1.0
bounding_box.xmin = int(det[0][0])
bounding_box.xmax = int(det[0][1])
bounding_box.ymin = int(det[0][2])
bounding_box.ymax = int(det[0][3])
bounding_boxes.bounding_boxes.append(bounding_box)
# Only publish detection if there are boxes in it
if bounding_boxes.bounding_boxes:
self.darknet_detection_pub.publish(bounding_boxes)
if camera.debug_image_pub:
image_message = self.bridge.cv2_to_imgmsg(cv_image, encoding="rgb8")
camera.debug_image_pub.publish(image_message)