t265_rpy.py
Go to the documentation of this file.
1 #!/usr/bin/python
2 # -*- coding: utf-8 -*-
3 ## License: Apache 2.0. See LICENSE file in root directory.
4 ## Copyright(c) 2019 Intel Corporation. All Rights Reserved.
5 
6 #####################################################
7 ## librealsense T265 rpy example ##
8 #####################################################
9 
10 # First import the library
11 import pyrealsense2 as rs
12 import math as m
13 
14 # Declare RealSense pipeline, encapsulating the actual device and sensors
15 pipe = rs.pipeline()
16 
17 # Build config object and request pose data
18 cfg = rs.config()
19 cfg.enable_stream(rs.stream.pose)
20 
21 # Start streaming with requested config
22 pipe.start(cfg)
23 
24 try:
25  while (True):
26  # Wait for the next set of frames from the camera
27  frames = pipe.wait_for_frames()
28 
29  # Fetch pose frame
30  pose = frames.get_pose_frame()
31  if pose:
32  # Print some of the pose data to the terminal
33  data = pose.get_pose_data()
34 
35  # Euler angles from pose quaternion
36  # See also https://github.com/IntelRealSense/librealsense/issues/5178#issuecomment-549795232
37  # and https://github.com/IntelRealSense/librealsense/issues/5178#issuecomment-550217609
38 
39  w = data.rotation.w
40  x = -data.rotation.z
41  y = data.rotation.x
42  z = -data.rotation.y
43 
44  pitch = -m.asin(2.0 * (x*z - w*y)) * 180.0 / m.pi;
45  roll = m.atan2(2.0 * (w*x + y*z), w*w - x*x - y*y + z*z) * 180.0 / m.pi;
46  yaw = m.atan2(2.0 * (w*z + x*y), w*w + x*x - y*y - z*z) * 180.0 / m.pi;
47 
48  print("Frame #{}".format(pose.frame_number))
49  print("RPY [deg]: Roll: {0:.7f}, Pitch: {1:.7f}, Yaw: {2:.7f}".format(roll, pitch, yaw))
50 
51 
52 finally:
53  pipe.stop()
static std::string print(const transformation &tf)


librealsense2
Author(s): Sergey Dorodnicov , Doron Hirshberg , Mark Horn , Reagan Lopez , Itay Carpis
autogenerated on Mon May 3 2021 02:50:11