Hi I'm trying to extract the point coordinates from a navigation local planner to be used in flight controller.

The topic I'm trying to subscribe to is the /local plan.

rostopic info /move_base/TebLocalPlannerROS/local_plan

-

   Type: nav_msgs/Path
    Publishers: 
     * /move_base (http://luengr-Precision-T1500:37491/)
     Subscribers: 
     * /rviz (http://luengr-Precision-T1500:41585/)

The messages within this I'd like to extract as variables are the xyz point position and orientation.

rosmsg show nav_msgs/Path

-

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
geometry_msgs/PoseStamped[] poses
  std_msgs/Header header
    uint32 seq
    time stamp
    string frame_id
  geometry_msgs/Pose pose
geometry_msgs/Point position
      float64 x
      float64 y
      float64 z
geometry_msgs/Quaternion orientation
      float64 x
      float64 y
      float64 z
      float64 w

I'm going to use Rosserial_mbed but I'm not sure about the structure of the rest of the .cpp from the tutorials. There's a neat tutorial for similar on the construct live #2 but this is with python and I believe it needs to be c++ so any advice or guidance would be greatly appreciated. Thanks.