ros_plugin_demo
Demonstration of the ROS PlugInLib
 All Data Structures Namespaces Files Functions Variables Pages
pose_array_from_path.cpp
1 #include "ros/ros.h"
2 #include "nav_msgs/Path.h"
3 #include "geometry_msgs/PoseArray.h"
4 
5 ros::Publisher setPoseArrayPub;
6 
7 void getPathCallback(const nav_msgs::Path event)
8 {
9  geometry_msgs::PoseArray msg;
10 
11  msg.header.stamp = ros::Time::now();
12  msg.header.frame_id = event.header.frame_id;
13 
14  //for(const std::vector<geometry_msgs::PoseStamped>::iterator it=event.poses.begin(); it != event.poses.end(); it++) {
15  for(int i=0; i < event.poses.size(); i++) {
16  msg.poses.push_back(event.poses.at(i).pose);
17  }
18 
19  setPoseArrayPub.publish(msg);
20 }
21 
22 int main(int argc, char **argv)
23 {
24  ros::init(argc, argv, "pose_array_from_path");
25  ros::NodeHandle roshandle;
26 
27  ros::Subscriber getPathSub = roshandle.subscribe("/path", 10, &getPathCallback);
28  setPoseArrayPub = roshandle.advertise<geometry_msgs::PoseArray>("pose_array", 10);
29 
30  ros::spin();
31 
32  return 0;
33 }