ROS控制M100笔记（四、实战发布和订阅话题C++代码编写）

这些基本都是成模块的，我将注释写在里面（http://wiki.ros.org/cn/ROS/Tutorials/WritingPublisherSubscriber%28c%2B%2B%29从这里面改编的）

//首先为了能够发布和订阅话题，我们必须#include "ros/ros.h"
#include "ros/ros.h"
//绝大部分发布消息的类型都需要声明，所以要包含相关的头文件，如我要输出字符串，就需要#include
#include "std_msgs/String.h"
//示例中没有使用<iostream.h>,it uses the new one,in this demo,they have the similar funtion.
#include <sstream>

//main function
int main(int argc, char **argv)
{
  //声明这个发布消息的节点名为“talker”,这个能够在rqt_graph上显示出来，（when running）
  ros::init(argc, argv, "talker");
  //声明节点，其名字为n。一般就是n or nh.
  ros::NodeHandle n;
  /* 以上基本上都是定式 */
  //stating that the topic name is “chatter”,whose type is std::String.
  //second param is stating the size of the message queue.
  //1000其实太大了，一般20就很够了
  ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);
  //这边用了一个ros延时函数，这样就能联合下面的loop_rate.sleep()控制发布的频率，这里设置为10hz
  ros::Rate loop_rate(10);
  int count = 0;//这个就开始声明变量了
  while (ros::ok())
  {
    std_msgs::String msg;
    std::stringstream ss;//变量声明
    ss << "hello world " << count;//输出
    msg.data = ss.str();//这边需要注意的是std_msg 下的大多数数据的格式都是data型的，
    //也可以在wiki.ros里查到
    ROS_INFO("%s", msg.data.c_str());//输出c_str()里的数据。
    //ros_info这函数能够在其它的一些窗口出现，以后自然会发现
    chatter_pub.publish(msg);//最核心的程序
    ros::spinOnce();//收集一次订阅的消息
    
    loop_rate.sleep();
    ++count;
  }
  return 0;
}

这里需要注意的是

ros::spinOnce()和ros::spin()这两个在消息订阅时起码要有一个，个人比较推荐前者，比较好进行一些控制。如果没有，那么消息就像是没有人拿的杂志一样，虽然发了出来，但是根本没有人读。/*此处调用ros::spinOnce()对于这个简单程序来说没啥必要，因为我们没有接收任何回调。然而，如果要在这个程序中添加订阅，但此处没有ros::spinOnce()的话，回调函数将永远不会被调用。所以还是加上吧。*/

可以发现，最重要的就是在确定好输出量后，利用节点代号（demo中为chatter_pub,一般发布消息的节点代号都会有一个_pub,Similarly, when we subscribe the topic, the name should be followed with "_sub"）.publish(输出量)。这里需要注意包含输出量的类型的头文件，声明输出量类型。

话题的订阅也很类似，在包含ros.h和输出变量后，除了已经定式的内容外，就需要声明订阅的量，不过这里还有一个回调函数需要声明。

代码如下：

#include "ros/ros.h"
#include "std_msgs/String.h"

void chatterCallback(const std_msgs::String::ConstPtr& msg)
{
  ROS_INFO("I heard: [%s]", msg->data.c_str());
}//回调函数

int main(int argc, char **argv)
{
  ros::init(argc, argv, "listener");//声明节点名为“listener”
  ros::NodeHandle n;
  ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);
  ros::spin();

  return 0;
}

回调函数的使用就是在sub里，每当ros::spin()/ros::spinOnce()调用时，都会subscribe 前面声明的内容。这样就会调用回调函数，此时就能得到数据。这里数据暂时存在& msg的地址里，所以届时需要存储数据到函数里的时候，只要声明变量，然后将&msg的内容赋值到变量里即可。回调函数能够在循环时也得到调用，应而得名。这里ros::spin()就是一个循环，其展开可以理解为为while(ros::ok){ros::spinOnce();}。

其中这个ROS_INFO（）和iostream中的printf()很像，但是个人觉得后者看得清楚，不过前面的信息更足。此外还有ROS_ERROR()这个函数，他会返回错误，在一些好一点的配色时会显示显眼的红色。而且还会在M100的飞控程序中出现错误信息，很有趣。