Ask Your Question
0

Subscribing to navigation goal

asked 2012-03-12 05:50:20 -0500

amck77 gravatar image

updated 2012-03-14 23:44:27 -0500

Hello,

Could anyone point me to where I could learn more about subscribing to a navigation goal. I have done the tutorials for subscribing and publishing to topics, but I don't know how to get the data out of the more complicated navigation message, into a way that I can use.

Thanks for any help.

Thank you mjcarrol for your example it was very helpful. To clarify for the future, to subscribe to a topic and use its information, I need to find the message type. Then I must include in my case for C++, the header file this message type uses, so geometry_msgs/PosesStamped. There is one thing I'm unsure about and it is mainly to do with my knowledge of C++ I think. How do you know to use pose.position.x?

edit retag flag offensive close merge delete

Comments

Can you clarify which navigation message you are referring to? There are several different kinds of navigation, depending on what stacks/packages you are using.

mjcarroll gravatar image mjcarroll  ( 2012-03-13 18:26:55 -0500 )edit

I am trying to subscribe to the topic /move_base_simple/goal . I'm using the Turtlebot navigation stack.

amck77 gravatar image amck77  ( 2012-03-14 05:46:34 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
4

answered 2012-03-14 06:53:50 -0500

mjcarroll gravatar image

So, the topic /move_base_simple/goal should have the message type PoseStamped

Looking at the message description, we can see that each incoming message should have a header field, containing the sequence number, time stamp, and coordinate frame_id. Additionally, each incoming message should have a pose field, which describes the cartesian (x,y,z) position and the quaternion (x,y,z,w) orientation of the robot.

I'm going to give you some example code in Python, but this could just as easily be done in C++.

#!/usr/bin/env python
import roslib; roslib.load_manifest('YOUR_PACKAGE_NAME_HERE')
import rospy
import tf.transformations
from geometry_msgs.msg import PoseStamped

def callback(msg):
    rospy.loginfo("Received at goal message!")
    rospy.loginfo("Timestamp: " + str(msg.header.stamp))
    rospy.loginfo("frame_id: " + str(msg.header.frame_id))

    # Copying for simplicity
    position = msg.pose.position
    quat = msg.pose.orientation
    rospy.loginfo("Point Position: [ %f, %f, %f ]"%(position.x, position.y, position.z))
    rospy.loginfo("Quat Orientation: [ %f, %f, %f, %f]"%(quat.x, quat.y, quat.z, quat.w))

    # Also print Roll, Pitch, Yaw
    euler = tf.transformations.euler_from_quaternion([quat.x, quat.y, quat.z, quat.w])
    rospy.loginfo("Euler Angles: %s"%str(euler))  

def listener():
    rospy.init_node('goal_listener', anonymous=True)
    rospy.Subscriber("/move_base_simple/goal", PoseStamped, callback)
    rospy.spin()

if __name__ == '__main__':
    listener()

In this example, we create a node called goal_listener that subscribes to the /move_base_simple/goal topic. We can then create a callback function (called callback), which accepts the message as a parameter msg.

From here, it is as simple as reading the named fields from the message, and using that data in whatever way that you want in your application. My example shows reading all of the fields from the PoseStamped message.

Hope this helps.

edit flag offensive delete link more

Your Answer

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

Add Answer

Question Tools

2 followers

Stats

Asked: 2012-03-12 05:50:20 -0500

Seen: 3,728 times

Last updated: Mar 14 '12