def camera_common(self, img_laser_ranges, img, img_bgr8):
(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:
# create list of detected objects
detected_objects = DetectedObjectList()
# 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)
cv2.rectangle(img_bgr8, (xmin, ymin), (xmax, ymax),
(255, 0, 0), 2)
# 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
# estimate the corresponding distance using the lidar
dist = self.estimate_distance(thetaleft, thetaright,
img_laser_ranges)
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]
self.object_publishers[cl].publish(object_msg)
# add detected object to detected objects list
detected_objects.objects.append(self.object_labels[cl])
detected_objects.ob_msgs.append(object_msg)
self.detected_objects_pub.publish(detected_objects)
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_callback(self, msg):
""" callback for camera images """
# save the corresponding laser scan
img_laser_ranges = list(self.laser_ranges)
try:
img = self.bridge.imgmsg_to_cv2(msg, "passthrough")
img_bgr8 = self.bridge.imgmsg_to_cv2(msg, "bgr8")
except CvBridgeError as e:
print(e)
(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)
cv2.rectangle(img_bgr8, (xmin, ymin), (xmax, ymax),
(255, 0, 0), 2)
# 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
# estimate the corresponding distance using the lidar
dist = self.estimate_distance(thetaleft, thetaright,
img_laser_ranges)
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]
self.object_publishers[cl].publish(object_msg)
# displays the camera image
cv2.imshow("Camera", img_bgr8)
cv2.waitKey(1)
def camera_common(self, img_laser_ranges, img, img_bgr8):
(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)
# print('(xcen, ycen)')
# print(xcen, ycen)
# print('(dx, dy)')
# print(xmax-xmin, ymax-ymin)
# print('area squared')
# print(np.sqrt((xmax-xmin)*(ymax-ymin)))
cv2.rectangle(img_bgr8, (xmin, ymin), (xmax, ymax),
(255, 0, 0), 2)
# 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
# estimate the corresponding distance using the lidar
dist = self.estimate_distance(thetaleft, thetaright,
img_laser_ranges)
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]
self.object_publishers[cl].publish(object_msg)
print('~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~')
print(num)
print('height: ' + str(ymax - ymin))
print(object_msg.name)
# displays the camera image (when run on laptop only!)
cv2.imshow("Camera", img_bgr8)
cv2.waitKey(1)
def loop(self):
""" the main loop of the robot. At each iteration, depending on its
mode (i.e. the finite state machine's state), if takes appropriate
actions. This function shouldn't return anything """
if not self.params.use_gazebo:
try:
origin_frame = "/map" if mapping else "/odom"
translation, rotation = self.trans_listener.lookupTransform(
origin_frame, '/base_footprint', rospy.Time(0))
self.x, self.y = translation[0], translation[1]
self.theta = tf.transformations.euler_from_quaternion(
rotation)[2]
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
pass
# logs the current mode
if self.prev_mode != self.mode:
rospy.loginfo("Current mode: %s", self.mode)
self.prev_mode = self.mode
########## Code starts here ##########
# TODO: Currently the state machine will just go to the pose without stopping
# at the stop sign.
if self.mode == Mode.IDLE:
# Send zero velocity
self.stay_idle()
elif self.mode == Mode.POSE:
stop_sign_msg = DetectedObject()
# Moving towards a desired pose
if self.close_to(self.x_g, self.y_g, self.theta_g):
self.mode = Mode.IDLE #if close to the goal, go to IDLE mode
elif stop_sign_msg is not None: #if received message from detector (check code)
self.mode = Mode.NAV #if stop sign detected, go to NAV mode
else:
self.go_to_pose()
elif self.mode == Mode.STOP:
# At a stop sign
if (self.has_stopped()):
self.init_crossing() #if robot has stopped for 3 sec, cross
elif self.mode == Mode.CROSS:
# Crossing an intersection
if (self.has_crossed()
): #if robot has crossed, go back to POSE mode
self.mode = Mode.POSE
elif self.mode == Mode.NAV:
if self.close_to(self.x_g, self.y_g, self.theta_g):
self.mode = Mode.IDLE #if close to goal, go to IDLE mode
else:
self.nav_to_pose()
#stop_sign_msg = DetectedObject() #from subscriber
#self.stop_sign_detected_callback(stop_sign_msg)
# I think we get stuck here - what do we do if we enter Nav mode and the stop sign detection is called but we are not close enough to the stop sign? Return to POSE mode?
#if:
#self.mode = Mode.POSE
#self.mode = Mode.STOP
else:
raise Exception("This mode is not supported: {}".format(
str(self.mode)))
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)