Ask Your Question
0

How to get a robot position (x,y) in a map instead of Odometry in python?

asked 2020-01-05 20:54:32 -0600

Redhwan gravatar image

updated 2020-01-08 06:29:04 -0600

I'd like to record robot position (x,y) in a map instead of Odometry in python because my Odometry is not accurate, I asked yesterday about this issue here. So, I need to record them from the map.

when I am using this command in a terminal, I see a good accuracy

rosrun tf tf_echo /map /base_link

how to get information of this command in python

my code when I use the Odometry:

#!/usr/bin/env python
import rospy
from nav_msgs.msg import Odometry


class OdometryModifier:

  def __init__(self):
    self.sub = rospy.Subscriber("odom", Odometry, self.callback)
  def callback(self, data):
    x = data.pose.pose.position.x
    y = data.pose.pose.position.y
    with open("target.csv", "a") as text_file:
        text_file.write("\n%.3f" % (x))
        text_file.write("\t%.3f" % (y))

if __name__ == '__main__':
    rospy.init_node('move_turtlebot', anonymous=True)
    try:
        odom = OdometryModifier()
        rospy.spin()
    except KeyboardInterrupt:
        rospy.loginfo('Shutting down')

how to get only information print position, quaternion

Update1

I updated my question after @billy's answer

the issue not in using google, tf isn't familiar with me.

import rospy
from tf import TransformListener
from geometry_msgs.msg import PointStamped
# from nav_msgs.msg import OccupancyGrid

class myNode:

    def __init__(self):
        self.tf = TransformListener()
        self.sub = rospy.Subscriber("/tf", PointStamped, self.some_method)
        # rospy.Subscriber("/map", TransformStamped, )
        # ...

    def some_method(self, point_stamped):
        print point_stamped

        if self.tf.frameExists("/base_link") and self.tf.frameExists("/map"):
            t = self.tf.getLatestCommonTime("/base_link", "/map")
            position, quaternion = self.tf.lookupTransform("/base_link", "/map", t)
            print position, quaternion
if __name__ == '__main__':
    rospy.init_node('move_turtlebot', anonymous=True)
    try:
        odom = myNode()
        rospy.spin()
    except KeyboardInterrupt:
        rospy.loginfo('Shutting down')

the output print point_stamped

.....
....
....

transforms: 
  - 
    header: 
      seq: 0
      stamp: 
        secs: 1578296497
        nsecs: 454806328
      frame_id: "odom"
    child_frame_id: "base_link"
    transform: 
      translation: 
        x: -0.000629110541654
        y: 7.50070441544e-06
        z: 0.0
      rotation: 
        x: 0.0
        y: 0.0
        z: 0.0161632095368
        w: 0.999869366796
transforms: 
  - 
    header: 
      seq: 0
      stamp: 
        secs: 1578296498
        nsecs: 123375996
      frame_id: "map"
    child_frame_id: "odom"
    transform: 
      translation: 
        x: -0.0020397693567
        y: 0.00738271227423
        z: 0.0
      rotation: 
        x: 0.0
        y: 0.0
        z: -0.0116367688543
        w: 0.999932290513transforms: 
  - 
    header: 
      seq: 0
      stamp: 
        secs: 1578296497
        nsecs: 971281007
      frame_id: "/camera_link"
    child_frame_id: "/camera_depth_frame"
    transform: 
      translation: 
        x: 0.0
        y: -0.02
        z: 0.0
      rotation: 
        x: 0.0
        y: 0.0
        z: 0.0
        w: 1.0
      ......
       ....

Update2

The output print self.tf.allFramesAsString()

I see Frame odom exists with parent map.

Frame RIA_TOP_FRAME exists with parent RIA_BASE.
Frame RIA_BASE exists with parent base_link.
Frame base_link exists with parent odom.
Frame camera_link exists with parent base_link.
Frame imu_link exists with parent base_link.
Frame laser_link exists with parent base_link.
Frame camera_depth_frame exists with parent camera_link.
Frame camera_depth_optical_frame exists with parent camera_depth_frame.
Frame camera_rgb_frame exists with parent camera_link.
Frame camera_rgb_optical_frame exists with parent camera_rgb_frame.
Frame rear_left_wheel exists with parent base_link.
Frame rear_right_wheel exists with parent base_link.
Frame map_ned exists with parent map.
Frame odom_ned exists with parent odom.
Frame odom exists with parent map.
Frame base_link_frd exists with parent base_link.
Frame front_left_wheel exists with parent base_link.
Frame front_right_wheel ...
(more)
edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
1

answered 2020-01-08 06:53:25 -0600

Redhwan gravatar image

updated 2020-01-08 06:56:43 -0600

this code fix it, I got it here

the same output: rosrun tf tf_echo /map /base_link

#!/usr/bin/env python
import rospy
import tf

if __name__ == '__main__':
    # initialize node
    rospy.init_node('tf_listener')
    # print in console that the node is running
    rospy.loginfo('started listener node !')
    # create tf listener
    listener = tf.TransformListener()
    # set the node to run 1 time per second (1 hz)
    rate = rospy.Rate(1.0)
    # loop forever until roscore or this node is down
    while not rospy.is_shutdown():
        try:
            # listen to transform
            (trans,rot) = listener.lookupTransform('/map', '/base_link', rospy.Time(0))
            # print the transform
            rospy.loginfo('---------')
            rospy.loginfo('Translation: ' + str(trans))
            rospy.loginfo('Rotation: ' + str(rot))
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            continue
        # sleep to control the node frequency
        rate.sleep()
edit flag offensive delete link more

Comments

simple, thanks for updating

billy gravatar imagebilly ( 2020-01-08 10:00:39 -0600 )edit

Thank you for your help.

Redhwan gravatar imageRedhwan ( 2020-01-08 18:25:52 -0600 )edit
1

answered 2020-01-05 21:56:22 -0600

billy gravatar image

updated 2020-01-07 02:26:02 -0600

From this page: http://wiki.ros.org/tf/TfUsingPython

import rospy
from tf import TransformListener

class myNode:

    def __init__(self, *args):
        self.tf = TransformListener()
        rospy.Subscriber(...)
        ...

    def some_method(self):
        if self.tf.frameExists("/base_link") and self.tf.frameExists("/map"):
            t = self.tf.getLatestCommonTime("/base_link", "/map")
            position, quaternion = self.tf.lookupTransform("/base_link", "/map", t)
            print position, quaternion

This took about 23 seconds to find using google, most of that because I am slow on the keyboard. I encourage you to use google (unless you're in china in which case you cannot use google). It will be much faster than asking questions and waiting for answers.

EDIT AFTER ADDITIONAL QUESTION AND INFO ADDED TO OP:

OK, are you sitting down? That code doesn't work on my machine. But maybe it will on yours.

Since your frames names do not include '/', you should use base_link instead of /base_link in the python code. That change will make the if self.tf.frameExists("base_link") part of the conditional TRUE. Remove all the "/"s from the python and see if that fixes it.

On my machine (ROS JADE on 14.04) the self.tf.frameExists("map") will never be true. That function only works on the child_frame_id field on my machine.

Stick print self.tf.allFramesAsString() in your code in place of the print point_stamped and see what you get back out. What I get is:

Frame laser exists with parent base_link.
Frame base_link exists with parent odom.
Frame imu exists with parent base_link.
Frame odom exists with parent map.

You can see there is no Frame called "map" so that part of the conditional is never true. Looks like I gave you some bad advice when referring you to this code.

If you figure out how to get a conditional that is capable of referencing the frame_id instead of the child_frame_id, please write back. I have no need for this info but now I'm frustrated and simply want to know.

Back to your original question:

You specifically asked about doing this in python so I didn't say anything about the robot pose publisher node: http://wiki.ros.org/robot_pose_publisher. It will give you the data you want on a topic that you can subscribe to. I think it's part of the binary install but written in C++. But you can read the topic "robot_pose" in python and do what you want.

edit flag offensive delete link more

Comments

first of all, thank you so much @billy. I updated my question after your answer. I can't see any output even though I launched the map and robot. what is wrong?? please help me more.

Redhwan gravatar imageRedhwan ( 2020-01-05 23:28:16 -0600 )edit

I can check later what's required to get it working - but can you please let me know what steps you took to troubleshoot?

billy gravatar imagebilly ( 2020-01-06 18:47:15 -0600 )edit

Thank you @billy, I will wait and I will appreciate your help

Redhwan gravatar imageRedhwan ( 2020-01-06 22:47:20 -0600 )edit

updated my answer with new info and alternate method to use.

billy gravatar imagebilly ( 2020-01-07 02:27:41 -0600 )edit

Thank you so much from my bottom heart, some progress happened after your answer's update I will wrote them here after 2 or 3 hours.

Redhwan gravatar imageRedhwan ( 2020-01-07 02:57:24 -0600 )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: 2020-01-05 20:54:32 -0600

Seen: 99 times

Last updated: Jan 08