Source code for morse.middleware.ros.orientation

import logging; logger = logging.getLogger("morse." + __name__)
import math
from geometry_msgs.msg import Quaternion
from morse.middleware.ros import ROSSubscriber, mathutils

[docs]class QuaternionReader(ROSSubscriber): """ Subscribe to a Quaternion topic and set roll,pitch,yaw local data. """ ros_class = Quaternion
[docs] def update(self, message): quaternion = mathutils.Quaternion((message.w, message.x, message.y, message.z)) euler = quaternion.to_euler()["roll"] = euler.x["pitch"] = euler.y["yaw"] = euler.z logger.debug("Set orientation to RPY (%.3f %.3f %.3f)" % \ tuple(math.degrees(a) for a in euler))