Python interactive_marker_server.InteractiveMarkerServer类代码示例

def main(args):
    server = InteractiveMarkerServer("guess_markers")
    guess = Guess()
    for i in range(len(guess.items)):
        server.insert(guess.markers[i], guess.process_feedback)
    server.applyChanges()
    rospy.spin()

class AbstractInteractiveMarker(object):
  def __init__(self, name, X_init, box_scale=0.1):
    self.name = name
    self.X = X_init
    self.marker = createBoxMarker(scale=box_scale)
    self.interactive_marker = create6DofInteractive('/map', name, name,
                                                    X_init, self.marker)
    self.marker_server = InteractiveMarkerServer(name + 'server')
    self.marker_server.insert(self.interactive_marker, self.markerMoved)
    self.marker_server.applyChanges()

  def markerMoved(self, feedback):
    self.X = transform.fromPose(feedback.pose)

 def init_int_marker(self):
     self.ims = InteractiveMarkerServer("simple_marker")
     self.im = InteractiveMarker()
     self.im.header.frame_id = "/ned"
     self.im.name = "my_marker"
     self.im.description = "Simple 1-DOF control"
     
     bm = Marker()
     bm.type = Marker.CUBE
     bm.scale.x = bm.scale.y = bm.scale.z = 0.45
     bm.color.r = 0.0
     bm.color.g = 0.5
     bm.color.b = 0.5
     bm.color.a = 1.0
     
     bc = InteractiveMarkerControl()
     bc.always_visible = True
     bc.markers.append(bm)
     
     self.im.controls.append(bc)
     
     rc = InteractiveMarkerControl()
     rc.name = "move_x"
     rc.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
     
     self.im.controls.append(rc)
     
     self.ims.insert(self.im, self.process_marker_feedback)
     self.ims.applyChanges()

    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 __init__(self, rate = 30.0, filename = None):
        # Marker server
        self.server = InteractiveMarkerServer('camera_marker')
        # TF broadcaster
        self.tf = TransformBroadcaster()

        # Marker pose
        self.pose_mutex = Lock()
        self.marker_position = (0.0, 0.0, 0.0)
        self.marker_orientation = (0.0, 0.0, 0.0, 1.0)

        # Load init position
        self.filename = filename
        if self.filename:
            with open(self.filename, 'r') as stream:
                init_data = yaml.load(stream)['init_position']
                self.marker_position = (init_data['x'], init_data['y'], init_data['z'])
                self.marker_orientation = (0.0, 0.0, 0.0, 1.0)

        # Register shutdown callback
        rospy.on_shutdown(self.shutdown) 

        # Add marker
        self.add_6DOF()

        # Timer for TF broadcaster
        rospy.Timer(rospy.Duration(1.0/rate), self.publish_transform)

    def __init__(self, from_frame, to_frame, position, orientation):
        self.server = InteractiveMarkerServer("posestamped_im")
        o = orientation
        r, p, y = euler_from_quaternion([o.x, o.y, o.z, o.w])
        rospy.loginfo("Publishing transform and PoseStamped from: " +
                      from_frame + " to " + to_frame +
                      " at " + str(position.x) + " " + str(position.y) + " " + str(position.z) +
                      " and orientation " + str(o.x) + " " + str(o.y) +
                      " " + str(o.z) + " " + str(o.w) + " or rpy " +
                      str(r) + " " + str(p) + " " + str(y))
        self.menu_handler = MenuHandler()
        self.from_frame = from_frame
        self.to_frame = to_frame
        # Transform broadcaster
        self.tf_br = tf.TransformBroadcaster()

        self.pub = rospy.Publisher('/posestamped', PoseStamped, queue_size=1)
        rospy.loginfo("Publishing posestampeds at topic: " + str(self.pub.name))
        pose = Pose()
        pose.position = position
        pose.orientation = orientation
        self.last_pose = pose
        self.print_commands(pose)
        self.makeGraspIM(pose)

        self.server.applyChanges()

 def __init__(self, name, X_init, box_scale=0.1):
   self.name = name
   self.X = X_init
   self.marker = createBoxMarker(scale=box_scale)
   self.interactive_marker = create6DofInteractive('/map', name, name,
                                                   X_init, self.marker)
   self.marker_server = InteractiveMarkerServer(name + 'server')
   self.marker_server.insert(self.interactive_marker, self.markerMoved)
   self.marker_server.applyChanges()

    def __init__(self, marker_name):

        host = rospy.get_param("mongodb_host")
        port = rospy.get_param("mongodb_port")

        self._client = pymongo.MongoClient(host, port)
        self._traj = dict()
        self._retrieve_logs()
        self._server = InteractiveMarkerServer(marker_name)

class PlanarInteractiveMarker(object):
  def __init__(self, name, frame_id, X_offset, text_offset, scale, axes_list=[0,1]):
    self.name = name
    self.scale = scale
    self.force_list = [0.] * len(axes_list)
    self.axes_list = axes_list

    # interactive marker
    self.marker = createBoxMarker(scale=0.001)
    intMarker = createVisInteractive(frame_id, name, name+' 2dcontrol', X_offset, self.marker)
    frame = toNumpy(Matrix3d.Identity())

    for i in axes_list:
      control = createTranslationControlMarker('control'+str(i), frame[i,:].tolist()[0])
      intMarker.controls.append(control)

    self.marker_server = InteractiveMarkerServer(name + 'server')
    self.marker_server.insert(intMarker, self.markerMoved)
    self.marker_server.applyChanges()

    # text display of forces
    self.text_marker = TextMarker(name, frame_id, text_offset)
    self.updateText()

  def updateText(self):
    self.text_marker.update(str(self.force_list[0]) + ' N \n' \
      + str(self.force_list[1]) + ' N')

  def markerMoved(self, feedback):
    #TODO: better coding, check the ordering of x,y,z
    for i in self.axes_list:
      if i == 0:
        self.force_list[i] = feedback.pose.position.x * self.scale
      elif i == 1:
        self.force_list[i] = feedback.pose.position.z * self.scale
      elif i == 2:
        self.force_list[i] = feedback.pose.position.y * self.scale
      else:
        print 'error, expected 0-2'

    self.updateText()
    self.text_marker.publish()

class MassInteractiveMarker(object):
  def __init__(self, name, frame_id, X_offset, text_offset, mass, scale):
    self.name = name
    self.mass = mass
    self.scale = scale

    # interactive marker
    self.marker = createBoxMarker(scale=0.001)
    self.interactive_marker = create1DofTransInteractive(frame_id, name, name+' control',
                                X_offset, self.marker, direction=-Vector3d.UnitY())

    self.marker_server = InteractiveMarkerServer(name + 'server')
    self.marker_server.insert(self.interactive_marker, self.markerMoved)
    self.marker_server.applyChanges()

    self.text_marker = TextMarker(name, frame_id, text_offset)

  def markerMoved(self, feedback):
    self.mass = abs(feedback.pose.position.z) * self.scale
    self.text_marker.update(str(self.mass) + ' kg')
    self.text_marker.publish()

    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 __init__(self, whicharm):
        self.whicharm = whicharm
        self.robot_state = pr2_control_utilities.RobotState()
        self.joint_controller = pr2_control_utilities.PR2JointMover(self.robot_state)
        self.planner = pr2_control_utilities.PR2MoveArm(self.joint_controller)
        self.server = InteractiveMarkerServer("~trajectory_markers")
        self.tf_listener = self.planner.tf_listener

        self.visualizer_pub = rospy.Publisher("~trajectory_markers_path",
                MarkerArray)
        self.trajectory_pub = rospy.Publisher("~trajectory_poses", 
                PoseArray)
        self.gripper_pose_pub = rospy.Publisher("~gripper_pose",
                PoseStamped)
        rospy.Subscriber("~overwrite_trajectory", 
                PoseArray,
                self.overwrite_trajectory)
        rospy.Service("~execute_trajectory", Empty, 
                self.execute_trajectory_srv)
        
        # create an interactive marker for our server
        int_marker = InteractiveMarker()
        int_marker.header.frame_id = "/base_link"
        int_marker.pose.position.x = 0.5
        int_marker.pose.position.z = 1.0
        int_marker.name = "move_" + whicharm + "_arm"
        int_marker.description = "Move the " + whicharm + " arm"
        int_marker.scale = 0.2
        self.server.insert(int_marker, self.main_callback)

        # create the main marker shape
        #color = (1,0,0,1)
        color = None
        self.gripper_marker = utils.makeGripperMarker(color=color)
        int_marker.controls.append(self.gripper_marker);
        #add the controls 
        utils.make_6DOF_marker(int_marker)

        self.int_marker = int_marker
        self.create_menu()
        self.server.applyChanges()

        self.trajectory = PoseArray()
        self.trajectory.header.frame_id = "/base_link"
        self.last_gripper_pose = None

        if whicharm == "right":
            self.ik_utils = self.planner.right_ik
        else:
            self.ik_utils = self.planner.left_ik

        self.planning_waiting_time = 10.0
        rospy.loginfo("PR2TrajectoryMarkers (%s) is ready", whicharm)

class WrenchInteractiveMarker(object):
  def __init__(self, name, frame_id, X_init, text_offset, scale):
    self.name = name
    self.marker = createBoxMarker()
    self.interactive_marker = create6DofInteractive(frame_id, name, name,
                                                    X_init, self.marker)
    self.marker_server = InteractiveMarkerServer(name + 'server')
    self.marker_server.insert(self.interactive_marker, self.markerMoved)
    self.marker_server.applyChanges()
    self.text_marker = TextMarker(name, frame_id, text_offset)

    self.X_init_inv = X_init.inv()
    self.scale = scale
    self.forces = Vector3d.Zero()
    self.torques = Vector3d.Zero()

  def markerMoved(self, feedback):
    X = transform.fromPose(feedback.pose) * self.X_init_inv
    self.forces = self.scale[0] * X.translation()
    self.torques = self.scale[1] * sva.rotationError(Matrix3d.Identity(), X.rotation())
    self.text_marker.update(str(self.forces) + '\n' + str(self.torques))
    self.text_marker.publish()

 def __init__(self, side_prefix):
     
     self.ik = IK(side_prefix)
     self.side_prefix = side_prefix
     self._im_server = InteractiveMarkerServer('ik_request_markers')
     self._tf_listener = TransformListener()
     self._menu_handler = MenuHandler()
     self._menu_handler.insert('Move arm here', callback=self.move_to_pose_cb)
     self.is_control_visible = False
     self.ee_pose = self.get_ee_pose()
     self.update_viz()
     self._menu_handler.apply(self._im_server, 'ik_target_marker')
     self._im_server.applyChanges()

    def __init__(self, side_prefix):

        
        self.side_prefix = side_prefix
        self._im_server = InteractiveMarkerServer('ik_request_markers_' + self.side_prefix)
        self._tf_listener = TransformListener()
        self._menu_handler = MenuHandler()

        self._menu_handler.insert('Move arm here', callback=self.move_to_pose_cb)

        self.r_joint_names = ['r_shoulder_pan_joint',
                              'r_shoulder_lift_joint',
                              'r_upper_arm_roll_joint',
                              'r_elbow_flex_joint',
                              'r_forearm_roll_joint',
                              'r_wrist_flex_joint',
                              'r_wrist_roll_joint']
        self.l_joint_names = ['l_shoulder_pan_joint',
                              'l_shoulder_lift_joint',
                              'l_upper_arm_roll_joint',
                              'l_elbow_flex_joint',
                              'l_forearm_roll_joint',
                              'l_wrist_flex_joint',
                              'l_wrist_roll_joint']

        # Create a trajectory action client
        r_traj_contoller_name = None
        l_traj_contoller_name = None
        if self.side_prefix == 'r':
                r_traj_controller_name = '/r_arm_controller/joint_trajectory_action'
                self.r_traj_action_client = SimpleActionClient(r_traj_controller_name,
                        JointTrajectoryAction)
                rospy.loginfo('Waiting for a response from the trajectory '
                        + 'action server for RIGHT arm...')
                self.r_traj_action_client.wait_for_server()
        
        else:
                l_traj_controller_name = '/l_arm_controller/joint_trajectory_action'
                self.l_traj_action_client = SimpleActionClient(l_traj_controller_name,
                        JointTrajectoryAction)
                rospy.loginfo('Waiting for a response from the trajectory '
                        + 'action server for LEFT arm...')
                self.l_traj_action_client.wait_for_server()

        self.is_control_visible = False
        self.ee_pose = self.get_ee_pose()
        self.ik = IK(side_prefix)
        self.update_viz()
        self._menu_handler.apply(self._im_server, 'ik_target_marker_' + self.side_prefix)
        self._im_server.applyChanges()
        print self.ik