Source code for morse.middleware.ros.odometry

import logging; logger = logging.getLogger("morse." + __name__)
import rospy
from geometry_msgs.msg import Vector3, Quaternion, Pose, Twist
from nav_msgs.msg import Odometry
from morse.middleware.ros import ROSPublisherTF, mathutils

[docs]class OdometryPublisher(ROSPublisherTF): """ Publish the odometry of the robot. And send the transformation between ``frame_id`` and ``child_frame_id`` args, default '/odom' and '/base_footprint' through TF. """ ros_class = Odometry default_frame_id = '/odom'
[docs] def initialize(self): ROSPublisherTF.initialize(self) # store the frame ids self.child_frame_id = self.kwargs.get("child_frame_id", "/base_footprint") logger.info("Initialized the ROS odometry sensor with frame_id '%s' " +\ "and child_frame_id '%s'", self.frame_id, self.child_frame_id)
[docs] def default(self, ci='unused'): odometry = Odometry() odometry.header = self.get_ros_header() odometry.child_frame_id = self.child_frame_id # fill pose and twist odometry.pose.pose = self.get_pose() odometry.twist.twist = self.get_twist() self.publish(odometry) # send current odometry transform self.sendTransform(self.get_position(), self.get_orientation(), odometry.header.stamp, odometry.child_frame_id, odometry.header.frame_id)
[docs] 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
[docs] def get_position(self): """ Get the position from the local_data and return a ROS Vector3 """ position = Vector3() position.x = self.data['x'] position.y = self.data['y'] position.z = self.data['z'] return position
[docs] def get_pose(self): """ Get the pose from the local_data and return a ROS Pose """ pose = Pose() pose.position = self.get_position() pose.orientation = self.get_orientation() return pose
[docs] def get_twist(self): """ Get the twist from the local_data and return a ROS Twist """ twist = Twist() twist.linear.x = self.data['vx'] twist.linear.y = self.data['vy'] twist.linear.z = self.data['vz'] twist.angular.x = self.data['wx'] twist.angular.y = self.data['wy'] twist.angular.z = self.data['wz'] return twist