MAVROS Yaw 방향 제어 예제

저작권: 쿼드(QUAD) 드론연구소 https://smartstore.naver.com/maponarooo

geometry_msgs/PoseStamped 토픽을 통해 기체의 위치와 방향을 제어할때 pose.orientation은 quaternion 값을 지정해야 하므로 아래와 같이 방향을 지정할 수 있습니다.

참고로 MAVROS는 ENU(FLU) 좌표계를 사용 합니다.

#저작권: 쿼드(QUAD) 드론연구소 https://smartstore.naver.com/maponarooo
# scipy 패키지의 Rotation 라이브러리를 이용합니다. 설치: pip3 install scipy
from scipy.spatial.transform import Rotation

pose = PoseStamped()
# 시계 반대방향 90도 회전
rot = Rotation.from_euler('xyz', [0, 0, -90], degrees=True)

# Convert to quaternions and print
quat = rot.as_quat()

pose.pose.position.x = 0
pose.pose.position.y = 0
pose.pose.position.z = 2
pose.pose.orientation.x = quat[0]
pose.pose.orientation.y = quat[1]
pose.pose.orientation.z = quat[2]
pose.pose.orientation.w = quat[3]