ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

Odometry to path rviz

asked 2015-05-20 10:09:11 -0600

Porti77 gravatar image

updated 2016-01-14 02:57:29 -0600

Hi! I have a node that publish a message nav_msgs/Odometry, and i want see the trajectory in rviz, i know that i need a nav_msgs/Path. Somebody know a node that do it?

I create a node

#!/usr/bin/env python
from __future__ import print_function
import rospy
from tf.transformations import quaternion_from_euler
from std_msgs.msg import String
from nav_msgs.msg import Odometry, Path
from geometry_msgs.msg import PoseWithCovarianceStamped, PoseStamped
from sensor_msgs.msg import Joy

import sys
import json
from math import sqrt
from collections import deque

import time

def callback(data):
        global xAnt
        global yAnt
        global cont

    #Is created the pose msg, its necessary do it each time because Python manages objects by reference, 
        #and does not make deep copies unless explicitly asked to do so.
        pose = PoseStamped()    

    #Set a atributes of the msg
        pose.header.frame_id = "odom"
        pose.pose.position.x = float(data.pose.pose.position.x)
        pose.pose.position.y = float(data.pose.pose.position.y)
        pose.pose.orientation.x = float(data.pose.pose.orientation.x)
        pose.pose.orientation.y = float(data.pose.pose.orientation.y)
        pose.pose.orientation.z = float(data.pose.pose.orientation.z)
        pose.pose.orientation.w = float(data.pose.pose.orientation.w)

    #To avoid repeating the values, it is found that the received values are differents
        if (xAnt != pose.pose.position.x and yAnt != pose.pose.position.y):
                #Set a atributes of the msg
                pose.header.seq = path.header.seq + 1
                pose.header.stamp = path.header.stamp
                #Published the msg


        rospy.loginfo("Valor del contador: %i" % cont)
                if cont>max_append:


    #Save the last position
        return path

if __name__ == '__main__':
        #Variable initialization
        global xAnt
        global yAnt
        global cont

        #Node and msg initialization

        #Rosparams that are set in the launch
        #max size of array pose msg from the path
        if not rospy.has_param("~max_list_append"):
                rospy.logwarn('The parameter max_list_append dont exists')
        max_append = rospy.set_param("~max_list_append",1000) 
        max_append = 1000
        if not (max_append > 0):
                rospy.logwarn('The parameter max_list_append not is correct')
        pub = rospy.Publisher('/path', Path, queue_size=1)

        path = Path() #creamos el mensaje path de tipo path 
        msg = Odometry()

        #Subscription to the topic
        msg = rospy.Subscriber('/odometry', Odometry, callback) 

        rate = rospy.Rate(30) # 30hz

                while not rospy.is_shutdown():
        except rospy.ROSInterruptException:

I fixed the bugs and now the code works succesfull. Regards

edit retag flag offensive close merge delete


Did you get this working I am having a similar issue

ngoldfarb gravatar image ngoldfarb  ( 2016-01-13 13:24:31 -0600 )edit

I updated the correct code above.

Porti77 gravatar image Porti77  ( 2016-01-14 03:23:24 -0600 )edit

My answer helped you?

Porti77 gravatar image Porti77  ( 2016-01-21 03:36:58 -0600 )edit

2 Answers

Sort by ยป oldest newest most voted

answered 2015-05-20 12:23:54 -0600

gvdhoorn gravatar image

You can probably use one of the packages in the answers to show robot trajectory in rviz real-time.

edit flag offensive delete link more

answered 2015-05-20 11:03:01 -0600

yohtm gravatar image

You can simply add the topic to Rviz and set the value of the keep parameter to 0. This will display all received odometry messages as arrows. You can tweak the position and angle tolerance to display more/less arrows.

More details on the Rviz Odometry page.

edit flag offensive delete link more


This option i know yet, but i want paint a trajectory as a line. Thaks

Porti77 gravatar image Porti77  ( 2015-05-20 11:08:29 -0600 )edit

Question Tools

1 follower


Asked: 2015-05-20 10:09:11 -0600

Seen: 6,148 times

Last updated: Jan 14 '16