Python tf.transformations.euler_from_quaternion() Examples

def on_pose_update(self, data):
        self._counter += 1
        # It's not yet time to send a localization message.
        if self._counter % self._modulo_to_send != 0:
            return
        loc = Location(data.pose.position.x, data.pose.position.y,
                       data.pose.position.z)
        quaternion = [
            data.pose.orientation.x, data.pose.orientation.y,
            data.pose.orientation.z, data.pose.orientation.w
        ]
        roll, pitch, yaw = euler_from_quaternion(quaternion)
        rotation = Rotation(np.degrees(pitch), np.degrees(yaw),
                            np.degrees(roll))
        timestamp = erdos.Timestamp(coordinates=[self._msg_cnt])
        pose = Pose(Transform(loc, rotation), self._forward_speed)
        self._logger.debug('@{}: NDT localization {}'.format(timestamp, pose))
        self._pose_stream.send(erdos.Message(timestamp, pose))
        self._pose_stream.send(erdos.WatermarkMessage(timestamp))
        self._msg_cnt += 1 

def visualRobotCallback(self, pose_stamped_msg):
        """
        Callback for ROS topic
        Get the new updated position of robot from camera
        :param pose_stamped_msg: (PoseStamped ROS message)
        """
        self.robot_pos[0] = pose_stamped_msg.pose.position.x
        self.robot_pos[1] = pose_stamped_msg.pose.position.y
        self.robot_yaw = euler_from_quaternion([pose_stamped_msg.pose.orientation.x, pose_stamped_msg.pose.orientation.y,
                                                pose_stamped_msg.pose.orientation.z, pose_stamped_msg.pose.orientation.w])[2]

        if NO_TARGET_MODE and self.target_pos_changed:
            # simulate the target's position update
            self.target_pos[0] = 0.0
            self.target_pos[1] = 0.0
            self.target_yaw = 0.0
            self.target_pos_changed = False 

def getOdometry(self, odom):
        self.position = odom.pose.pose.position
        orientation = odom.pose.pose.orientation
        orientation_list = [orientation.x, orientation.y, orientation.z, orientation.w]
        _, _, yaw = euler_from_quaternion(orientation_list)

        goal_angle = math.atan2(self.goal_y - self.position.y, self.goal_x - self.position.x)

        heading = goal_angle - yaw
        if heading > pi:
            heading -= 2 * pi

        elif heading < -pi:
            heading += 2 * pi

        self.heading = round(heading, 2) 

def getOdometry(self, odom):
        self.position = odom.pose.pose.position
        orientation = odom.pose.pose.orientation
        orientation_list = [orientation.x, orientation.y, orientation.z, orientation.w]
        _, _, yaw = euler_from_quaternion(orientation_list)

        goal_angle = math.atan2(self.goal_y - self.position.y, self.goal_x - self.position.x)

        heading = goal_angle - yaw
        if heading > pi:
            heading -= 2 * pi

        elif heading < -pi:
            heading += 2 * pi

        self.heading = round(heading, 2) 

def getOdometry(self, odom):
        self.position = odom.pose.pose.position
        orientation = odom.pose.pose.orientation
        orientation_list = [orientation.x, orientation.y, orientation.z, orientation.w]
        _, _, yaw = euler_from_quaternion(orientation_list)

        goal_angle = math.atan2(self.goal_y - self.position.y, self.goal_x - self.position.x)

        heading = goal_angle - yaw
        if heading > pi:
            heading -= 2 * pi

        elif heading < -pi:
            heading += 2 * pi

        self.heading = round(heading, 2) 

def getOdometry(self, odom):
        self.position = odom.pose.pose.position
        orientation = odom.pose.pose.orientation
        orientation_list = [orientation.x, orientation.y, orientation.z, orientation.w]
        _, _, yaw = euler_from_quaternion(orientation_list)

        goal_angle = math.atan2(self.goal_y - self.position.y, self.goal_x - self.position.x)

        heading = goal_angle - yaw
        if heading > pi:
            heading -= 2 * pi

        elif heading < -pi:
            heading += 2 * pi

        self.heading = round(heading, 2) 

def draw_box(self, marker, lifetime, color):
        """
        draw box from ros marker
        """
        box = carla.BoundingBox()
        box.location.x = marker.pose.position.x
        box.location.y = -marker.pose.position.y
        box.location.z = marker.pose.position.z
        box.extent.x = marker.scale.x / 2
        box.extent.y = marker.scale.y / 2
        box.extent.z = marker.scale.z / 2

        roll, pitch, yaw = euler_from_quaternion([
            marker.pose.orientation.x,
            marker.pose.orientation.y,
            marker.pose.orientation.z,
            marker.pose.orientation.w
        ])
        rotation = carla.Rotation()
        rotation.roll = math.degrees(roll)
        rotation.pitch = math.degrees(pitch)
        rotation.yaw = -math.degrees(yaw)
        rospy.loginfo("Draw Box {} (rotation: {}, color: {}, lifetime: {})".format(
            box, rotation, color, lifetime))
        self.debug.draw_box(box, rotation, thickness=0.1, color=color, life_time=lifetime) 

def on_goal(self, goal):
        """
        Callback for /move_base_simple/goal

        Receiving a goal (e.g. from RVIZ '2D Nav Goal') triggers a new route calculation.

        :return:
        """
        rospy.loginfo("Received goal, trigger rerouting...")
        carla_goal = carla.Transform()
        carla_goal.location.x = goal.pose.position.x
        carla_goal.location.y = -goal.pose.position.y
        carla_goal.location.z = goal.pose.position.z + 2  # 2m above ground
        quaternion = (
            goal.pose.orientation.x,
            goal.pose.orientation.y,
            goal.pose.orientation.z,
            goal.pose.orientation.w
        )
        _, _, yaw = euler_from_quaternion(quaternion)
        carla_goal.rotation.yaw = -math.degrees(yaw)

        self.goal = carla_goal
        self.reroute() 

def parse_imu_data(msg):
	# Get yaw angle.
	global tm_imu, psi

	tm_imu = msg.header.stamp.secs + 1e-9 * msg.header.stamp.nsecs
	ori = msg.orientation
	quat = (ori.x, ori.y, ori.z, ori.w)
	roll, pitch, yaw = euler_from_quaternion(quat)

	assert(-m.pi <= yaw)
	assert(m.pi >= yaw)

	psi = yaw + 0.5 * m.pi
	if psi > m.pi:
		psi = - (2*m.pi - psi)		

	# yaw in the Genesis OxTS coord system is wrt N = 0 (longitudinal axis of vehicle).
	# in the OxTS driver code, there is a minus sign for heading
	# (https://github.com/MPC-Car/GenesisAutoware/blob/master/ros/src/sensing/drivers/oxford_gps_eth/src/node.cpp#L10)
	# so heading is actually ccw radians from N = 0. 

def visualTargetCallback(self, pose_stamped_msg):
        """
        Callback for ROS topic
        Get the new updated position of robot from camera
        Only update when target position should have been moved (eg. reset env)
        :param pose_stamped_msg: (PoseStamped ROS message)
        """

        if self.target_pos_changed:
            if pose_stamped_msg.pose.position.x < TARGET_MAX_X and pose_stamped_msg.pose.position.x > TARGET_MIN_X  \
                    and pose_stamped_msg.pose.position.y > TARGET_MIN_Y and pose_stamped_msg.pose.position.y < TARGET_MAX_Y:
                self.target_pos[0] = pose_stamped_msg.pose.position.x
                self.target_pos[1] = pose_stamped_msg.pose.position.y
                self.target_yaw = euler_from_quaternion([pose_stamped_msg.pose.orientation.x, pose_stamped_msg.pose.orientation.y,
                                                         pose_stamped_msg.pose.orientation.z, pose_stamped_msg.pose.orientation.w])[2]
                self.target_pos_changed = False 

def goalCB(self, poseStamped):
        # reset timestamp because of bug: https://github.com/ros/geometry/issues/82
        poseStamped.header.stamp = rospy.Time(0)
        try:
            robotToTarget1 = self.listener.transformPose("/base_footprint", poseStamped)
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
            rospy.logerr("Error while transforming pose: %s", str(e))
            return
        quat = robotToTarget1.pose.orientation
        (roll,pitch,yaw) = euler_from_quaternion((quat.x, quat.y, quat.z, quat.w))
        self.motionProxy.moveTo(robotToTarget1.pose.position.x, robotToTarget1.pose.position.y, yaw) 

def _get_xyt(self, pose):
        """Processes the pose message to get (x,y,theta)"""
        orientation_list = [
            pose.pose.orientation.x,
            pose.pose.orientation.y,
            pose.pose.orientation.z,
            pose.pose.orientation.w,
        ]
        (roll, pitch, yaw) = euler_from_quaternion(orientation_list)
        goal_position = [pose.pose.position.x, pose.pose.position.y, yaw]
        return goal_position 

def get_slope_force(vehicle_info, vehicle_status):
    """
    Calculate the force of a carla vehicle faces when driving on a slope.

    :param vehicle_info: the vehicle info
    :type vehicle_info: carla_ros_bridge.CarlaEgoVehicleInfo
    :param vehicle_status: the ego vehicle status
    :type vehicle_status: carla_ros_bridge.CarlaEgoVehicleStatus
    :return: slope force [N, >0 uphill, <0 downhill]
    :rtype: float64
    """
    dummy_roll, pitch, dummy_yaw = euler_from_quaternion(
        [vehicle_status.orientation.x, vehicle_status.orientation.y,
         vehicle_status.orientation.z, vehicle_status.orientation.w])
    slope_force = get_acceleration_of_gravity(
        vehicle_info) * get_vehicle_mass(vehicle_info) * math.sin(-pitch)
    return slope_force 

def get_camera_transform(self):
        """
        Calculate the CARLA camera transform
        """
        if not self.pose:
            rospy.loginfo("no pose!")
            return None
        if self.pose.header.frame_id != self.role_name:
            rospy.logwarn("Unsupported frame received. Supported {}, received {}".format(
                self.role_name, self.pose.header.frame_id))
            return None
        sensor_location = carla.Location(x=self.pose.pose.position.x,
                                         y=-self.pose.pose.position.y,
                                         z=self.pose.pose.position.z)
        quaternion = (
            self.pose.pose.orientation.x,
            self.pose.pose.orientation.y,
            self.pose.pose.orientation.z,
            self.pose.pose.orientation.w
        )
        roll, pitch, yaw = euler_from_quaternion(quaternion)
        # rotate to CARLA
        sensor_rotation = carla.Rotation(pitch=math.degrees(roll)-90,
                                         roll=math.degrees(pitch),
                                         yaw=-math.degrees(yaw)-90)
        return carla.Transform(sensor_location, sensor_rotation) 

def update_alpha(self):
        _, _, quat = self.bot.arm.get_ee_pose("/base_link")
        (_, pitch, _) = euler_from_quaternion(quat)
        self.target_alpha = -pitch 

def odometry_updated(self, odo):
        """
        callback on new odometry
        """
        self._vehicle_location = odo.pose.pose.position
        quaternion = (
            odo.pose.pose.orientation.x,
            odo.pose.pose.orientation.y,
            odo.pose.pose.orientation.z,
            odo.pose.pose.orientation.w
        )
        _, _, self._vehicle_yaw = euler_from_quaternion(quaternion) 

def unpack_odom(self, msg):
        """
        Converts an Odometry message into a state vector.

        """
        q = [msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w]
        return np.array([msg.pose.pose.position.x, msg.pose.pose.position.y, trns.euler_from_quaternion(q)[2],
                         msg.twist.twist.linear.x, msg.twist.twist.linear.y, msg.twist.twist.angular.z]) 

def unpack_pose(self, msg):
        """
        Converts a Pose message into a state vector with zero velocity.

        """
        q = [msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w]
        return np.array([msg.position.x, msg.position.y, trns.euler_from_quaternion(q)[2], 0, 0, 0]) 

def __update_callback(self, msg):
        # Update the MVP Controller asynchronously
        if not self._in_velo_loop:
            # Stop the callback lagging behind
            return

        res = self.entropy_srv()
        if not res.success:
            # Something has gone wrong, 0 velocity.
            self.BAD_UPDATE = True
            self.curr_velo = Twist()
            return

        self.viewpoints = res.no_viewpoints

        # Calculate the required angular velocity to match the best grasp.
        q = tfh.quaternion_to_list(res.best_grasp.pose.orientation)
        curr_R = np.array(self.robot_state.O_T_EE).reshape((4, 4)).T
        cpq = tft.quaternion_from_matrix(curr_R)
        dq = tft.quaternion_multiply(q, tft.quaternion_conjugate(cpq))
        d_euler = tft.euler_from_quaternion(dq)
        res.velocity_cmd.angular.z = d_euler[2]

        self.best_grasp = res.best_grasp
        self.curr_velo = res.velocity_cmd

        tfh.publish_pose_as_transform(self.best_grasp.pose, 'panda_link0', 'G', 0.05) 

def get_euler_orientation(orientation):
    quaternion = (
        orientation.x,
        orientation.y,
        orientation.z,
        orientation.w
    )

    return euler_from_quaternion(quaternion) 

def get_kinematic_chain(self, target, source, reference):
        """
        Gets the pose for all joints in the kinematic chain from source to
        target.
        :param target: name of target joint
        :param source : name of source joint
        :param reference: name of reference joint as needed by ros function
                          listener.chain
        :type target: string
        :type source: string
        :type reference: string
        :return: kinematic chain, transforms as translation and quaternions,
                 transforms as matrices
        :rtype: list of strings, list of transforms, list of numpy matrices
        """
        # All frames here.
        listener = self.listener
        chain = listener.chain(target, rospy.Time(), source, rospy.Time(), reference)
        Ts, TMs = [], []

        np.set_printoptions(precision=4, suppress=True)
        TMcum = np.eye(4, 4)
        for i in range(len(chain) - 1):
            t = listener.lookupTransform(chain[i + 1], chain[i], rospy.Time())
            t = [[np.float64(_) for _ in t[0]], [np.float64(_) for _ in t[1]]]
            t1_euler = tfx.euler_from_quaternion(t[1])
            tm = tfx.compose_matrix(translate=t[0], angles=t1_euler)
            Ts.append(t)
            TMs.append(tm)

            t = listener.lookupTransform(chain[i + 1], chain[0], rospy.Time())
            t1_euler = tfx.euler_from_quaternion(t[1])
            tm = tfx.compose_matrix(translate=t[0], angles=t1_euler)
            TMcum = np.dot(TMs[i], TMcum)
            eye = np.dot(tm, np.linalg.inv(TMcum))
            assert np.allclose(eye - np.eye(4, 4), np.zeros((4, 4)), atol=0.1)

        # Sanity check to make sure we understand what is happening here.
        # for i in range(len(chain)-1):
        #   t = listener.lookupTransform(chain[i+1], chain[0], rospy.Time(0))
        #   tm = tfx.compose_matrix(translate=t[0], angles=t[1])
        #   TMcum = np.dot(TMs[i], TMcum)
        #   eye = np.dot(tm, np.linalg.inv(TMcum))
        #   print(eye-np.eye(4,4))
        # assert(np.allclose(eye-np.eye(4,4), np.zeros((4,4)), atol=1e-2))
        return chain, Ts, TMs