def add_image(self):
        rospy.loginfo("add image called!")

        image_marker = Marker()
        image_marker.type = image_marker.POINTS
        image_marker.scale.x = 0.05
        image_marker.scale.y = 0.05
        image_marker.scale.z = 0.05

        for col in xrange(0, self.image.cols, 3):
            for row in xrange(0, self.image.rows, 3):
                # sinking it a bit s.t. the Interactive Markers are easier to grab
                (xx, yy) = im_utils.metersFromPixels(row, col, self.ppm, self.image.rows, self.image.cols)
                pt = Point(xx, yy, -0.05)
                image_marker.points.append(pt)

                (red, green, blue) = im_utils.rosFromCVbridgeColor(self.image[row,col])
                cc = ColorRGBA(red, green, blue, 1.0)
                image_marker.colors.append(cc)

        image_control = InteractiveMarkerControl()
        image_control.always_visible = True
        image_control.markers.append(image_marker)

        image_im = InteractiveMarker()
        image_im.header.frame_id = self.frame_id
        image_im.name = "CameraIM"
        image_im.description = ""
        image_im.controls.append(image_control)
        self.im_server.insert(image_im)
        self.im_server.applyChanges()
    def add_image(self):
        rospy.loginfo("add image called!")

        image_marker = Marker()
        image_marker.type = image_marker.POINTS
        image_marker.scale.x = 0.05
        image_marker.scale.y = 0.05
        image_marker.scale.z = 0.05

        for col in xrange(0, self.image.cols, 3):
            for row in xrange(0, self.image.rows, 3):
                # sinking it a bit s.t. the Interactive Markers are easier to grab
                (xx, yy) = im_utils.metersFromPixels(row, col, self.ppm,
                                                     self.image.rows,
                                                     self.image.cols)
                pt = Point(xx, yy, -0.05)
                image_marker.points.append(pt)

                (red, green,
                 blue) = im_utils.rosFromCVbridgeColor(self.image[row, col])
                cc = ColorRGBA(red, green, blue, 1.0)
                image_marker.colors.append(cc)

        image_control = InteractiveMarkerControl()
        image_control.always_visible = True
        image_control.markers.append(image_marker)

        image_im = InteractiveMarker()
        image_im.header.frame_id = self.frame_id
        image_im.name = "CameraIM"
        image_im.description = ""
        image_im.controls.append(image_control)
        self.im_server.insert(image_im)
        self.im_server.applyChanges()
Ejemplo n.º 3
0
    def add_image(self):
        rospy.loginfo("add image called!")

        menu_handler = MenuHandler()
        menu_foreground = menu_handler.insert("Foreground",
                                              callback=self.foreground_cb)
        menu_background = menu_handler.insert("Background",
                                              callback=self.background_cb)

        image_marker = Marker()
        image_marker.type = image_marker.POINTS
        image_marker.scale.x = 0.05
        image_marker.scale.y = 0.05
        image_marker.scale.z = 0.05

        # displaying every pixel is too much; downsample before turning image into IM
        for col in xrange(0, self.image.cols, 3):
            for row in xrange(0, self.image.rows, 3):
                # sinking it a bit s.t. the Interactive Markers are easier to grab
                (xx, yy) = im_utils.metersFromPixels(row, col, self.ppm,
                                                     self.image.rows,
                                                     self.image.cols)

                mask_val = self.mask[row, col]
                (red, green,
                 blue) = im_utils.rosFromCVbridgeColor(self.image[row, col])
                # TODO: these are magic numbers from opencv, indicating forground/probable foreground
                # (in OpenCV, they're cv::GC_FGC cf::GC_PR_FGD)
                if (mask_val == 1) or (mask_val == 3):
                    pt = Point(xx, yy, 0.0)
                    cc = ColorRGBA(red, green, blue, 1.0)
                else:
                    # putting this one literally in the background
                    pt = Point(xx, yy, -0.5)
                    grey = im_utils.greyFromRGB(red, green, blue)
                    # int is required in order to not be random shades! 0.5 makes it lighter
                    grey = max(int(0.5 * grey), 1)
                    cc = ColorRGBA(grey, grey, grey, 0.5)
                image_marker.points.append(pt)
                image_marker.colors.append(cc)

        image_control = InteractiveMarkerControl()
        image_control.always_visible = True
        image_control.interaction_mode = InteractiveMarkerControl.MENU
        image_control.markers.append(image_marker)

        image_im = InteractiveMarker()
        image_im.header.frame_id = self.frame_id
        image_im.name = "LabeledImage"
        image_im.description = ""
        image_im.controls.append(image_control)

        self.im_server.insert(image_im)
        menu_handler.apply(self.im_server, "LabeledImage")
        self.im_server.applyChanges()
    def add_image(self):
        rospy.loginfo("add image called!")

        menu_handler = MenuHandler()
        menu_foreground = menu_handler.insert("Foreground", callback=self.foreground_cb)
        menu_background = menu_handler.insert("Background", callback=self.background_cb)

        image_marker = Marker()
        image_marker.type = image_marker.POINTS
        image_marker.scale.x = 0.05
        image_marker.scale.y = 0.05
        image_marker.scale.z = 0.05

        # displaying every pixel is too much; downsample before turning image into IM
        for col in xrange(0, self.image.cols, 3):
            for row in xrange(0, self.image.rows, 3):
                # sinking it a bit s.t. the Interactive Markers are easier to grab
                (xx, yy) = im_utils.metersFromPixels(row, col, self.ppm, self.image.rows, self.image.cols)



                mask_val = self.mask[row,col]
                (red, green, blue) = im_utils.rosFromCVbridgeColor(self.image[row,col])
                # TODO: these are magic numbers from opencv, indicating forground/probable foreground
                # (in OpenCV, they're cv::GC_FGC cf::GC_PR_FGD)
                if (mask_val == 1) or (mask_val == 3):
                    pt = Point(xx, yy, 0.0)
                    cc = ColorRGBA(red, green, blue, 1.0)
                else:
                    # putting this one literally in the background
                    pt = Point(xx, yy, -0.5)
                    grey = im_utils.greyFromRGB(red, green, blue)
                    # int is required in order to not be random shades! 0.5 makes it lighter                
                    grey = max(int(0.5*grey),1)
                    cc = ColorRGBA(grey, grey, grey, 0.5)
                image_marker.points.append(pt)
                image_marker.colors.append(cc)

        image_control = InteractiveMarkerControl()
        image_control.always_visible = True
        image_control.interaction_mode = InteractiveMarkerControl.MENU
        image_control.markers.append(image_marker)

        image_im = InteractiveMarker()
        image_im.header.frame_id = self.frame_id
        image_im.name = "LabeledImage"
        image_im.description = ""
        image_im.controls.append(image_control)

        self.im_server.insert(image_im)
        menu_handler.apply(self.im_server, "LabeledImage")
        self.im_server.applyChanges()