2 #include "nav_msgs/Path.h" 
    3 #include "geometry_msgs/PoseArray.h" 
    5 ros::Publisher setPoseArrayPub;
 
    7 void getPathCallback(
const nav_msgs::Path event)
 
    9     geometry_msgs::PoseArray msg;
 
   11     msg.header.stamp = ros::Time::now();
 
   12     msg.header.frame_id = 
event.header.frame_id;
 
   15     for(
int i=0; i < 
event.poses.size(); i++) {
 
   16         msg.poses.push_back(event.poses.at(i).pose);
 
   19     setPoseArrayPub.publish(msg);
 
   22 int main(
int argc, 
char **argv)
 
   24     ros::init(argc, argv, 
"pose_array_from_path");
 
   25     ros::NodeHandle roshandle;
 
   27     ros::Subscriber getPathSub = roshandle.subscribe(
"/path", 10, &getPathCallback);
 
   28     setPoseArrayPub = roshandle.advertise<geometry_msgs::PoseArray>(
"pose_array", 10);