Python InteractiveMarkerServer.clear方法代碼示例

# 需要導入模塊: from interactive_markers.interactive_marker_server import InteractiveMarkerServer [as 別名]
# 或者: from interactive_markers.interactive_marker_server.InteractiveMarkerServer import clear [as 別名]
class World:
    """Object recognition and localization related stuff"""

    tf_listener = None
    objects = []

    def __init__(self):

        if World.tf_listener == None:
            World.tf_listener = TransformListener()
        self._lock = threading.Lock()
        self.surface = None
        self._tf_broadcaster = TransformBroadcaster()
        self._im_server = InteractiveMarkerServer("world_objects")
        bb_service_name = "find_cluster_bounding_box"
        rospy.wait_for_service(bb_service_name)
        self._bb_service = rospy.ServiceProxy(bb_service_name, FindClusterBoundingBox)
        rospy.Subscriber("interactive_object_recognition_result", GraspableObjectList, self.receieve_object_info)
        self._object_action_client = actionlib.SimpleActionClient("object_detection_user_command", UserCommandAction)
        self._object_action_client.wait_for_server()
        rospy.loginfo("Interactive object detection action " + "server has responded.")
        self.clear_all_objects()
        # The following is to get the table information
        rospy.Subscriber("tabletop_segmentation_markers", Marker, self.receieve_table_marker)

    def _reset_objects(self):
        """Function that removes all objects"""
        self._lock.acquire()
        for i in range(len(World.objects)):
            self._im_server.erase(World.objects[i].int_marker.name)
            self._im_server.applyChanges()
        if self.surface != None:
            self._remove_surface()
        self._im_server.clear()
        self._im_server.applyChanges()
        World.objects = []
        self._lock.release()

    def receieve_table_marker(self, marker):
        """Callback function for markers to determine table"""
        if marker.type == Marker.LINE_STRIP:
            if len(marker.points) == 6:
                rospy.loginfo("Received a TABLE marker.")
                xmin = marker.points[0].x
                ymin = marker.points[0].y
                xmax = marker.points[2].x
                ymax = marker.points[2].y
                depth = xmax - xmin
                width = ymax - ymin

                pose = Pose(marker.pose.position, marker.pose.orientation)
                pose.position.x = pose.position.x + xmin + depth / 2
                pose.position.y = pose.position.y + ymin + width / 2
                dimensions = Vector3(depth, width, 0.01)
                self.surface = World._get_surface_marker(pose, dimensions)
                self._im_server.insert(self.surface, self.marker_feedback_cb)
                self._im_server.applyChanges()

    def receieve_object_info(self, object_list):
        """Callback function to receive object info"""
        self._lock.acquire()
        rospy.loginfo("Received recognized object list.")
        if len(object_list.graspable_objects) > 0:
            for i in range(len(object_list.graspable_objects)):
                models = object_list.graspable_objects[i].potential_models
                if len(models) > 0:
                    object_pose = None
                    best_confidence = 0.0
                    for j in range(len(models)):
                        if best_confidence < models[j].confidence:
                            object_pose = models[j].pose.pose
                            best_confidence = models[j].confidence
                    if object_pose != None:
                        rospy.logwarn("Adding the recognized object " + "with most confident model.")
                        self._add_new_object(object_pose, Vector3(0.2, 0.2, 0.2), True, object_list.meshes[i])
                else:
                    rospy.logwarn("... this is not a recognition result, " + "it is probably just segmentation.")
                    cluster = object_list.graspable_objects[i].cluster
                    bbox = self._bb_service(cluster)
                    cluster_pose = bbox.pose.pose
                    if cluster_pose != None:
                        rospy.loginfo(
                            "Adding unrecognized object with pose:"
                            + World.pose_to_string(cluster_pose)
                            + "\n"
                            + "In ref frame"
                            + str(bbox.pose.header.frame_id)
                        )
                        self._add_new_object(cluster_pose, bbox.box_dims, False)
        else:
            rospy.logwarn("... but the list was empty.")
        self._lock.release()

    @staticmethod
    def get_pose_from_transform(transform):
        """Returns pose for transformation matrix"""
        pos = transform[:3, 3].copy()
        rot = tf.transformations.quaternion_from_matrix(transform)
        return Pose(Point(pos[0], pos[1], pos[2]), Quaternion(rot[0], rot[1], rot[2], rot[3]))

#.........這裏部分代碼省略.........

# 需要導入模塊: from interactive_markers.interactive_marker_server import InteractiveMarkerServer [as 別名]
# 或者: from interactive_markers.interactive_marker_server.InteractiveMarkerServer import clear [as 別名]
class World:
    '''Object recognition and localization related stuff'''

    tf_listener = None
    objects = []

    def __init__(self):

        if World.tf_listener == None:
            World.tf_listener = TransformListener()
        self._lock = threading.Lock()
        self.surface = None
        self._tf_broadcaster = TransformBroadcaster()
        self._im_server = InteractiveMarkerServer('world_objects')
        bb_service_name = 'find_cluster_bounding_box'
        rospy.wait_for_service(bb_service_name)
        self._bb_service = rospy.ServiceProxy(bb_service_name,
                                            FindClusterBoundingBox)
        rospy.Subscriber('interactive_object_recognition_result',
            GraspableObjectList, self.receive_object_info)
        self._object_action_client = actionlib.SimpleActionClient(
            'object_detection_user_command', UserCommandAction)
        self._object_action_client.wait_for_server()
        rospy.loginfo('Interactive object detection action ' +
                      'server has responded.')
        self.clear_all_objects()
        # The following is to get the table information
        rospy.Subscriber('tabletop_segmentation_markers',
                         Marker, self.receive_table_marker)

        rospy.Subscriber("ar_pose_marker",
                         AlvarMarkers, self.receive_ar_markers)
        self.marker_dims = Vector3(0.04, 0.04, 0.01)


    def receive_ar_markers(self, data):
        '''Callback function to receive marker info'''
        self._lock.acquire()
        if len(data.markers) > 0:
            rospy.loginfo('Received non-empty marker list.')
            for i in range(len(data.markers)):
                marker = data.markers[i]
                self._add_new_object(marker.pose.pose, self.marker_dims, marker.id)
        self._lock.release()

    def _reset_objects(self):
        '''Function that removes all objects'''
        self._lock.acquire()
        for i in range(len(World.objects)):
            self._im_server.erase(World.objects[i].int_marker.name)
            self._im_server.applyChanges()
        if self.surface != None:
            self._remove_surface()
        self._im_server.clear()
        self._im_server.applyChanges()
        World.objects = []
        self._lock.release()

    def get_tool_id(self):
        if (len(self.objects) == 0):
            rospy.warning('There are no fiducials, cannot get tool ID.')
            return None
        elif (len(self.objects) > 1):
            rospy.warning('There are more than one fiducials, returning the first as tool ID.')
        return World.objects[0].marker_id

    def get_surface(self):
        if (len(self.objects) < 4):
            rospy.warning('There are not enough fiducials to detect surface.')
            return None
        elif (len(self.objects) > 4):
            rospy.warning('There are more than four fiducials for surface, will use first four.')
        return 

        points = [World.objects[0].position, World.objects[1].position,
                    World.objects[2].position, World.objects[3].position]
        s = Surface(points)
        return s

    def receive_table_marker(self, marker):
        '''Callback function for markers to determine table'''
        if (marker.type == Marker.LINE_STRIP):
            if (len(marker.points) == 6):
                rospy.loginfo('Received a TABLE marker.')
                xmin = marker.points[0].x
                ymin = marker.points[0].y
                xmax = marker.points[2].x
                ymax = marker.points[2].y
                depth = xmax - xmin
                width = ymax - ymin

                pose = Pose(marker.pose.position, marker.pose.orientation)
                pose.position.x = pose.position.x + xmin + depth / 2
                pose.position.y = pose.position.y + ymin + width / 2
                dimensions = Vector3(depth, width, 0.01)
                self.surface = World._get_surface_marker(pose, dimensions)
                self._im_server.insert(self.surface,
                                     self.marker_feedback_cb)
                self._im_server.applyChanges()

#.........這裏部分代碼省略.........

# 需要導入模塊: from interactive_markers.interactive_marker_server import InteractiveMarkerServer [as 別名]
# 或者: from interactive_markers.interactive_marker_server.InteractiveMarkerServer import clear [as 別名]
class InteractivePose2DTF(Node):
    """
    A node that provides an interactive draggable marker, which publishes a tf
    """

    pub_marker = Publisher('/marker', MarkerArray, queue_size=1)

    world_frame = Param(str, default='map')
    target_frame = Param(str, default='base_link')

    def __init__(self):
        self.transform = None

        super(InteractivePose2DTF, self).__init__()

        self.pub_tf = tf2_ros.TransformBroadcaster()

        self.interact = InteractiveMarkerServer("interactive_markers")

        self.marker_pose = Pose()

        self._set_marker(ColorRGBA(0.5, 0.5, 0.5, 0.5))

    def _set_marker(self, color):
        self.interact.clear()

        marker = InteractiveMarker(
            header=Header(
                frame_id=self.world_frame
            ),
            scale=1,
            name="Robot",
            controls=[
                InteractiveMarkerControl(
                    interaction_mode=InteractiveMarkerControl.MOVE_ROTATE,
                    orientation=Quaternion(*transformations.quaternion_from_euler(0, np.pi/2, 0)),
                    markers=[
                        Marker(
                            type=Marker.ARROW,
                            scale=Vector3(0.5, 0.05, 0.05),
                            color=color,
                            pose=Pose(
                                orientation=Quaternion(0, 0, 0, 1)
                            )
                        )
                    ]
                )
            ],
            pose=self.marker_pose
        )

        self.interact.insert(marker, self.processFeedback)
        self.interact.applyChanges()

    def processFeedback(self, feedback):
        if feedback.event_type != InteractiveMarkerFeedback.POSE_UPDATE:
            return

        self.marker_pose = feedback.pose


    @Timer(rospy.Duration(0.1))
    def timer_tf_pub(self, event):
        t = TransformStamped()
        t.header.stamp = rospy.Time.now()
        t.header.frame_id = self.world_frame
        t.child_frame_id = self.target_frame
        t.transform.translation = self.marker_pose.position
        t.transform.rotation = self.marker_pose.orientation
        self.pub_tf.sendTransform(t)

    @Subscriber('~color', ColorRGBA)
    def sub_color(self, color):
        self._set_marker(color)

    @Subscriber('/initialpose', PoseWithCovarianceStamped)
    def sub_initialpose(self, pose):
        """ The default topic used by rviz """
        self.marker_pose = pose.pose.pose
        self._set_marker(ColorRGBA(0.5, 0.5, 0.5, 0.5))