Ask Your Question
3

Odometry to path rviz

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

Porti77 gravatar image

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

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
                path.header.frame_id="odom"
                path.header.stamp=rospy.Time.now()
                pose.header.stamp = path.header.stamp
                path.poses.append(pose)
                #Published the msg

        cont=cont+1

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

        pub.publish(path)

    #Save the last position
        xAnt=pose.pose.orientation.x
        yAnt=pose.pose.position.y
        return path




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



        #Node and msg initialization
        rospy.init_node('path_plotter')


        #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')
                sys.exit()
        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

    try:
                while not rospy.is_shutdown():
                    #rospy.spin()
                    rate.sleep()
        except rospy.ROSInterruptException:
                pass

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

edit retag flag offensive close merge delete

Comments

Did you get this working I am having a similar issue

ngoldfarb gravatar imagengoldfarb ( 2016-01-13 13:24:31 -0500 )edit

I updated the correct code above.

Porti77 gravatar imagePorti77 ( 2016-01-14 03:23:24 -0500 )edit

My answer helped you?

Porti77 gravatar imagePorti77 ( 2016-01-21 03:36:58 -0500 )edit

2 Answers

Sort by ยป oldest newest most voted
0

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

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
0

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

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

Comments

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

Porti77 gravatar imagePorti77 ( 2015-05-20 11:08:29 -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

1 follower

Stats

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

Seen: 3,591 times

Last updated: Jan 14 '16