（十三）ROS发布者和订阅者

参考http://wiki.ros.org/ROS/Tutorials/WritingPublisherSubscriber(c++)
本文实现发布者和订阅者，发布者发布信息，信息的内容是编号，
订阅者订阅发布者消息，并将解析出来的编号保存为文件．
直接上代码

１．发布者

新建发布者节点

#include "ros/ros.h"
#include "std_msgs/String.h"
#include <sstream>
int main(int argc, char **argv)
{
  ros::init(argc, argv, "pubmsg");
  ros::NodeHandle n;
  ros::Publisher chatter_pub = n.advertise<std_msgs::String>("frameindex", 1000);
  ros::Rate loop_rate(10);
  int count = 0;
  while (ros::ok())
  {
    std_msgs::String msg;
    std::stringstream ss;
    ss << "frame id" << count;
    msg.data = ss.str();
    ROS_INFO("%s", msg.data.c_str());
    chatter_pub.publish(msg);
    ros::spinOnce();
    loop_rate.sleep();
    ++count;
  }
  return 0;
}

２．订阅者

#include "ros/ros.h"
#include "std_msgs/String.h"
#include<iostream>
#include<fstream>
using namespace std;
ofstream ofile;
void chatterCallback(const std_msgs::String::ConstPtr& msg)
{
  string submsg=msg->data;
  string::size_type position =submsg.find('d');
  string subStr=submsg.substr(position+1);
  int index;
  stringstream ss;
  ss<<subStr;
  ss>>index;
  cout<<"index="<<index<<endl;
  ofstream ofile;
  ofile.open("/home/rootroot/index.txt");
  ofile<<subStr<<endl;
   ofile.close();
  if(position!=submsg.npos)
  {
      cout<<"find position"<<position<<endl;
  }else{
      cout<<"not find"<<endl;
  }
}

int main(int argc, char **argv)
{

  ros::init(argc, argv, "submsg");
  ros::NodeHandle n;

  ros::Subscriber sub = n.subscribe("frameindex", 1000, chatterCallback);
  ros::spin();

  return 0;
}