Python geometry_msgs.msg 模块，Quaternion() 实例源码

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 odom_add_offset(odom, offset):
    """
    Takes in two odometry messages and returns a new odometry message that has
    the two added together.

    WARN: Both messages should be in the same frame!
    """
    new_odom = copy.deepcopy(odom)
    new_odom.pose.pose.position.x += offset.pose.pose.position.x
    new_odom.pose.pose.position.y += offset.pose.pose.position.y
    quat = new_odom.pose.pose.orientation
    quat_offset = offset.pose.pose.orientation
    quat_arr = np.array([quat.x, quat.y, quat.z, quat.w])
    offset_arr = np.array([quat_offset.x, quat_offset.y, quat_offset.z, quat_offset.w])
    theta = tr.euler_from_quaternion(quat_arr, 'sxyz')[2]
    theta_offset = tr.euler_from_quaternion(offset_arr, 'sxyz')[2]
    new_odom.pose.pose.orientation = Quaternion(*tr.quaternion_from_euler(theta+theta_offset, 0, 0, 'szyx'))
    return new_odom

def fmt(obj, nest_level=0):
    """ Format any common object """
    if nest_level > 10:
        return ""
    if isinstance(obj, float):
        return "{0:.3f}".format(obj)
    if isinstance(obj, list):
        return "(" + ",".join(map(lambda x: fmt(x, nest_level + 1), obj)) + ")"
    if isinstance(obj, (numpy.ndarray, numpy.generic)):
        return fmt(obj.tolist(), nest_level + 1)
    if isinstance(obj, dict):
        pairs = map(lambda x, y: "(" + fmt(x) + "," + fmt(y, nest_level + 1) + ")", obj.items())
        return fmt(pairs)
    if isinstance(obj, Vector3):
        return "({},{},{})".format(fmt(obj.x), fmt(obj.y), fmt(obj.z))
    if isinstance(obj, Quaternion):
        return "({},{},{},{})".format(fmt(obj.x), fmt(obj.y), fmt(obj.z), fmt(obj.w))
    # print " obj " + str(obj) + " is of type " + str(type(obj))
    return str(obj)

def determine_hand_move_time(self, sidename, position, orientation, cur_transform):
        move_time = self.HAND_TRAJECTORY_TIME
        desired_q = msg_q_to_q(orientation)
        cur_q = msg_q_to_q(cur_transform.rotation)
        rotation_amount = quaternion_multiply(desired_q, conjugate_q(cur_q))
        rot_w = rotation_amount[3]
        if rot_w < 0: rot_w = -rot_w
        add_time = 0
        if rot_w < 0.85:
            add_time = self.HAND_ROTATION_TIME
        # if rot_w < 0.92
        #    add_time = self.HAND_ROTATION_TIME
        # if rot_w < 0.75:
        #    add_time += self.HAND_ROTATION_TIME
        if add_time > 0:
            rospy.loginfo('Doing significant {} hand rotation, '
                          'adding rotation wait of {}.'.format(sidename, add_time))
            move_time += add_time
        return move_time

    # Moves the hand center to an absolute world position (Vector3) and orientation (Quaternion).

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 _place(self):
        self.state = "place"
        rospy.loginfo("Placing...")

        place_pose = self.int_markers['place_pose']
        # It seems positions and orientations are randomly required to
        # be actual Point and Quaternion objects or lists/tuples. The
        # least coherent API ever seen.
        self.br.sendTransform(Point2list(place_pose.pose.position),
                              Quaternion2list(place_pose.pose.orientation),
                              rospy.Time.now(),
                              "place_pose",
                              self.robot.base)
        self.robot.place(place_pose)

        # For cubelets:
        place_pose.pose.position.z += 0.042
        # For YCB:
        # place_pose.pose.position.z += 0.026
        self.place_pose = place_pose

def callback_state(self, data):
        for idx, cube in enumerate(data.name):
            self.cubes_state.setdefault(cube, [0] * 3)
            pose = self.cubes_state[cube]
            cube_init = self.CubeMap[cube]["init"]
            pose[0] = data.pose[idx].position.x + cube_init[0]
            pose[1] = data.pose[idx].position.y + cube_init[1]
            pose[2] = data.pose[idx].position.z + cube_init[2]

    # def add_cube(self, name):
    #     p = PoseStamped()
    #     p.header.frame_id = ros_robot.get_planning_frame()
    #     p.header.stamp = rospy.Time.now()
    #
    #     # p.pose = self._arm.get_random_pose().pose
    #     p.pose.position.x = -0.18
    #     p.pose.position.y = 0
    #     p.pose.position.z = 0.046
    #
    #     q = quaternion_from_euler(0.0, 0.0, 0.0)
    #     p.pose.orientation = Quaternion(*q)
    #     ros_scene.add_box(name, p, (0.02, 0.02, 0.02))
    #
    # def remove_cube(self, name):
    #     ros_scene.remove_world_object(name)

def set_cube_pose(self, name, pose, orient=None):
        """
        :param name: cube name, a string
        :param pose: cube position, a list of three float, [x, y, z]
        :param orient: cube orientation, a list of three float, [ix, iy, iz]
        :return:
        """
        self.cubes_pose.link_name = "cubes::" + name
        p = self.cubes_pose.pose
        cube_init = self.CubeMap[name]["init"]
        p.position.x = pose[0] - cube_init[0]
        p.position.y = pose[1] - cube_init[1]
        p.position.z = pose[2] - cube_init[2]
        if orient is None:
            orient = [0, 0, 0]
        q = quaternion_from_euler(orient[0], orient[1], orient[2])
        p.orientation = Quaternion(*q)
        self.pose_pub.publish(self.cubes_pose)

def set_cube_pose(self, name, pose, orient=None):
        """
        :param name: cube name, a string
        :param pose: cube position, a list of three float, [x, y, z]
        :param orient: cube orientation, a list of three float, [ix, iy, iz]
        :return:
        """
        self.cubes_pose.link_name = "cubes::" + name
        p = self.cubes_pose.pose
        p.position.x = pose[0] + 0.18
        p.position.y = pose[1]
        p.position.z = pose[2] - 0.05
        if orient is None:
            orient = [0, 0, 0]
        q = quaternion_from_euler(orient[0], orient[1], orient[2])
        p.orientation = Quaternion(*q)
        self.pose_pub.publish(self.cubes_pose)


# create cube

def _add_table(self, name):
        p = PoseStamped()
        p.header.frame_id = self._robot.get_planning_frame()
        p.header.stamp = rospy.Time.now()

        p.pose.position.x = 0.2
        p.pose.position.y = 0.0
        p.pose.position.z = 0.1

        q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0))
        p.pose.orientation = Quaternion(*q)

        # Table size from ~/.gazebo/models/table/model.sdf, using the values
        # for the surface link.
        self._scene.add_box(name, p, (0.005, 0.005, 0.005))

        return p.pose

def _add_grasp_block_(self, name):
        p = PoseStamped()
        p.header.frame_id = self._robot.get_planning_frame()
        p.header.stamp = rospy.Time.now()

        # p.pose = self._arm.get_random_pose().pose
        p.pose.position.x = 0.021
        p.pose.position.y = 0.008
        p.pose.position.z = 0.2885

        q = quaternion_from_euler(0.0, 0.0, 0.0)
        p.pose.orientation = Quaternion(*q)

        # Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae,
        # using the measure tape tool from meshlab.
        # The box is the bounding box of the coke cylinder.
        # The values are taken from the cylinder base diameter and height.
        self._scene.add_box(name, p, (0.01, 0.01, 0.01))

        return p.pose

def dict_to_list_euler(pose_dict):

    """
    Convert a pose dictionary to a list in the order x, y, z,
    orientation x, orientation y, orientation z.
    """

    qtn = Quaternion()
    qtn.x = pose_dict['orientation'].x
    qtn.y = pose_dict['orientation'].y
    qtn.z = pose_dict['orientation'].z
    qtn.w = pose_dict['orientation'].w
    elr_x, elr_y, elr_z, = tf.transformations.euler_from_quaternion(
        [qtn.x,qtn.y,qtn.z,qtn.w])

    lst = []
    lst.append(pose_dict['position'].x)
    lst.append(pose_dict['position'].y)
    lst.append(pose_dict['position'].z)
    lst.append(elr_x)
    lst.append(elr_y)
    lst.append(elr_z)
    return lst

def _add_table(self, name):
        p = PoseStamped()
        p.header.frame_id = self._robot.get_planning_frame()
        p.header.stamp = rospy.Time.now()

        p.pose.position.x = 0.45
        p.pose.position.y = 0.0
        p.pose.position.z = 0.22

        q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0))
        p.pose.orientation = Quaternion(*q)

        # Table size from ~/.gazebo/models/table/model.sdf, using the values
        # for the surface link.
        self._scene.add_box(name, p, (0.5, 0.4, 0.02))

        return p.pose

def _add_grasp_block_(self, name):
        p = PoseStamped()
        p.header.frame_id = self._robot.get_planning_frame()
        p.header.stamp = rospy.Time.now()

        p.pose.position.x = 0.25
        p.pose.position.y = 0.05
        p.pose.position.z = 0.32

        q = quaternion_from_euler(0.0, 0.0, 0.0)
        p.pose.orientation = Quaternion(*q)

        # Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae,
        # using the measure tape tool from meshlab.
        # The box is the bounding box of the coke cylinder.
        # The values are taken from the cylinder base diameter and height.
        self._scene.add_box(name, p, (0.03, 0.03, 0.09))

        return p.pose

def test_transform(self):
        t = Transform(
            translation=Vector3(1, 2, 3),
            rotation=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(Transform, t_mat)

        np.testing.assert_allclose(msg.translation.x, t.translation.x)
        np.testing.assert_allclose(msg.translation.y, t.translation.y)
        np.testing.assert_allclose(msg.translation.z, t.translation.z)
        np.testing.assert_allclose(msg.rotation.x, t.rotation.x)
        np.testing.assert_allclose(msg.rotation.y, t.rotation.y)
        np.testing.assert_allclose(msg.rotation.z, t.rotation.z)
        np.testing.assert_allclose(msg.rotation.w, t.rotation.w)

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_transform(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 Transform(
            translation=Vector3(*trans),
            rotation=Quaternion(*quat)
        )
    else:
        res = np.empty(shape, dtype=np.object_)
        for idx in np.ndindex(shape):
            res[idx] = Transform(
                translation=Vector3(*transformations.translation_from_matrix(arr[idx])),
                rotation=Quaternion(*transformations.quaternion_from_matrix(arr[idx]))
            )

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 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 publish_line_segments(pub_ns, _arr1, _arr2, c='b', stamp=None, frame_id='camera', flip_rb=False, size=0.002): 
    """
    Publish point cloud on:
    pub_ns: Namespace on which the cloud will be published
    arr: numpy array (N x 3) for point cloud data
    c: Option 1: 'c', 'b', 'r' etc colors accepted by matplotlibs color
       Option 2: float ranging from 0 to 1 via matplotlib's jet colormap
       Option 3: numpy array (N x 3) with r,g,b vals ranging from 0 to 1
    s: supported only by matplotlib plotting
    alpha: supported only by matplotlib plotting
    """
    arr1, carr = copy_pointcloud_data(_arr1, c, flip_rb=flip_rb)
    arr2 = (deepcopy(_arr2)).reshape(-1,3)

    marker = vis_msg.Marker(type=vis_msg.Marker.LINE_LIST, ns=pub_ns, action=vis_msg.Marker.ADD)
    if not arr1.shape == arr2.shape: raise AssertionError    

    marker.header.frame_id = frame_id
    marker.header.stamp = stamp if stamp is not None else rospy.Time.now()
    marker.scale.x = size
    marker.scale.y = size
    marker.color.b = 1.0
    marker.color.a = 1.0
    marker.pose.position = geom_msg.Point(0,0,0)
    marker.pose.orientation = geom_msg.Quaternion(0,0,0,1)

    # Handle 3D data: [ndarray or list of ndarrays]
    arr12 = np.hstack([arr1, arr2])
    inds, = np.where(~np.isnan(arr12).any(axis=1))

    marker.points = []
    for j in inds: 
        marker.points.extend([geom_msg.Point(arr1[j,0], arr1[j,1], arr1[j,2]), 
                              geom_msg.Point(arr2[j,0], arr2[j,1], arr2[j,2])])

    # RGB (optionally alpha)
    marker.colors = [std_msg.ColorRGBA(carr[j,0], carr[j,1], carr[j,2], 1.0) 
                     for j in inds]

    marker.lifetime = rospy.Duration()
    _publish_marker(marker)

def publish_pose(pose, stamp=None, frame_id='camera'): 
    msg = geom_msg.PoseStamped();

    msg.header.frame_id = frame_id
    msg.header.stamp = stamp if stamp is not None else rospy.Time.now()    

    tvec = pose.tvec
    x,y,z,w = pose.quat.to_xyzw()
    msg.pose.position = geom_msg.Point(tvec[0],tvec[1],tvec[2])
    msg.pose.orientation = geom_msg.Quaternion(x,y,z,w)

    _publish_pose(msg)

def process_msg(msg, who):
    msg._type == 'nav_msgs/Odometry'

    global last_rear, last_front, last_yaw, last_front_t

    if who == 'rear':
        last_rear = get_position_from_odometry(msg)
    elif who == 'front' and last_rear is not None:
        cur_front = get_position_from_odometry(msg)
        last_yaw = get_yaw(cur_front, last_rear)

        twist_lin = np.dot(rotMatZ(-last_yaw), get_speed_from_odometry(msg))

        if last_front is not None:
            dt = msg.header.stamp.to_sec() - last_front_t
            speed = (cur_front - last_front)/dt
            speed = np.dot(rotMatZ(-last_yaw), speed)
            print '1', speed
            print '2', twist_lin
            print '3', np.sqrt((speed-twist_lin).dot(speed-twist_lin))

            odo = Odometry()
            odo.header.frame_id = '/base_link'
            odo.header.stamp = rospy.Time.now()
            speed_yaw = get_yaw([0,0,0], -speed) #[-speed[0],-speed[1]*100,0])
            speed_yaw_q = ros_tf.transformations.quaternion_from_euler(0, 0, speed_yaw)
            odo.pose.pose.orientation = Quaternion(*list(speed_yaw_q))
            #odo.twist.twist.linear = Point(x=speed[0], y=speed[1], z=speed[2])
            odo.twist.covariance = list(np.eye(6,6).reshape(1,-1).squeeze())
            pub = rospy.Publisher('odo_speed', Odometry, queue_size=1).publish(odo)

        #print last_yaw
        last_front = cur_front
        last_front_t = msg.header.stamp.to_sec()

        return twist_lin

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 _tuple_to_msg_transform(tf_t):
        """
        Converts a tf tuple into a geometry_msgs/Transform message

        :type tf_t: ((float, float, float), (float, float, float, float))
        :rtype: geometry_msgs.msg.Transform
        """
        transl = Vector3(*tf_t[0])
        rot = Quaternion(*tf_t[1])
        return Transform(transl, rot)

def from_dict(self, in_dict):
        """
        Sets values parsed from a given dictionary.

        :param in_dict: input dictionary.
        :type in_dict: dict[string, string|dict[string,float]]

        :rtype: None
        """
        self.eye_on_hand = in_dict['eye_on_hand']
        self.transformation = TransformStamped(
            child_frame_id=in_dict['tracking_base_frame'],
            transform=Transform(
                Vector3(in_dict['transformation']['x'],
                        in_dict['transformation']['y'],
                        in_dict['transformation']['z']),
                Quaternion(in_dict['transformation']['qx'],
                           in_dict['transformation']['qy'],
                           in_dict['transformation']['qz'],
                           in_dict['transformation']['qw'])
            )
        )
        if self.eye_on_hand:
            self.transformation.header.frame_id = in_dict['robot_effector_frame']
        else:
            self.transformation.header.frame_id = in_dict['robot_base_frame']

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 quat_to_angle(quat):
    rot = PyKDL.Rotation.Quaternion(quat.x, quat.y, quat.z, quat.w)
    return rot.GetRPY()[2]

def get_odom(self):
        # Get the current transform between the odom and base frames
        try:
            (trans, rot)  = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
            rospy.loginfo("TF Exception")
            return

        return (Point(*trans), quat_to_angle(Quaternion(*rot)))

def quat_to_angle(quat):
    rot = PyKDL.Rotation.Quaternion(quat.x, quat.y, quat.z, quat.w)
    return rot.GetRPY()[2]

def get_odom(self):
        # Get the current transform between the odom and base frames
        try:
            (trans, rot)  = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
            rospy.loginfo("TF Exception")
            return

        return (Point(*trans), quat_to_angle(Quaternion(*rot)))

def quat_to_angle(quat):
    rot = PyKDL.Rotation.Quaternion(quat.x, quat.y, quat.z, quat.w)
    return rot.GetRPY()[2]

def quat_to_angle(quat):
    rot = PyKDL.Rotation.Quaternion(quat.x, quat.y, quat.z, quat.w)
    return rot.GetRPY()[2]

def get_odom(self):
        # Get the current transform between the odom and base frames
        try:
            (trans, rot)  = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
            rospy.loginfo("TF Exception")
            return

        return (Point(*trans), quat_to_angle(Quaternion(*rot)))

def get_ros_quaternion(self, almath_quaternion):
        output = Quaternion()
        output.w = almath_quaternion.w
        output.x = almath_quaternion.x
        output.y = almath_quaternion.y
        output.z = almath_quaternion.z
        return output

def q_to_msg_q(v):
    return Quaternion(v[0], v[1], v[2], v[3])

def get_lean_points(self, angle):
        """ Return a set of trajectory points to accomplish a lean """
        point = SO3TrajectoryPointRosMessage()

        point.time = 1.0  # Just give it a second to get there
        rotate = quaternion_about_axis(radians(angle), [0, 0, -1])
        point.orientation = Quaternion(rotate[1], rotate[2], rotate[3], rotate[0])
        point.angular_velocity = Vector3(0, 0, 0)
        log('Lean to: {}'.format(point))

        points = []
        points.append(point)
        return points

def lean_body_to(self, angle, wait=True):
        """ Lean forward or back to a given angle """
        chest_position = self.zarj.transform.lookup_transform(
            'world', 'torso', rospy.Time()).transform

        log("Lean body to {} degrees".format(angle))
        msg = ChestTrajectoryRosMessage()
        msg.unique_id = self.zarj.uid

        msg.execution_mode = msg.OVERRIDE

        euler = euler_from_quaternion((chest_position.rotation.w,
                                       chest_position.rotation.x,
                                       chest_position.rotation.y,
                                       chest_position.rotation.z))

        point = SO3TrajectoryPointRosMessage()
        point.time = 1.0
        point.orientation = chest_position.rotation

        qua = quaternion_from_euler(euler[0], radians(angle), euler[2])
        point.orientation = Quaternion(qua[1], qua[2], qua[3], qua[0])
        point.angular_velocity = Vector3(0.0, 0.0, 0.0)

        msg.taskspace_trajectory_points = [point]

        self.chest_publisher.publish(msg)
        if wait:
            rospy.sleep(point.time + 0.2)

def turn_body(self, angle):
        """
            Turn the torso by a given angle

            Angles smaller than 110 or larger than 250 are unstable
        """
        log("Turn body {} degrees".format(angle))
        chest_position = self.zarj.transform.lookup_transform(
            'world', 'torso', rospy.Time()).transform

        msg = ChestTrajectoryRosMessage()
        msg.unique_id = self.zarj.uid

        msg.execution_mode = msg.OVERRIDE

        euler = euler_from_quaternion((chest_position.rotation.w,
                                       chest_position.rotation.x,
                                       chest_position.rotation.y,
                                       chest_position.rotation.z))

        point = SO3TrajectoryPointRosMessage()
        point.time = 1.0

        qua = quaternion_from_euler(euler[0] + radians(angle), euler[1],
                                    euler[2])
        point.orientation = Quaternion(qua[1], qua[2], qua[3], qua[0])
        point.angular_velocity = Vector3(0.0, 0.0, 0.0)

        msg.taskspace_trajectory_points = [point]

        self.chest_publisher.publish(msg)

def turn_body_to(self, angle, wait=True):
        """
            Turn the torso to a given angle as related to the pelvis

            Angles smaller than 110 or larger than 250 are unstable
        """
        log("Turn body to {} degrees".format(angle))
        chest_position = self.zarj.transform.lookup_transform(
            'pelvis', 'torso', rospy.Time()).transform

        msg = ChestTrajectoryRosMessage()
        msg.unique_id = self.zarj.uid

        msg.execution_mode = msg.OVERRIDE

        euler = euler_from_quaternion((chest_position.rotation.w,
                                       chest_position.rotation.x,
                                       chest_position.rotation.y,
                                       chest_position.rotation.z))

        qua = quaternion_from_euler(radians(-angle-180), euler[1], euler[2])

        pose = PoseStamped()
        pose.pose.orientation = Quaternion(qua[1], qua[2], qua[3], qua[0])
        pose.header.frame_id = 'pelvis'
        pose.header.stamp = rospy.Time()
        result = self.zarj.transform.tf_buffer.transform(pose, 'world')


        point = SO3TrajectoryPointRosMessage()
        point.time = 1.0
        point.orientation = result.pose.orientation
        point.angular_velocity = Vector3(0.0, 0.0, 0.0)

        msg.taskspace_trajectory_points = [point]

        self.chest_publisher.publish(msg)

        if wait:
            rospy.sleep(point.time + 0.1)

def update_camera_transform(self):
        t = self.tl.getLatestCommonTime("/root", "/camera_link")
        position, quaternion = self.tl.lookupTransform("/root",
                                                       "/camera_link",
                                                       t)
        position, quaternion = self.update_transformation(list(position), list(quaternion))

        #get ICP to find rotation and translation
        #multiply old position and orientation by rotation and translation
        #publish new pose to /update_camera_alignment
        self.update_transform.publish( Pose(Point(*position), Quaternion(*quaternion)))

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 orientation_from_quaternion(q):
    return Quaternion(*q)

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

def angle_to_quaternion(angle):
    """Convert an angle in radians into a quaternion _message_."""
    return Quaternion(*tf.transformations.quaternion_from_euler(0, 0, angle))

def numpy_to_odom(arr, frame_id=None):
    """
    Takes in a numpy array [x,y,theta] and a frame ID and converts it to an
    Odometry message. 

    WARN: May require a timestamp.
    """
    odom = Odometry()
    if frame_id is not None: odom.header.frame_id = frame_id
    odom.pose.pose.position.x = arr[0]
    odom.pose.pose.position.y = arr[1]
    odom.pose.pose.orientation = Quaternion(*tr.quaternion_from_euler(arr[2], 0, 0, 'szyx'))
    return odom

def get_odom(self):
        # Get the current transform between the odom and base frames
        try:
            (trans, rot)  = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
            rospy.loginfo("TF Exception")
            return

        return (Point(*trans), quat_to_angle(Quaternion(*rot)))

def get_odom(self):
        # Get the current transform between the odom and base frames
        try:
            (trans, rot)  = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
            rospy.loginfo("TF Exception")
            return

        return (Point(*trans), quat_to_angle(Quaternion(*rot)))

def get_odom_angle(self):
        # Get the current transform between the odom and base frames
        try:
            (trans, rot)  = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
            rospy.loginfo("TF Exception")
            return

        # Convert the rotation from a quaternion to an Euler angle
        return quat_to_angle(Quaternion(*rot))

def __init__(self):
        rospy.init_node("speech")
        rospy.on_shutdown(self.shutdown)

        self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)

        rospy.loginfo("Waiting for move_base action server...")

        # Wait up to 60 seconds for the action server to become available
        self.move_base.wait_for_server(rospy.Duration(60))    

        self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)
        nav_goal = MoveBaseGoal()
        nav_goal.target_pose.header.frame_id = 'base_link'
        nav_goal.target_pose.header.stamp = rospy.Time.now()
        q_angle = quaternion_from_euler(0, 0,0, axes='sxyz')
        q = Quaternion(*q_angle)
        nav_goal.target_pose.pose =Pose( Point(0.3,0,0),q)
        move_base_state = SimpleActionState('move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb, 
                                                exec_timeout=rospy.Duration(60.0),
                                                server_wait_timeout=rospy.Duration(60.0))


        smach_top=StateMachine(outcomes=['succeeded','aborted','preempted',"valid","invalid"])
        with smach_top:
            StateMachine.add("Wait_4_Statr", MonitorState("task_comming", xm_Task, self.start_cb),
                                                  transitions={'invalid':'NAV',
                                                               'valid':'Wait_4_Statr',
                                                               'preempted':'NAV'})

            StateMachine.add("NAV", move_base_state, transitions={"succeeded":"Wait_4_Stop","aborted":"NAV","preempted":"Wait_4_Stop"})
            StateMachine.add("Wait_4_Stop",MonitorState("task_comming", xm_Task, self.stop_cb),
                                                  transitions={'invalid':'',
                                                               'valid':'Wait_4_Stop',
                                                               'preempted':''})

            smach_top.execute()

def people_msg_cb(self,msg):
        self.people_msg=msg
        if self.people_msg.pos is not None:
            goal = MoveBaseGoal()
            goal.target_pose.header.frame_id = 'base_link'
            q_angle = quaternion_from_euler(0, 0,0)
            self.q=Quaternion(*q_angle)
            goal.target_pose.pose=(Pose(Point(self.people_msg.pos.x-0.5,self.people_msg.pos.y,self.people_msg.pos.z),self.q))
            self.move_base.send_goal(goal)
            rospy.spin()