def get_orientation(self):
""" Get the orientation from the local_data and return a quaternion """
euler = mathutils.Euler((self.data['roll'],
self.data['pitch'],
self.data['yaw']))
quaternion = euler.to_quaternion()
return quaternion
def default(self, ci='unused'):
pose = PoseStamped()
pose.header = self.get_ros_header()
pose.pose.position.x = self.data['x']
pose.pose.position.y = self.data['y']
pose.pose.position.z = self.data['z']
euler = mathutils.Euler(
(self.data['roll'], self.data['pitch'], self.data['yaw']))
pose.pose.orientation = euler.to_quaternion()
self.publish(pose)
def get_orientation(self):
""" Get the orientation from the local_data
and return a ROS geometry_msgs.Quaternion
"""
ros_quat = Quaternion()
if 'orientation' in self.data:
mathutils_quat = self.data['orientation']
else:
euler = mathutils.Euler(
(self.data['roll'], self.data['pitch'], self.data['yaw']))
mathutils_quat = euler.to_quaternion()
ros_quat.x = mathutils_quat.x
ros_quat.y = mathutils_quat.y
ros_quat.z = mathutils_quat.z
ros_quat.w = mathutils_quat.w
return ros_quat
def default_action(self):
""" Main loop of the actuator.
Implements the component behaviour
"""
""" Move the robot. """
quaternion = mathutils.Quaternion(
(self.local_data['qw'], self.local_data['qx'],
self.local_data['qy'], self.local_data['qz']))
euler = quaternion.to_euler()
# New parent position
position = mathutils.Vector(
(self.local_data['x'], -self.local_data['y'],
-self.local_data['z']))
# New parent orientation
orientation = mathutils.Euler([euler.x, -euler.y, -euler.z])
self.robot_parent.force_pose(position, orientation.to_matrix())