python odometry

maybe I should have reread the navigation tutorials before making that last post. What I really need is good example on how to publish the odometry in python given that I have already amount the robot has travelled in the x and y axis between the last publishing and the current publishing and the current angle the robot

edit retag close merge delete

Sort by » oldest newest most voted In general, odometry has to be published in fixed frame. So, you need to accumulate x, y and orientation (yaw).

Following is the stripped snippet from a working node. Please do appropriate modifications to suit your application needs.

Matrix P is a covariance matrix from EKF with [x, y, yaw] system state. Description of nav_msgs/Odometry may be helpful.

#!/usr/bin/env python
import numpy as np
import rospy
import roslib
import tf
import PyKDL as kdl

# Messages
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Point, Quaternion

class RosOdomPublisher:
gps_ekf_odom_pub = rospy.Publisher('/odom', Odometry)

publish_odom_tf = True

frame_id = '/odom'
child_frame_id = '/base_footprint'

# Covariance
P = np.mat(np.diag([0.0]*3))

def publish_odom(self):
msg = Odometry()
msg.header.frame_id = self.frame_id # i.e. '/odom'
msg.child_frame_id = self.child_frame_id # i.e. '/base_footprint'

msg.pose.pose.position = Point(self.x, self.y, self.z)
msg.pose.pose.orientation = Quaternion(*(kdl.Rotation.RPY(R, P, Y).GetQuaternion()))

p_cov = np.array([0.0]*36).reshape(6,6)

# position covariance
p_cov[0:2,0:2] = self.P[0:2,0:2]
# orientation covariance for Yaw
# x and Yaw
p_cov[5,0] = p_cov[0,5] = self.P[2,0]
# y and Yaw
p_cov[5,1] = p_cov[1,5] = self.P[2,1]
# Yaw and Yaw
p_cov[5,5] = self.P[2,2]

msg.pose.covariance = tuple(p_cov.ravel().tolist())

pos = (msg.pose.pose.position.x,
msg.pose.pose.position.y,
msg.pose.pose.position.z)

ori = (msg.pose.pose.orientation.x,
msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z,
msg.pose.pose.orientation.w)

# Publish odometry message
self.gps_ekf_odom_pub.publish(msg)

# Also publish tf if necessary
if self.publish_odom_tf:

...
more

I understand most of the code but the portion between position covariance comment and lining reading msg.pose.covariance=tuple(P_cov.rave1().tolist()) has me mystified what is it for

You can ignore this part. It simply converts covariance matrix for [x, y, yaw] to [x, y, z, roll, pitch, yaw].