Ask Your Question
0

I don't get paint a path msg [closed]

asked 2015-06-08 02:58:55 -0500

Porti77 gravatar image

updated 2015-06-08 06:12:42 -0500

I attemp paint a path msg in rviz, i have a odometry msg and I created a node to transform this in path msg, the msg that i get is the next:

---
header: 
  seq: 5
  stamp: 
    secs: 1431593506
    nsecs: 572904821
  frame_id: odom
poses: 
  - 
    header: 
      seq: 4
      stamp: 
        secs: 1431593506
        nsecs: 534717082
      frame_id: odom
    pose: 
      position: 
        x: 0.369169249199
        y: 0.27659386606
        z: 0.0
      orientation: 
        x: 0.0
        y: 0.0
        z: 0.0
        w: 1.0
  - 
    header: 
      seq: 4
      stamp: 
        secs: 1431593506
        nsecs: 534717082
      frame_id: odom
    pose: 
      position: 
        x: 0.369169249199
        y: 0.27659386606
        z: 0.0
      orientation: 
        x: 0.0
        y: 0.0
        z: 0.0
        w: 1.0
  - 
    header: 
      seq: 4
      stamp: 
        secs: 1431593506
        nsecs: 534717082
      frame_id: odom
    pose: 
      position: 
        x: 0.369169249199
        y: 0.27659386606
        z: 0.0
      orientation: 
        x: 0.0
        y: 0.0
        z: 0.0
        w: 1.0
  - 
    header: 
      seq: 4
      stamp: 
        secs: 1431593506
        nsecs: 534717082
      frame_id: odom
    pose: 
      position: 
        x: 0.369169249199
        y: 0.27659386606
        z: 0.0
      orientation: 
        x: 0.0
        y: 0.0
        z: 0.0
        w: 1.0
  - 
    header: 
      seq: 4
      stamp: 
        secs: 1431593506
        nsecs: 534717082
      frame_id: odom
    pose: 
      position: 
        x: 0.369169249199
        y: 0.27659386606
        z: 0.0
      orientation: 
        x: 0.0
        y: 0.0
        z: 0.0
        w: 1.0
---


 ![#!/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 json
from math import sqrt
from collections import deque


def callback(data):
    global xAnt
    global yAnt

    #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
        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
    xAnt=0.0
    yAnt=0.0

    #Node and msg initialization
    rospy.init_node('path_plotter', anonymous=True)
    pub = rospy.Publisher('/path/odometry', Path, queue_size=1)
    path = Path() #creamos el mensaje path de tipo path 
    msg = Odometry()

    #Subscription to the topic
    msg = rospy.Subscriber('/odometry ...
(more)
edit retag flag offensive reopen merge delete

Closed for the following reason the question is answered, right answer was accepted by Porti77
close date 2015-06-08 06:13:16.395167

Comments

1

it looks like all of your points have the same X and Y. Can you share the code for your node that creates the path from Odometry?

ahendrix gravatar imageahendrix ( 2015-06-08 03:04:35 -0500 )edit

2 Answers

Sort by ยป oldest newest most voted
2

answered 2015-06-08 04:07:31 -0500

ahendrix gravatar image

Python manages objects by reference, and does not make deep copies unless explicitly asked to do so.

Since you only create one PoseStamped object at the beginning of your program, you're modifying a single object and inserting multiple references to the same object into your list of poses.

Instead, you should create a new pose every time you want to append a pose to your path, or simply append the Pose object that is contained within the Odometry message.

edit flag offensive delete link more
0

answered 2015-06-08 05:13:09 -0500

Porti77 gravatar image

Thanks for the help of Austin, I did !! I can now see the path from a message odometry !! Regards and thsnks =)

edit flag offensive delete link more

Question Tools

1 follower

Stats

Asked: 2015-06-08 02:58:55 -0500

Seen: 550 times

Last updated: Jun 08 '15