Python InteractiveMarkerServer.get方法代码示例

# 需要导入模块: from interactive_markers.interactive_marker_server import InteractiveMarkerServer [as 别名]
# 或者: from interactive_markers.interactive_marker_server.InteractiveMarkerServer import get [as 别名]

#.........这里部分代码省略.........
        rospy.loginfo("PR2TrajectoryMarkers (%s) is ready", whicharm)

    def create_menu(self):
        """
        Create and populates all the menu entries
        """
        menu_handler = MenuHandler()
        menu_handler.insert("Point the head", 
                callback = self.move_head)
        menu_handler.insert("Add position to trajectory", 
                callback = self.add_point)
        menu_handler.insert("Place marker over gripper", 
                callback = self.place_gripper)
        menu_handler.insert("Execute trjectory", 
                callback = self.execute_trajectory)
        menu_handler.insert("Clear trajectory", 
                callback = self.clear_trajectory)
        menu_handler.insert("Publish trajectory", 
                callback = self.publish_trajectory)
        menu_handler.insert("Move the arm (planning)", 
                callback = self.plan_arm)
        menu_handler.insert("Move the arm (collision-free)", 
                callback = self.collision_free_arm)
        menu_handler.insert("Move the arm to trajectory start (collision-free)",
                callback = self.arm_trajectory_start)
        menu_handler.insert("Update planning scene", 
                callback = self.update_planning)

        menu_handler.apply(self.server, self.int_marker.name)

    def main_callback(self, feedback):
        """
        If the mouse button is released change the gripper color according to 
        the IK result. Quite awkward, trying to get a nicer way to do it.
        """

        #publish the gripper pose
        gripper_pos = PoseStamped()
        gripper_pos.header.frame_id = feedback.header.frame_id
        gripper_pos.pose = feedback.pose
        self.gripper_pose_pub.publish(gripper_pos)
                
        if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
            self.last_gripper_pose =  feedback.pose

        if (self.last_gripper_pose and 
                    feedback.event_type ==  InteractiveMarkerFeedback.MOUSE_UP):
            self.last_gripper_pose = None
            pos = PoseStamped()
            pos.header.frame_id = feedback.header.frame_id
            pos.pose = feedback.pose
            if self.whicharm == "right":
                ik = self.planner.check_ik_right_arm
            else:
                ik = self.planner.check_ik_left_arm
            
            if ik(pos):
                color = None
            else:
                color = (1,0,0,1)

            int_marker = self.server.get(self.int_marker.name)
            int_marker.controls.remove(self.gripper_marker)
            self.gripper_marker = utils.makeGripperMarker(color=color)
            int_marker.controls.append(self.gripper_marker)
            #rospy.loginfo("Control: %s", int_marker.controls)

# 需要导入模块: from interactive_markers.interactive_marker_server import InteractiveMarkerServer [as 别名]
# 或者: from interactive_markers.interactive_marker_server.InteractiveMarkerServer import get [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        
#.........这里部分代码省略.........