Python InteractiveMarkerServer.setCallback方法代碼示例

# 需要導入模塊: from interactive_markers.interactive_marker_server import InteractiveMarkerServer [as 別名]
# 或者: from interactive_markers.interactive_marker_server.InteractiveMarkerServer import setCallback [as 別名]
class FakeMarkerServer():
    def __init__(self):
        # create a simple TF listener
        self.tf_listener = tf.TransformListener()
        self.grasp_check = rospy.ServiceProxy('/interactive_world_hackathon/grasp_check', GraspCheck)
        self.speak = rospy.ServiceProxy('/tts/speak', Speak)
        self.play = rospy.ServiceProxy('/tts/play', Speak)
        # create the nav client
        self.nav = actionlib.SimpleActionClient('navigate_action', navigateAction)
        self.nav.wait_for_server()
        # create the backup client
        self.backup = actionlib.SimpleActionClient('backup_action', BackupAction)
        self.backup.wait_for_server()
        # create the place action client
        self.place = actionlib.SimpleActionClient('object_manipulator/object_manipulator_place', PlaceAction)
        self.place.wait_for_server()
        # Segmentation client
        self.segclient = actionlib.SimpleActionClient('/object_detection_user_command', UserCommandAction)
        self.segclient.wait_for_server()
        self.recognition=None
        # create the IMGUI action client
        self.imgui = actionlib.SimpleActionClient('imgui_action', IMGUIAction)
        self.imgui.wait_for_server()
        # listen for graspable objects
        rospy.Subscriber('/interactive_object_recognition_result', GraspableObjectList, self.proc_grasp_list)
        # create the save service
        rospy.Service('~save_template', SaveTemplate, self.save)
        self.load_server = actionlib.SimpleActionServer('load_template', LoadAction, execute_cb=self.load, auto_start=False)
        self.load_server.start()
        # create the IM server
        self.server = InteractiveMarkerServer('~fake_marker_server')
        # create return list of templates
        rospy.Service('~print_templates', PrintTemplates, self.get_templates)
        # used to get model meshes
        self.get_mesh = rospy.ServiceProxy('/objects_database_node/get_model_mesh', GetModelMesh)
        # hack to get the grasp
        rospy.Subscriber('/object_manipulator/object_manipulator_pickup/result', PickupActionResult, self.store_grasp)
        self.last_grasp = None
        self.objects = []
        self.objects.append(OBJECT1)
        self.objects.append(OBJECT2)
        self.objects.append(OBJECT3)
        self.reset_objects()
        # check for saved templates
        try:
            self.templates = pickle.load(open(SAVE_FILE, 'rb'))
            rospy.loginfo('Loaded ' + str(len(self.templates.keys())) + ' template(s).')
        except:
            self.templates = dict()
            rospy.loginfo('New template file started.')
        self.play('/home/rctoris/wav/GLaDOS_generic_security_camera_destroyed-2.wav')

    #Service: rosservice call /fake_object_markers/print_templates
    #Returns a string of template names
    #ex) list: ['test_template1','test_template2','test_template3']
    def get_templates(self, req):
        temp_list = []
        if self.templates.keys() is None:
            self.publish_feedback('No templates')
            return
        for obj in self.templates.keys():
            temp_list.append(str(obj))
        #print temp_list
        return PrintTemplatesResponse(temp_list)
        
    def store_grasp(self, msg):
        self.last_grasp = msg.result.grasp
     
    # Given a mesh_id creates a name with format 'object + mesh_id' 
    # ex.)Given '1234', creates 'object_1234' name
    def create_name(self, mesh_id):
        return 'object_' + str(mesh_id)
       
    # Creates a mesh of the given object with the given pose to be visualized by template maker 
    def create_mesh(self, mesh_id, pose):
        response = self.get_mesh(mesh_id)
        mesh = response.mesh
        # build the mesh marker
        marker = Marker()
        marker.color.r = 1.0
        marker.color.g = 0.0
        marker.color.b = 0.0
        marker.color.a = 0.66
        marker.frame_locked = False
        marker.type = Marker.TRIANGLE_LIST
        # add the mesh
        for j in range(len(mesh.triangles)):
            marker.points.append(mesh.vertices[mesh.triangles[j].vertex_indices[0]])
            marker.points.append(mesh.vertices[mesh.triangles[j].vertex_indices[1]])
            marker.points.append(mesh.vertices[mesh.triangles[j].vertex_indices[2]])
        # create the interactive marker
        name =  self.create_name(mesh_id)
        self.server.insert(self.create_im(marker, pose, name), self.process_feedback)
        self.server.setCallback(name, self.release, InteractiveMarkerFeedback.MOUSE_UP)
        self.server.applyChanges()
    
    # Creates an interactive marker 
    # - at the given location and pose 
    # - with a given name 
    # - for given marker object        
#.........這裏部分代碼省略.........