Python InteractiveMarkerServer.insert方法代码示例

# 需要导入模块: from interactive_markers.interactive_marker_server import InteractiveMarkerServer [as 别名]
# 或者: from interactive_markers.interactive_marker_server.InteractiveMarkerServer import insert [as 别名]
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()

# 需要导入模块: from interactive_markers.interactive_marker_server import InteractiveMarkerServer [as 别名]
# 或者: from interactive_markers.interactive_marker_server.InteractiveMarkerServer import insert [as 别名]
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)

# 需要导入模块: from interactive_markers.interactive_marker_server import InteractiveMarkerServer [as 别名]
# 或者: from interactive_markers.interactive_marker_server.InteractiveMarkerServer import insert [as 别名]
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()

# 需要导入模块: from interactive_markers.interactive_marker_server import InteractiveMarkerServer [as 别名]
# 或者: from interactive_markers.interactive_marker_server.InteractiveMarkerServer import insert [as 别名]
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()

# 需要导入模块: from interactive_markers.interactive_marker_server import InteractiveMarkerServer [as 别名]
# 或者: from interactive_markers.interactive_marker_server.InteractiveMarkerServer import insert [as 别名]
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()

# 需要导入模块: from interactive_markers.interactive_marker_server import InteractiveMarkerServer [as 别名]
# 或者: from interactive_markers.interactive_marker_server.InteractiveMarkerServer import insert [as 别名]
    translate_y.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
    translate_y.orientation.w = 1
    translate_y.orientation.x = 0
    translate_y.orientation.y = 0
    translate_y.orientation.z = 1
    translate_y.name = "translate_y"
    interactive_marker.controls.append(translate_y)

    translate_z = InteractiveMarkerControl()
    translate_z.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
    translate_z.orientation.w = 1
    translate_z.orientation.x = 0
    translate_z.orientation.y = 1
    translate_z.orientation.z = 0
    translate_z.name = "translate_z"
    interactive_marker.controls.append(translate_z)

    def update_object(feedback):
        p = feedback.pose.position
        sim_object.set_position(p.x, p.y, p.z)
        print feedback.marker_name + " is now at " + str(p.x) + ", " + str(p.y) + ", " + str(p.z)

    # add the interactive marker to our collection &
    # tell the server to call update_object() when feedback arrives for it
    server.insert(interactive_marker, update_object)

    # 'commit' changes and send to all clients
    server.applyChanges()

    rospy.spin()

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

#.........这里部分代码省略.........
            now = rospy.get_rostime() 
            self.br.sendTransform(tuple(self.obs_centroid), (0,0,0,1), now, 
                            'obs_centroid', 'velodyne')
            self.obs_marker.header.frame_id = 'obs_centroid'
            self.obs_marker.pose.position = Point(0,0,0)
            self.obs_marker.pose.orientation = Quaternion(*self.orient)
            self.add_bbox_lidar()


    def add_bbox(self):
        inputName = '/image_raw'
        rospy.Subscriber(inputName, Image, self.handle_img_msg, queue_size=1)
         

    def handle_img_msg(self, img_msg):
        img = None
        bridge = cv_bridge.CvBridge()
        try:
            img = bridge.imgmsg_to_cv2(img_msg, 'bgr8')
        except cv_bridge.CvBridgeError as e:
            rospy.logerr( 'image message to cv conversion failed :' )
            rospy.logerr( e )
            print( e )
            return
    
        #tx, ty, tz, yaw, pitch, roll = [0.00749025, -0.40459941, -0.51372948, 
        #                                -1.66780896, -1.59875352, -3.05415572]
        #translation = [tx, ty, tz, 1]
        #rotationMatrix = tf.transformations.euler_matrix(roll, pitch, yaw)
        #rotationMatrix[:, 3] = translation
        md = self.metadata
        dims = np.array([md['l'], md['w'], md['h']])
        outputName = '/image_bbox'
        imgOutput = rospy.Publisher(outputName, Image, queue_size=1)
        obs_centroid = self.obs_centroid
   
        if self.obs_centroid is None:
            rospy.loginfo("Couldn't find obstacle centroid")
            imgOutput.publish(bridge.cv2_to_imgmsg(img, 'bgr8'))
            return
        
        # print centroid info
        rospy.loginfo(str(obs_centroid))

        # case when obstacle is not in camera frame
        if obs_centroid[0]<2.5 :
            imgOutput.publish(bridge.cv2_to_imgmsg(img, 'bgr8'))
            return
    
        # get bbox 
        R = tf.transformations.quaternion_matrix(self.orient)
        corners = [0.5*np.array([i,j,k])*dims for i in [-1,1] 
                    for j in [-1,1] for k in [-1,1]]
        corners = [obs_centroid + R.dot(list(c)+[1])[:3] for c in corners]
        projected_pts = []
        cameraModel = PinholeCameraModel()
        cam_info = load_cam_info(self.calib_file)
        cameraModel.fromCameraInfo(cam_info)
        projected_pts = [cameraModel.project3dToPixel(list(pt)+[1]) for pt in corners]
        #for pt in corners:
        #    rotated_pt = rotationMatrix.dot(list(pt)+[1])
        #    projected_pts.append(cameraModel.project3dToPixel(rotated_pt))
        projected_pts = np.array(projected_pts)
        center = np.mean(projected_pts, axis=0)
        out_img = drawBbox(img, projected_pts)
        imgOutput.publish(bridge.cv2_to_imgmsg(out_img, 'bgr8'))


    def processFeedback(self, feedback ):
        p = feedback.pose.orientation
        self.rotation_offset = [p.x, p.y, p.z, p.w]
        p = feedback.pose.position
        self.offset = [p.x, p.y, p.z]
        self.server.applyChanges()
    
    def obs_processFeedback(self, feedback ):
        p = feedback.pose.orientation
        self.orient = (p.x, p.y, p.z, p.w)
        now = rospy.get_rostime()
        self.br.sendTransform(tuple(self.obs_centroid), (0,0,0,1), now, 
                            'obs_centroid', 'velodyne')
        self.marker.pose.orientation = Quaternion(*self.orient)
        self.server.applyChanges()
    
    def add_bbox_lidar(self):
        if self.obs_centroid is None :
            return
    
        now = rospy.get_rostime() 
        self.velodyne_marker.header.stamp = now #rospy.get_rostime()
        self.obs_marker.header.stamp = now #rospy.get_rostime()
        
        # tell the server to call processFeedback() when feedback arrives for it
        self.server.insert(self.velodyne_marker, self.processFeedback)
        self.server.applyChanges()
        self.server.insert(self.obs_marker, self.obs_processFeedback)
        #self.menu_handler.apply(self.server, self.int_marker.name)
    
        # 'commit' changes and send to all clients
        self.server.applyChanges()

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

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

#.........这里部分代码省略.........

        control = InteractiveMarkerControl()
        control.orientation.w = 1
        control.orientation.x = 1
        control.orientation.y = 0
        control.orientation.z = 0
        control.name = "move_x"
        control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
        control.orientation_mode = InteractiveMarkerControl.FIXED
        int_marker.controls.append(control)

        control = InteractiveMarkerControl()
        control.orientation.w = 1
        control.orientation.x = 0
        control.orientation.y = 1
        control.orientation.z = 0
        control.name = "rotate_z"
        control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
        control.orientation_mode = InteractiveMarkerControl.FIXED
        int_marker.controls.append(control)

        control = InteractiveMarkerControl()
        control.orientation.w = 1
        control.orientation.x = 0
        control.orientation.y = 1
        control.orientation.z = 0
        control.name = "move_z"
        control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
        control.orientation_mode = InteractiveMarkerControl.FIXED
        int_marker.controls.append(control)

        control = InteractiveMarkerControl()
        control.orientation.w = 1
        control.orientation.x = 0
        control.orientation.y = 0
        control.orientation.z = 1
        control.name = "rotate_y"
        control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
        control.orientation_mode = InteractiveMarkerControl.FIXED
        int_marker.controls.append(control)

        control = InteractiveMarkerControl()
        control.orientation.w = 1
        control.orientation.x = 0
        control.orientation.y = 0
        control.orientation.z = 1
        control.name = "move_y"
        control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
        control.orientation_mode = InteractiveMarkerControl.FIXED
        int_marker.controls.append(control)

        control = InteractiveMarkerControl()
        control.orientation.w = 1
        control.orientation.x = 0
        control.orientation.y = 0
        control.orientation.z = 1
        control.name = "move_3d"
        control.interaction_mode = InteractiveMarkerControl.MOVE_3D
        control.orientation_mode = InteractiveMarkerControl.FIXED
        int_marker.controls.append(control)

        self.menu_handler.insert("Publish transform",
                                 callback=self.processFeedback)
        self.menu_handler.insert("Stop publishing transform",
                                 callback=self.processFeedback)

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

    def run(self):
        r = pi/2.0
        p = pi
        y = pi/2.0
        o = quaternion_from_euler(r, p, y)
        opt_frame = self.to_frame + "_rgb_optical_frame"

        r = rospy.Rate(20)
        while not rospy.is_shutdown():
            if self.tf_br is not None:
                pos = self.last_pose.position
                ori = self.last_pose.orientation
                self.tf_br.sendTransform(
                                         (pos.x, pos.y, pos.z),
                                         (ori.x, ori.y, ori.z, ori.w),
                                         rospy.Time.now(),
                                         self.to_frame + "_link",
                                         self.from_frame
                                         )
            ps = PoseStamped()
            ps.pose = self.last_pose
            ps.header.frame_id = self.from_frame
            ps.header.stamp = rospy.Time.now()
            self.pub.publish(ps)
            br = tf.TransformBroadcaster()
            br.sendTransform((0, 0, 0),
                             o,
                              rospy.Time.now(),
                              opt_frame,
                              self.to_frame + "_link")
            r.sleep()

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

    _offset = 0.09
    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 get_ee_pose(self):
        from_frame = '/base_link'
        to_frame = '/' + self.side_prefix + '_wrist_roll_link'
        try:
            t = self._tf_listener.getLatestCommonTime(from_frame, to_frame)
            (pos, rot) = self._tf_listener.lookupTransform(from_frame, to_frame, t)
        except:
            rospy.logwarn('Could not get end effector pose through TF.')
            pos = [1.0, 0.0, 1.0]
            rot = [0.0, 0.0, 0.0, 1.0]

	return Pose(Point(pos[0], pos[1], pos[2]),
			Quaternion(rot[0], rot[1], rot[2], rot[3]))

    def move_to_pose_cb(self, dummy):
        rospy.loginfo('You pressed the `Move arm here` button from the menu.')

        target_pose = GripperMarkers._offset_pose(self.ee_pose)
        ik_solution = self.ik.get_ik_for_ee(target_pose)
    
        #TODO: Send the arms to ik_solution

    def marker_clicked_cb(self, feedback):
        if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
            self.ee_pose = feedback.pose
            self.update_viz()
            self._menu_handler.reApply(self._im_server)
            self._im_server.applyChanges()

        elif feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
            rospy.loginfo('Changing visibility of the pose controls.')
            if (self.is_control_visible):
                self.is_control_visible = False
            else:
                self.is_control_visible = True
        else:
            rospy.loginfo('Unhandled event: ' + str(feedback.event_type))

    def update_viz(self):
        
        menu_control = InteractiveMarkerControl()
        menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
        menu_control.always_visible = True
        frame_id = 'base_link'
        pose = self.ee_pose
        
        menu_control = self._add_gripper_marker(menu_control)
        text_pos = Point()
        text_pos.x = pose.position.x
        text_pos.y = pose.position.y
        text_pos.z = pose.position.z + 0.1
        text = 'x=' + str(pose.position.x) + ' y=' + str(pose.position.y) + ' x=' + str(pose.position.z)
        menu_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING,
                                           id=0, scale=Vector3(0, 0, 0.03),
                                           text=text,
                                           color=ColorRGBA(0.0, 0.0, 0.0, 0.5),
                                           header=Header(frame_id=frame_id),
                                           pose=Pose(text_pos, Quaternion(0, 0, 0, 1))))
        int_marker = InteractiveMarker()
        int_marker.name = 'ik_target_marker'
        int_marker.header.frame_id = frame_id
        int_marker.pose = pose
        int_marker.scale = 0.2
        self._add_6dof_marker(int_marker)
        int_marker.controls.append(menu_control)
        self._im_server.insert(int_marker, self.marker_clicked_cb)

    def _add_gripper_marker(self, control):
        is_hand_open=False
        if is_hand_open:
            angle = 28 * numpy.pi / 180.0
        else:
            angle = 0
        transform1 = tf.transformations.euler_matrix(0, 0, angle)
        transform1[:3, 3] = [0.07691 - GripperMarkers._offset, 0.01, 0]
        transform2 = tf.transformations.euler_matrix(0, 0, -angle)
        transform2[:3, 3] = [0.09137, 0.00495, 0]
        t_proximal = transform1
        t_distal = tf.transformations.concatenate_matrices(transform1, transform2)
        mesh1 = self._make_mesh_marker()
        mesh1.mesh_resource = ('package://pr2_description/meshes/gripper_v0/gripper_palm.dae')
        mesh1.pose.position.x = -GripperMarkers._offset
        mesh1.pose.orientation.w = 1
#.........这里部分代码省略.........

# 需要导入模块: from interactive_markers.interactive_marker_server import InteractiveMarkerServer [as 别名]
# 或者: from interactive_markers.interactive_marker_server.InteractiveMarkerServer import insert [as 别名]
class TFMarkerServer(object):
    """TFMarkerServer"""
    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 add_6DOF(self, init_position = Point( 0.0, 0.0, 0.0), frame_id = 'map'):
        marker = InteractiveMarker()
        marker.header.frame_id = frame_id
        marker.pose.position = init_position
        marker.scale = 0.3

        marker.name = 'camera_marker'
        marker.description = 'Camera 6-DOF pose control'

        # X axis rotation
        control = InteractiveMarkerControl()
        control.orientation.w = 1
        control.orientation.x = 1
        control.orientation.y = 0
        control.orientation.z = 0
        control.name = "rotate_x"
        control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
        marker.controls.append(control)
        # X axis traslation
        control = InteractiveMarkerControl()
        control.orientation.w = 1
        control.orientation.x = 1
        control.orientation.y = 0
        control.orientation.z = 0
        control.name = "move_x"
        control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
        marker.controls.append(control)
        # Y axis rotation
        control = InteractiveMarkerControl()
        control.orientation.w = 1
        control.orientation.x = 0
        control.orientation.y = 1
        control.orientation.z = 0
        control.name = "rotate_y"
        control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
        marker.controls.append(control)
        # Y axis traslation
        control = InteractiveMarkerControl()
        control.orientation.w = 1
        control.orientation.x = 0
        control.orientation.y = 1
        control.orientation.z = 0
        control.name = "move_y"
        control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
        marker.controls.append(control)
        # Z axis rotation
        control = InteractiveMarkerControl()
        control.orientation.w = 1
        control.orientation.x = 0
        control.orientation.y = 0
        control.orientation.z = 1
        control.name = "rotate_z"
        control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
        marker.controls.append(control)
        # Z axis traslation
        control = InteractiveMarkerControl()
        control.orientation.w = 1
        control.orientation.x = 0
        control.orientation.y = 0
        control.orientation.z = 1
        control.name = "move_z"
        control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
        marker.controls.append(control)
        # Add marker to server
        self.server.insert(marker, self.marker_feedback)
        self.server.applyChanges()

    def publish_transform(self, timer_event):
        time = rospy.Time.now()
#.........这里部分代码省略.........

# 需要导入模块: from interactive_markers.interactive_marker_server import InteractiveMarkerServer [as 别名]
# 或者: from interactive_markers.interactive_marker_server.InteractiveMarkerServer import insert [as 别名]
class PR2TrajectoryMarkers(object):
    """
    A class to create and store a  trajectory for one PR2 arm. The created
    trajectory can be published as a PoseArray message.

    This class published on the following topics:
    ~trajectory_markers are the main interactive markers.
    ~trajectory_poses a markerarray to display the trajectory.
    ~trajectory_poses a posesarray with the resulting pose

    The class subscribes to the topic ~overwrite_trajectory_
    to change the stored trajectory. This is useful to resume working on a 
    trajectory after re-starting the node. The message type is PoseArray.

    A std_srvs/Empty service named ~execute_trajectory is provided to 
    externally trigger the execution of the trajectory.

    Constructor:
    TrajectoryMarkers(whicharm = "left")
    or
    TrajectoryMarkers(whicharm = "right")
    """
    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)

    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", 
#.........这里部分代码省略.........

# 需要导入模块: from interactive_markers.interactive_marker_server import InteractiveMarkerServer [as 别名]
# 或者: from interactive_markers.interactive_marker_server.InteractiveMarkerServer import insert [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 insert [as 别名]
    control_rotate_y = makeInteractiveMarkerControl(
        interactive_marker, InteractiveMarkerControl.ROTATE_AXIS)
    control_rotate_z = makeInteractiveMarkerControl(
        interactive_marker, InteractiveMarkerControl.ROTATE_AXIS)
    control_sphere = makeInteractiveMarkerControl(
        interactive_marker, InteractiveMarkerControl.MOVE_ROTATE_3D)
    marker = Marker()
    marker.color.r = 0.2
    marker.color.g = 0.3
    marker.color.b = 0.7
    marker.color.a = 0.5

    marker.type = Marker.SPHERE
    marker.scale.x = 0.2
    marker.scale.y = 0.2
    marker.scale.z = 0.2
    control_sphere.markers.append(marker)

    setOrientation(1,1,0,0, control_slide_x)
    setOrientation(1,0,1,0, control_slide_y)
    setOrientation(1,0,0,1, control_slide_z)
    setOrientation(1,1,0,0, control_rotate_x)
    setOrientation(1,0,1,0, control_rotate_y)
    setOrientation(1,0,0,1, control_rotate_z)
    setOrientation(1,1,0,0, control_sphere)


    server.insert(interactive_marker, feedback)
    server.applyChanges()
    rospy.spin()

# 需要导入模块: from interactive_markers.interactive_marker_server import InteractiveMarkerServer [as 别名]
# 或者: from interactive_markers.interactive_marker_server.InteractiveMarkerServer import insert [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))