Python transformations.euler_from_quaternion方法代码示例

# 需要导入模块: from tf import transformations [as 别名]
# 或者: from tf.transformations import euler_from_quaternion [as 别名]
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 

# 需要导入模块: from tf import transformations [as 别名]
# 或者: from tf.transformations import euler_from_quaternion [as 别名]
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 

# 需要导入模块: from tf import transformations [as 别名]
# 或者: from tf.transformations import euler_from_quaternion [as 别名]
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 

# 需要导入模块: from tf import transformations [as 别名]
# 或者: from tf.transformations import euler_from_quaternion [as 别名]
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. 

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

# 需要导入模块: from tf import transformations [as 别名]
# 或者: from tf.transformations import euler_from_quaternion [as 别名]
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) 

# 需要导入模块: from tf import transformations [as 别名]
# 或者: from tf.transformations import euler_from_quaternion [as 别名]
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) 

# 需要导入模块: from tf import transformations [as 别名]
# 或者: from tf.transformations import euler_from_quaternion [as 别名]
def get_euler_orientation(orientation):
    quaternion = (
        orientation.x,
        orientation.y,
        orientation.z,
        orientation.w
    )

    return euler_from_quaternion(quaternion) 

# 需要导入模块: from tf import transformations [as 别名]
# 或者: from tf.transformations import euler_from_quaternion [as 别名]
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) 

# 需要导入模块: from tf import transformations [as 别名]
# 或者: from tf.transformations import euler_from_quaternion [as 别名]
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]) 

# 需要导入模块: from tf import transformations [as 别名]
# 或者: from tf.transformations import euler_from_quaternion [as 别名]
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]) 

# 需要导入模块: from tf import transformations [as 别名]
# 或者: from tf.transformations import euler_from_quaternion [as 别名]
def update_alpha(self):
        _, _, quat = self.bot.arm.get_ee_pose("/base_link")
        (_, pitch, _) = euler_from_quaternion(quat)
        self.target_alpha = -pitch 

# 需要导入模块: from tf import transformations [as 别名]
# 或者: from tf.transformations import euler_from_quaternion [as 别名]
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 

# 需要导入模块: from tf import transformations [as 别名]
# 或者: from tf.transformations import euler_from_quaternion [as 别名]
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) 

# 需要导入模块: from tf import transformations [as 别名]
# 或者: from tf.transformations import euler_from_quaternion [as 别名]
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