Esempio n. 1
0
    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)
Esempio n. 2
0
    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()
Esempio n. 3
0
    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)
Esempio n. 4
0
    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)
Esempio n. 5
0
    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)