五、ROS小车闭环控制：多(同步)输入节点单输出节点的闭环控制架构

上一篇说了异步，我觉得适合懒人，全都放在上位机一起搞就行了。

同步可以参考这篇使用了松同步机制的文章：

https://blog.csdn.net/start_from_scratch/article/details/52337689

和这篇wiki：http://wiki.ros.org/message_filters

然后有趣的是，名字叫做message_filters，却不是我们常说的filter。

他起的作用是将多个输出进行统一缓存，然后等最后进来的信息到了一起按照当前的时间输出，这对精确度要求比较高的控制是很有好处的，不过恕我直言，ROS1这废柴的周期精确度（我还没深入研究，当前简单了解的是这样，轻喷），数据卡的再精确也就是心理安慰成分比较多。所以真的要用，松同步就好了。

要注意的是，进行的同步信息必须是带有时间戳的，也就是***Stamped或者本身信息类型就有这个属性的（当然你自己写的这个类型也要包含时间戳，一般在hearder里），不然会报错。

那么开始实现！

代码：

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

#include 
#include 
#include 

int counter;
const double pi = 3.141592653;
double imu_test;
//这里要特别注意，要加ConstPtr！！！
//odom_input变成了指针，不再是变量，使用的时候要注意了！
void callback(const nav_msgs::OdometryConstPtr& odom_input,
              const sensor_msgs::ImuConstPtr& imu_input) {
  //控制量同步进入
  //可以开始控制闭环了，讲输出结果给到全局变量，直接输出就好。
  

  ROS_INFO("odom test is: %lf ", (double)odom_input->twist.twist.linear.x);
  ROS_INFO("imu test is: %lf ", (double)imu_input->header.stamp.Time::nsec/1000000); //还是看看精确程度如何
  counter++;
}

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

  ros::init(argc, argv, "sync_control_node"); //对外起个好听的名字
  ros::NodeHandle nh;                         //内部代号土一点有福气
  ros::Publisher pub_;

  message_filters::Subscriberodom_sub(nh, "/odom",5); //订阅odom
  message_filters::Subscriberimu_sub(nh, "/imu/data",5); //订阅imu

  pub_ = nh.advertise("/cmd_vel", 10); //发布cmd_vel
  geometry_msgs::Twist output;

  typedef message_filters::sync_policies::ApproximateTime MySyncPolicy; //定义这个是为了后面少写一点容易看懂

 
  // ApproximateTime takes a queue size as its constructor argument, hence
  // MySyncPolicy(10)
  //这句是说后面那个参数的设置，我认为应该是说这个数值应该大于你的各个订阅的缓冲区之和，猜测而已。

  message_filters::Synchronizer sync(MySyncPolicy(10), odom_sub, imu_sub);
  sync.registerCallback(boost::bind(&callback, _1, _2)); //全同步采集，进入callback计算控制量counter
  
ros::Rate loop_rate(30);
  while (nh.ok()) {

    output.angular.z = 0;
    output.linear.x = 0.1 * sin(counter * pi / 50);
    output.linear.y = 0;
    pub_.publish(output);
    ros::spinOnce();
    loop_rate.sleep();
  }
  return 0;
}

 

容易犯错的地方加了注释，而且还有个地方，就是roboware里面，cmakelist.txt要手动加点东西，findpackage那里要加上message_filters

 

变成这个样子：

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  message_filters
)

然后跟前面的工程一样，果然还是差不多的效果，就是消息同步了，让人觉得很满足。

通过我观察输出来时，设置成30Hz，感觉不到30Hz，丢数丢的更多了！！通过数据看，实际差不多是20Hz进行一次输出，我觉得如果输入再多一些，说不定会更慢，毕竟是要等到到齐了才统一输入数据，因此如果提高消息发送节点的频率，应该是会使最终的整体频率上升，然而具体跟设定值差多少，似乎看运气。

各有各的好？