Subscribing to navigation goal

edit

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.