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)