Ask Your Question
1

python odometry

asked 2013-09-16 10:49:44 -0500

Massbuilder gravatar image

updated 2014-01-28 17:17:58 -0500

ngrennan gravatar image

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 flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
3

answered 2013-09-16 12:22:27 -0500

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

roslib.load_manifest('odom_publisher')

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

  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.stamp = rospy.Time.now()
      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:
          self.tf_br.sendTransform(pos, ori, msg.header.stamp, msg.child_frame_id, msg.header.frame_id)

      ...
edit flag offensive delete link more

Comments

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

Massbuilder gravatar imageMassbuilder ( 2013-09-16 13:00:22 -0500 )edit

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

Boris gravatar imageBoris ( 2013-09-16 13:29:41 -0500 )edit

Did you work out how to use this

burf2000 gravatar imageburf2000 ( 2017-01-16 06:51:38 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

3 followers

Stats

Asked: 2013-09-16 10:49:44 -0500

Seen: 8,250 times

Last updated: Sep 16 '13