python类Pose()的实例源码

def ik_solve(limb, pos, orient):
    ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
    iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
    ikreq = SolvePositionIKRequest()

    hdr = Header(stamp=rospy.Time.now(), frame_id='base')
    poses = {
        str(limb): PoseStamped(header=hdr, pose=Pose(position=pos, orientation=orient))}

    ikreq.pose_stamp.append(poses[limb])
    try:
        rospy.wait_for_service(ns, 5.0)
        resp = iksvc(ikreq)
    except (rospy.ServiceException, rospy.ROSException), e:
        rospy.logerr("Service call failed: %s" % (e,))
        return 1

    if resp.isValid[0]:
        limb_joints = dict(zip(resp.joints[0].name, resp.joints[0].position))
        return limb_joints
    else:
        return Errors.RaiseNotReachable(pos)

def GetWayPoints(self,Data):
        filename = Data.data
        #clear all existing marks in rviz
        for marker in self.makerArray.markers:
            marker.action = Marker.DELETE
        self.makerArray_pub.publish(self.makerArray)

        # clear former lists
        self.waypoint_list.clear()
        self.marker_list[:] = []
        self.makerArray.markers[:] = []

        f = open(filename,'r')
        for line in f.readlines():
            j = line.split(",")
            current_wp_name     = str(j[0])        
            current_point       = Point(float(j[1]),float(j[2]),float(j[3]))
            current_quaternion  = Quaternion(float(j[4]),float(j[5]),float(j[6]),float(j[7]))

            self.waypoint_list[current_wp_name] = Pose(current_point,current_quaternion)

        f.close()

        self.create_markers()
        self.makerArray_pub.publish(self.makerArray)

def observationToPose(self, observation):
        pose    = geometry_msgs.msg.Pose()

        if observation['orientation'] == None:
            # if observation is ball
            observation['orientation']  = 0.0

        if self.do_side_invert == True:
            observation['x']    = -observation['x']
            observation['y']    = -observation['y']
            observation['orientation']    = observation['orientation'] + math.pi

        pose.position.x = observation['x'] / 1000
        pose.position.y = observation['y'] / 1000
        pose.position.z = observation['z'] / 1000

        quat = quaternion_from_euler(0.0, 0.0, observation['orientation'])
        pose.orientation.x = quat[0]
        pose.orientation.y = quat[1]
        pose.orientation.z = quat[2]
        pose.orientation.w = quat[3]

        return  pose

def spawn_model(model_name):
    """ Spawns a model in front of the RGBD camera.

        Args: None

        Returns: None
    """
    initial_pose = Pose()
    initial_pose.position.x = 0
    initial_pose.position.y = 1
    initial_pose.position.z = 1

    # Spawn the new model #
    model_path = rospkg.RosPack().get_path('sensor_stick')+'/models/'
    model_xml = ''

    with open (model_path + model_name + '/model.sdf', 'r') as xml_file:
        model_xml = xml_file.read().replace('\n', '')

    spawn_model_prox = rospy.ServiceProxy('gazebo/spawn_sdf_model', SpawnModel)
    spawn_model_prox('training_model', model_xml, '', initial_pose, 'world')

def __init__(self, image_path=None):
        height = int(rospy.get_param("~grid_height", 800))
        width = int(rospy.get_param("~grid_width", 800))
        resolution = rospy.get_param("~grid_resolution", .25)
        ogrid_topic = rospy.get_param("/lqrrt_node/ogrid_topic", "/ogrid")

        self.grid_drawer = DrawGrid(height, width, image_path)
        self.ogrid_pub = rospy.Publisher(ogrid_topic, OccupancyGrid, queue_size=1)

        m = MapMetaData()
        m.resolution = resolution
        m.width = width
        m.height = height
        pos = np.array([-width * resolution / 2, -height * resolution / 2, 0])
        quat = np.array([0, 0, 0, 1])
        m.origin = Pose()
        m.origin.position.x, m.origin.position.y = pos[:2]
        self.map_meta_data = m

        rospy.Timer(rospy.Duration(1), self.pub_grid)

def __init__(self, limb_name, topic='/sensor_values'):
        super(FingerSensorBaxter, self).__init__(limb_name)
        self.listen = tf.TransformListener()
        self.inside = np.zeros(14)
        self.tip = np.zeros(2)
        self.inside_offset = np.zeros_like(self.inside)
        self.tip_offset = np.zeros_like(self.tip)
        # Picking level
        self.level = None
        self.last_sensor_update = 0
        self.sensor_sub = rospy.Subscriber(topic,
                                           Int32MultiArray,
                                           self.update_sensor_values,
                                           queue_size=1)
        self.object_frame = ""
        self.camera_centroids = np.zeros((num_cubes, 3))
        self.touch_centroids = np.zeros((num_cubes, 3))
        self.current_index = 0
        self.update_transform = rospy.Publisher("/update_camera_alignment",
                                                Pose,
                                                queue_size = 1)
        #self.zero_sensor()

def update_translation(self):
        t = self.tl.getLatestCommonTime("/root", "/camera_link")
        position, quaternion = self.tl.lookupTransform("/root",
                                                       "/camera_link",
                                                       t)
        print("Cam Pos:")
        print(position)

        translation = self.touch_centroids[0] - self.camera_centroids[0]
        print("Translation: ")
        print(translation)
        position = position + translation
        print("New Cam Pos:")
        print(position)
        #print(self.touch_centroids)
        #print(self.camera_centroids)

        self.update_transform.publish( Pose(Point(*position), Quaternion(*quaternion)))

def __init__(self, limb_name, topic='/sensor_values'):
        super(FingerSensorBaxter, self).__init__(limb_name)
        self.listen = tf.TransformListener()
        self.inside = np.zeros(14)
        self.tip = np.zeros(2)
        self.inside_offset = np.zeros_like(self.inside)
        self.tip_offset = np.zeros_like(self.tip)
        # Picking level
        self.level = None
        self.last_sensor_update = 0
        self.sensor_sub = rospy.Subscriber(topic,
                                           Int32MultiArray,
                                           self.update_sensor_values,
                                           queue_size=1)
        self.object_frame = ""
        self.perc_centroids = np.array([])

        self.touch_centroids = np.array([])

        self.update_transform = rospy.Publisher("/update_camera_alignment",
                                                Pose,
                                                queue_size = 1)
        self.handle_found = False
        #self.zero_sensor()
        #import ipdb;ipdb.set_trace()

def position(self, target_position, trans, height):
        """
        Calculate simple position of the robot's arm.

        Args:
            target_position (Pose): Wanted coordinates of robot's tool
            trans: Calculated transformation
            height (float): Height offset, depends on the number of disks on the rod

        Returns:
            target_position (Pose): Modified coordinates and orientation of robot's tool
        """
        roll = -math.pi / 2
        pitch = 0
        yaw = -math.pi / 2
        quaternion = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
        target_position.orientation.x = quaternion[0]
        target_position.orientation.y = quaternion[1]
        target_position.orientation.z = quaternion[2]
        target_position.orientation.w = quaternion[3]
        target_position.position.x = trans[0]
        target_position.position.y = trans[1]
        target_position.position.z = trans[2] + height
        return target_position

def marker_from_circle(circle, index=0, linewidth=0.1, color=ColorRGBA(1, 0, 0, 1), z=0., lifetime=10.0):
    marker = Marker()
    marker.header = make_header("/map")

    marker.ns = "Markers_NS"
    marker.id = index
    marker.type = Marker.CYLINDER
    marker.action = 0 # action=0 add/modify object
    # marker.color.r = 1.0
    # marker.color.g = 0.0
    # marker.color.b = 0.0
    # marker.color.a = 0.4
    marker.color = color
    marker.lifetime = rospy.Duration.from_sec(lifetime)

    marker.pose = Pose()
    marker.pose.position.z = z
    marker.pose.position.x = circle.center[0]
    marker.pose.position.y = circle.center[1]

    marker.scale = Vector3(circle.radius*2.0, circle.radius*2.0, 0)
    return marker

def marker_from_point_radius(point, radius, index=0, linewidth=0.1, color=ColorRGBA(1, 0, 0, 1), z=0., lifetime=10.0):
    marker = Marker()
    marker.header = make_header("/map")

    marker.ns = "Speed_NS"
    marker.id = index
    marker.type = Marker.CYLINDER
    marker.action = 0 # action=0 add/modify object
    # marker.color.r = 1.0
    # marker.color.g = 0.0
    # marker.color.b = 0.0
    # marker.color.a = 0.4
    marker.color = color
    marker.lifetime = rospy.Duration.from_sec(lifetime)

    marker.pose = Pose()
    marker.pose.position.z = z
    marker.pose.position.x = point[0]
    marker.pose.position.y = point[1]

    marker.scale = Vector3(radius*2.0, radius*2.0, 0.001)
    return marker

def list_to_pose(pose_list):
    pose_msg = Pose()
    if len(pose_list) == 7:
        pose_msg.position.x = pose_list[0]
        pose_msg.position.y = pose_list[1]
        pose_msg.position.z = pose_list[2]
        pose_msg.orientation.x = pose_list[3]
        pose_msg.orientation.y = pose_list[4]
        pose_msg.orientation.z = pose_list[5]
        pose_msg.orientation.w = pose_list[6]
    elif len(pose_list) == 6: 
        pose_msg.position.x = pose_list[0]
        pose_msg.position.y = pose_list[1]
        pose_msg.position.z = pose_list[2]
        q = tf.transformations.quaternion_from_euler(pose_list[3], pose_list[4], pose_list[5])
        pose_msg.orientation.x = q[0]
        pose_msg.orientation.y = q[1]
        pose_msg.orientation.z = q[2]
        pose_msg.orientation.w = q[3]
    else:
        raise MoveItCommanderException("Expected either 6 or 7 elements in list: (x,y,z,r,p,y) or (x,y,z,qx,qy,qz,qw)")
    return pose_msg

def set_pose_target(self, pose, end_effector_link = ""):
        """ Set the pose of the end-effector, if one is available. The expected input is a Pose message, a PoseStamped message or a list of 6 floats:"""
        """ [x, y, z, rot_x, rot_y, rot_z] or a list of 7 floats [x, y, z, qx, qy, qz, qw] """
        if len(end_effector_link) > 0 or self.has_end_effector_link():
            ok = False
            if type(pose) is PoseStamped:
                old = self.get_pose_reference_frame()
                self.set_pose_reference_frame(pose.header.frame_id)
                ok = self._g.set_pose_target(conversions.pose_to_list(pose.pose), end_effector_link)
                self.set_pose_reference_frame(old)
            elif type(pose) is Pose:
                ok = self._g.set_pose_target(conversions.pose_to_list(pose), end_effector_link)
            else:
                ok = self._g.set_pose_target(pose, end_effector_link)
            if not ok:
                raise MoveItCommanderException("Unable to set target pose")
        else:
            raise MoveItCommanderException("There is no end effector to set the pose for")

def go(self, joints = None, wait = True):
        """ Set the target of the group and then move the group to the specified target """
        if type(joints) is bool:
            wait = joints
            joints = None

        elif type(joints) is JointState:
            self.set_joint_value_target(joints)

        elif type(joints) is Pose:
            self.set_pose_target(joints)

        elif not joints == None:
            try:
                self.set_joint_value_target(self.get_remembered_joint_values()[joints])
            except:
                self.set_joint_value_target(joints)
        if wait:
            return self._g.move()
        else:
            return self._g.async_move()

def plan(self, joints = None):
        """ Return a motion plan (a RobotTrajectory) to the set goal state (or specified by the joints argument) """
        if type(joints) is JointState:
            self.set_joint_value_target(joints)

        elif type(joints) is Pose:
            self.set_pose_target(joints)

        elif not joints == None:
            try:
                self.set_joint_value_target(self.get_remembered_joint_values()[joints])
            except:
                self.set_joint_value_target(joints)
        plan = RobotTrajectory()
        plan.deserialize(self._g.compute_plan())
        return plan

def _publish_grasps(self, grasps):
        """
        Publish grasps as poses, using a PoseArray message
        """

        if self._grasps_pub.get_num_connections() > 0:
            msg = PoseArray()
            msg.header.frame_id = self._robot.get_planning_frame()
            msg.header.stamp = rospy.Time.now()

            for grasp in grasps:
                p = grasp.grasp_pose.pose

                msg.poses.append(Pose(p.position, p.orientation))

            self._grasps_pub.publish(msg)

def test_pose(self):
        t = Pose(
            position=Point(1.0, 2.0, 3.0),
            orientation=Quaternion(*transformations.quaternion_from_euler(np.pi, 0, 0))
        )

        t_mat = ros_numpy.numpify(t)

        np.testing.assert_allclose(t_mat.dot([0, 0, 1, 1]), [1.0, 2.0, 2.0, 1.0])

        msg = ros_numpy.msgify(Pose, t_mat)

        np.testing.assert_allclose(msg.position.x, t.position.x)
        np.testing.assert_allclose(msg.position.y, t.position.y)
        np.testing.assert_allclose(msg.position.z, t.position.z)
        np.testing.assert_allclose(msg.orientation.x, t.orientation.x)
        np.testing.assert_allclose(msg.orientation.y, t.orientation.y)
        np.testing.assert_allclose(msg.orientation.z, t.orientation.z)
        np.testing.assert_allclose(msg.orientation.w, t.orientation.w)

def numpy_to_pose(arr):
    from tf import transformations

    shape, rest = arr.shape[:-2], arr.shape[-2:]
    assert rest == (4,4)

    if len(shape) == 0:
        trans = transformations.translation_from_matrix(arr)
        quat = transformations.quaternion_from_matrix(arr)

        return Pose(
            position=Vector3(*trans),
            orientation=Quaternion(*quat)
        )
    else:
        res = np.empty(shape, dtype=np.object_)
        for idx in np.ndindex(shape):
            res[idx] = Pose(
                position=Vector3(*transformations.translation_from_matrix(arr[idx])),
                orientation=Quaternion(*transformations.quaternion_from_matrix(arr[idx]))
            )

def spawn_sdf(gazebo_name, path_to_sdf, position, orientation, static):
    try:
        with open (path_to_sdf, "r") as f:
            xml=f.read().replace('\n', '')
        if static:
            root = ET.fromstring(xml) #parse the xml and retrieve the root element (sdf tag)*
            static_tag = ET.Element(tag='static')
            static_tag.text='true'
            root.find('model').append(static_tag) # find the model tag and insert a static child
            xml = ET.tostring(root) # get the new xml
            print(xml)

        GazeboObject.spawn_sdf_srv.wait_for_service()
        resp_spawn = GazeboObject.spawn_sdf_srv(gazebo_name, xml, "/", Pose(position=position, orientation=orientation), "world")
        if resp_spawn.success:
            rospy.loginfo("creation of "+ gazebo_name + " successful")
            return True
        else:
            rospy.logerr("Could not spawn "+path_to_sdf+" (from "+path_to_sdf+"), status : "+resp_spawn.status_message)
            return False
    except Exception as e:
        rospy.logerr("Could not spawn "+path_to_sdf+" (from "+path_to_sdf+")" )
        rospy.logerr(e)
        return False

def geom_pose_from_rt(rt): 
    msg = geom_msg.Pose()
    msg.position = geom_msg.Point(x=rt.tvec[0], y=rt.tvec[1], z=rt.tvec[2])
    xyzw = rt.quat.to_xyzw()
    msg.orientation = geom_msg.Quaternion(x=xyzw[0], y=xyzw[1], z=xyzw[2], w=xyzw[3])
    return msg

def GetWayPoints(self,Data):
        # dir = os.path.dirname(__file__)
        # filename = dir+'/locationPoint.txt'
        filename = Data.data
        rospy.loginfo(str(filename))
        rospy.loginfo(self.locations)
        self.locations.clear()
        rospy.loginfo(self.locations)

        f = open(filename,'r')

        for i in f.readlines():
            j = i.split(",")
            current_wp_name     = str(j[0])
            rospy.loginfo(current_wp_name)          

            current_point       = Point(float(j[1]),float(j[2]),float(j[3]))
            current_quaternion  = Quaternion(float(j[4]),float(j[5]),float(j[6]),float(j[7]))

            self.locations[current_wp_name] = Pose(current_point,current_quaternion)

        f.close()
        rospy.loginfo(self.locations)

        #Rviz Marker
        self.init_markers()

        self.IsWPListOK = True

def msgToPose(self, detection_pose):
        pose = Pose()

        pose.position.x = detection_pose.x / 1000
        pose.position.y = detection_pose.y / 1000

        if hasattr(detection_pose, 'orientation'):
            quat_tuple = quaternion_from_euler(0.0, 0.0, detection_pose.orientation)
            pose.orientation = Quaternion(*quat_tuple)

        return pose

def __init__(self, limb_name, topic='/sensor_values'):
        super(FingerSensorBaxter, self).__init__(limb_name)
        self.listen = tf.TransformListener()
        self.inside = np.zeros(14)
        self.tip = np.zeros(2)
        self.inside_offset = np.zeros_like(self.inside)
        self.tip_offset = np.zeros_like(self.tip)
        # Picking level
        self.level = None
        self.last_sensor_update = 0
        self.sensor_sub = rospy.Subscriber(topic,
                                           Int32MultiArray,
                                           self.update_sensor_values,
                                           queue_size=1)
        self.object_frame = ""
        self.perc_centroids = np.array([])

        self.touch_centroids = np.array([])

        self.update_transform = rospy.Publisher("/update_camera_alignment",
                                                Pose,
                                                queue_size = 1)
        self.handle_found = False

        self.handles_found = 0
        self.cups_scanned = 0
        #self.zero_sensor()
        #import ipdb;ipdb.set_trace()

def move_calib_position(self):
        # move arm to the calibration position
        self.calib_pose = PoseStamped(
            Header(0, rospy.Time(0), self.robot.base),
            Pose(Point(0.338520675898,-0.175860479474,0.0356775075197),
                 Quaternion(-0.0183493755758,0.708424150944, 0.704712092876, 0.0343413949013)))
        self.robot.move_ik(self.calib_pose)

def _pick(self):
        # State not modified until picking succeeds
        try:
            obj_to_get = int(self.character)
        except ValueError:
            rospy.logerr("Please provide a number in picking mode")
            return

        frame_name = "object_pose_{}".format(obj_to_get)

        rospy.loginfo("Picking object {}...".format(obj_to_get))
        if self.tl.frameExists(self.robot.base) and self.tl.frameExists(frame_name):
            t = self.tl.getLatestCommonTime(self.robot.base, frame_name)
            position, quaternion = self.tl.lookupTransform(self.robot.base,
                                                           frame_name,
                                                           t)
            print("position", position)
            print("quaternion", quaternion)

            position = list(position)
            # Vertical orientation
            self.br.sendTransform(position,
                                  [1, 0, 0, 0],
                                  rospy.Time.now(),
                                  "pick_pose",
                                  self.robot.base)
            # Ignore orientation from perception
            pose = Pose(Point(*position),
                        Quaternion(1, 0, 0, 0))
            h = Header()
            h.stamp = t
            h.frame_id = self.robot.base
            stamped_pose = PoseStamped(h, pose)
            self.robot.pick(stamped_pose)
            self.state = 'postpick'

def make_empty_pose():
    return Pose(
        Point(0, 0, 0),
        Quaternion(0, 0, 0, 1)
    )

def particle_to_pose(particle):
    ''' Converts a particle in the form [x, y, theta] into a Pose object '''
    pose = Pose()
    pose.position.x = particle[0]
    pose.position.y = particle[1]
    pose.orientation = angle_to_quaternion(particle[2])
    return pose

def _publish_grasps(self, grasps):
        """
        Publish grasps as poses, using a PoseArray message
        """
        if self._grasps_pub.get_num_connections() > 0:
            msg = PoseArray()
            msg.header.frame_id = self._robot.get_planning_frame()
            msg.header.stamp = rospy.Time.now()

            for grasp in grasps:
                p = grasp.grasp_pose.pose

                msg.poses.append(Pose(p.position, p.orientation))

            self._grasps_pub.publish(msg)

def __init__(self, max_random_start,
               observation_dims, data_format, display, use_cumulated_reward):

    self.max_random_start = max_random_start
    self.action_size = 28

    self.use_cumulated_reward = use_cumulated_reward
    self.display = display
    self.data_format = data_format
    self.observation_dims = observation_dims

    self.screen = np.zeros((self.observation_dims[0],self.observation_dims[1]), np.uint8)

    #self.dirToTarget = 0
    self.robotPose = Pose()
    self.goalPose = Pose()
    self.robotRot = 0.0
    self.prevDist = 0.0
    self.numWins = 0
    self.ep_reward = 0.0
    self.readyForNewData = True
    self.terminal = 0
    self.sendTerminal = 0
    self.minFrontDist = 6

    self.r = 1
    self.ang = 0

    rospy.wait_for_service('reset_positions')
    self.resetStage = rospy.ServiceProxy('reset_positions', EmptySrv)


    # Subscribers:
    rospy.Subscriber('/input_data', stage_message, self.stageCB, queue_size=10)

    # publishers:
    self.pub_vel_ = rospy.Publisher('cmd_vel', Twist, queue_size = 10)
    self.pub_rew_ = rospy.Publisher("lastreward",Float64,queue_size=10)
    self.pub_goal_ = rospy.Publisher("target", Pose, queue_size = 15)

def __init__(self, max_random_start,
               observation_dims, data_format, display, use_cumulated_reward):

    self.max_random_start = max_random_start
    self.action_size = 28

    self.use_cumulated_reward = use_cumulated_reward
    self.display = display
    self.data_format = data_format
    self.observation_dims = observation_dims

    self.screen = np.zeros((self.observation_dims[0],self.observation_dims[1]), np.uint8)

    #self.dirToTarget = 0
    self.robotPose = Pose()
    self.goalPose = Pose()
    self.robotRot = 0.0
    self.prevDist = 0.0
    self.numWins = 0
    self.ep_reward = 0.0
    self.readyForNewData = True
    self.terminal = 0
    self.sendTerminal = 0
    self.minFrontDist = 6

    rospy.wait_for_service('reset_positions')
    self.resetStage = rospy.ServiceProxy('reset_positions', EmptySrv)


    # Subscribers:
    rospy.Subscriber('/input_data', stage_message, self.stageCB, queue_size=10)

    # publishers:
    self.pub_vel_ = rospy.Publisher('cmd_vel', Twist, queue_size = 10)
    self.pub_rew_ = rospy.Publisher("lastreward",Float64,queue_size=10)
    self.pub_goal_ = rospy.Publisher("target", Pose, queue_size = 15)