Python InteractiveMarkerServer.applyChanges方法代码示例

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

    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 processFeedback(self, feedback):
        """
        :type feedback: InteractiveMarkerFeedback
        """
        s = "Feedback from marker '" + feedback.marker_name
        s += "' / control '" + feedback.control_name + "'"

        mp = ""
        if feedback.mouse_point_valid:
            mp = " at " + str(feedback.mouse_point.x)
            mp += ", " + str(feedback.mouse_point.y)
            mp += ", " + str(feedback.mouse_point.z)
            mp += " in frame " + feedback.header.frame_id

        if feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
            rospy.logdebug(s + ": button click" + mp + ".")

        elif feedback.event_type == InteractiveMarkerFeedback.MENU_SELECT:
            rospy.logdebug(s + ": menu item " + str(feedback.menu_entry_id) + " clicked" + mp + ".")
            if feedback.menu_entry_id == 1:
                rospy.logdebug("Start publishing transform...")
                if self.tf_br is None:
                    self.tf_br = tf.TransformBroadcaster()
            elif feedback.menu_entry_id == 2:
                rospy.logdebug("Stop publishing transform...")
                self.tf_br = None

            # When clicking this event triggers!
        elif feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
            rospy.logdebug(s + ": pose changed")

        elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_DOWN:
            rospy.logdebug(s + ": mouse down" + mp + ".")

        elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP:
            rospy.logdebug(s + ": mouse up" + mp + ".")

        # TODO: Print here the commands
        # tf static transform
        self.print_commands(feedback.pose)


        self.last_pose = deepcopy(feedback.pose)

        self.server.applyChanges()

    def print_commands(self, pose, decimals=4):
        p = pose.position
        o = pose.orientation

        print "\n---------------------------"
        print "Static transform publisher command (with roll pitch yaw):"
        common_part = "rosrun tf static_transform_publisher "
        pos_part = str(round(p.x, decimals)) + " " + str(round(p.y, decimals)) + " "+ str(round(p.z, decimals))
        r, p, y = euler_from_quaternion([o.x, o.y, o.z, o.w])
        ori_part = str(round(r, decimals)) + " " + str(round(p, decimals)) + " " + str(round(y, decimals))
        static_tf_cmd = common_part + pos_part + " " + ori_part + " " + self.from_frame + " " + self.to_frame + " 50"
        print "  " + static_tf_cmd
        print
        print "Static transform publisher command (with quaternion):"
        ori_q = str(round(o.x, decimals)) + " " + str(round(o.y, decimals)) + " " + str(round(o.z, decimals)) + " " + str(round(o.w, decimals))
        static_tf_cmd = common_part + pos_part + " " + ori_q + " " + self.from_frame + " " + self.to_frame + " 50"
        print "  " + static_tf_cmd
        print

        print "Roslaunch line of static transform publisher (rpy):"
        node_name = "from_" + self.from_frame + "_to_" + self.to_frame + "_static_tf"
        roslaunch_part = '  <node name="' + node_name + '" pkg="tf" type="static_transform_publisher" args="' +\
                         pos_part + " " + ori_part + " " + self.from_frame + " " + self.to_frame + " 50" + '" />'
        print roslaunch_part
        print
#.........这里部分代码省略.........

# 需要导入模块: from interactive_markers.interactive_marker_server import InteractiveMarkerServer [as 别名]
# 或者: from interactive_markers.interactive_marker_server.InteractiveMarkerServer import applyChanges [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 applyChanges [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 applyChanges [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 applyChanges [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()
        
        # Code for moving the robots joints
        switch_srv_name = 'pr2_controller_manager/switch_controller'
        rospy.loginfo('Waiting for switch controller service...')
        rospy.wait_for_service(switch_srv_name)
        self.switch_service_client = rospy.ServiceProxy(switch_srv_name,
                                                 SwitchController)
                                                 
        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_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()
        
        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()

    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.logerr('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.')
        joint_positions = self.ik.get_ik_for_ee(self.ee_pose)
        print 'joint: ' + str(joint_positions)
        if(joint_positions != None):
            self.toggle_arm(self.side_prefix, 'Freeze', False)
            #joint_positions = [-0.993560463247, -0.357041076593, 0.0534981848008, -1.24934444608, -3.10068096283, -1.7906747183, -28.4704855309]
            print 'returned from IK: ' + str(joint_positions)
            #print 'random position : ' + str([-0.993560463247, -0.357041076593, 0.0534981848008, -1.24934444608, -3.10068096283, -1.7906747183, -28.4704855309])
            #joint_positions = [-1.03129153, -0.35312086, -0.08, -1.25402906, -2.98718287, -1.7816027, 3.03965124]
            self.move_to_joints(self.side_prefix, list(joint_positions), 5.0)
        print 'done'
        
    def toggle_arm(self, side, toggle, button):
        controller_name = side + '_arm_controller'
        print 'toggle'
        start_controllers = []
        stop_controllers = []
        if (toggle == 'Relax'):
            stop_controllers.append(controller_name)
        else:
            start_controllers.append(controller_name)
        self.set_arm_mode(start_controllers, stop_controllers)

    def set_arm_mode(self, start_controllers, stop_controllers):
        try:
            self.switch_service_client(start_controllers, stop_controllers, 1)
        except rospy.ServiceException:
#.........这里部分代码省略.........

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

# 需要导入模块: from interactive_markers.interactive_marker_server import InteractiveMarkerServer [as 别名]
# 或者: from interactive_markers.interactive_marker_server.InteractiveMarkerServer import applyChanges [as 别名]
class World:
    '''Object recognition and localization related stuff'''

    tf_listener = None
    objects = []
    world = None
    segmentation_service = rospy.get_param("/pr2_pbd_interaction/tabletop_segmentation_service")

    def __init__(self):
        if World.tf_listener is None:
            World.tf_listener = TransformListener()
        self._lock = threading.Lock()
        self.surface = None
        self._tf_broadcaster = TransformBroadcaster()
        self._im_server = InteractiveMarkerServer('world_objects')
        self.clear_all_objects()
        self.relative_frame_threshold = 0.4
        # rospy.Subscriber("ar_pose_marker",
        #                  AlvarMarkers, self.receive_ar_markers)
        self.is_looking_for_markers = False
        self.marker_dims = Vector3(0.04, 0.04, 0.01)
        World.world = self

    @staticmethod
    def get_world():
        if World.world is None:
            World.world = World()
        return World.world

    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 receive_ar_markers(self, data):
        '''Callback function to receive marker info'''
        self._lock.acquire()
        if self.is_looking_for_markers:
            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, False, id=marker.id)
        self._lock.release()

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