Преобразование углов

Важная часть практического задания, это перевод углового положения робота из системы кватернионов в более понятные для движения в одной плоскости углы Эйлера.

Теоретическая часть этой задачи, довольно сложная и потребует знаний высшей математики, материалы для изучения находятся по ссылкам в конце страницы.

Мы же поступим очень просто, возьмем готовые функции и поверим, что они делают то что нам нужно.

Обычно, в двухмерной плоскости, угол положения робота называется theta (тэтта). Для того, чтобы перевести угол из квартериона в угол theta, можно воспользоваться готовой функцией на python:

import math

def quaternion_to_theta(odom):

    return(2*math.degrees(math.asin(odom.pose.pose.orientation.z)*math.copysign(1,odom.pose.pose.orientation.w)))

Если вам необходимы все углы Эйлера (углы крена, тангажа и рыскания), лучше воспользоваться встроенными функциями ROS, в модуле tf

from tf.transformations import *

# Преобразование из Эйлера в кватернион
quaternion = quaternion_from_euler(0, 0, 0)

# Преобразование из кватерниона в углы Эйлера
euler = euler_from_quaternion(quaternion)

Дополнительные материалы

1. https://ru.wikipedia.org/wiki/Кватернионы_и_вращение_пространства
2. https://habr.com/ru/post/426863/
3. https://www.youtube.com/watch?v=iFQ0Dm5M6Yg
4. Описание структуры данных Odometry

Пример программы Подписчика на данные реального робота: https://youtu.be/F0PeigWTwEU