Subscribing to navigation goal

Question

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?

---

Originally posted by amck77 on ROS Answers with karma: 13 on 2012-03-12

Post score: 0

---

Original comments

Comment by mjcarroll on 2012-03-13:
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.

Comment by amck77 on 2012-03-14:
I am trying to subscribe to the topic /move_base_simple/goal . I'm using the Turtlebot navigation stack.

Answer 1

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.

---

Originally posted by mjcarroll with karma: 6414 on 2012-03-14

This answer was ACCEPTED on the original site

Post score: 4