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

前面第一篇基本搞出了闭环的样子，也搞懂了订阅和发布节点信息的方法。

第二和第三篇搞定了IMU信息的输入，那么，我们想要结合里程仪信息（目前是轮式的，我后面要做视觉的）和IMU信息，怎么实现闭环呢。

首先还是参考这个多线程的例子：

https://blog.csdn.net/cyliujc/article/details/78707583

因为按照第一篇的架构，多输入好像实现不了，那么就搞复杂一点吧。

基本的操作第一篇说的差不多了，很多ROS小车大同小异，不多说了。

这里先搞个异步的多接收，用多线程实现，也就是接收的频率可以不同。

下一篇再尝试一下同步接收方式，按照官网的说明，同步接收方式是所有消息全都收到了才一起输出。

上程序：

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

int counter;
const double pi = 3.141592653;
double imu_test;
class SubscribeAndPublish {
public:

  SubscribeAndPublish() {
    // Topic you want to publish 这里发布cmd_vel控制小车

    pub_ = n_.advertise("/cmd_vel", 10);

    // Topic you want to subscribe 这里订阅/odom和/imu/data 
   
    sub_odom = n_.subscribe("/odom", 10, &SubscribeAndPublish::callback, this);
    sub_imu = n_.subscribe("/imu/data",10,&SubscribeAndPublish::callback2, this);
  }

  void callback(const nav_msgs::Odometry &odom_input);
  void callback2(const sensor_msgs::Imu &imu_input);
  int node_ok() { return n_.ok(); }

private:
  ros::NodeHandle n_;
  ros::Publisher pub_;
  ros::Subscriber sub_odom;
  ros::Subscriber sub_imu;

}; // End of class SubscribeAndPublish

int main(int argc, char **argv) {
  // Initiate ROS
  ros::init(argc, argv, "subscribe_and_publish");

  // Create an object of class SubscribeAndPublish that will take care of
  // everything
  SubscribeAndPublish SAPObject;
  counter = 0;
  ros::MultiThreadedSpinner s(2);  //多线程
  ros::spin(s);

  return 0;
}

  void SubscribeAndPublish::callback(const nav_msgs::Odometry &odom_input) {
    ros::Rate loop_rate(30);
    geometry_msgs::Twist output;
    //.... do something with the odom_input and generate the output...
    // 这里应该是控制闭环
    //目前小车坐标系不明。
    output.angular.z = 0;
    output.linear.x = 0.1 * sin(counter*pi/50);
    output.linear.y = 0;

    ROS_INFO("odom test is: %lf ", (double)odom_input.twist.twist.linear.x);    
    pub_.publish(output);
    counter++;
    loop_rate.sleep();

  }

void SubscribeAndPublish::callback2(const sensor_msgs::Imu &imu_input)
{
  //ROS_INFO("imu test is: %lf /n", (double)imu_input.angular_velocity.x);
  ros::Rate loop_rate(30);
  ROS_INFO("imu test is: %lf ", imu_test);
  imu_test = (double)imu_input.header.stamp.Time::nsec/1000000;//把时间采回来看看丢了多少
  loop_rate.sleep();
}

 

这样就可以双线程进行信息采集，其中一个线程同时作为控制线程，与上面贴link略有区别，刚好作为对照，可以看出怎样用的可行的。编译方式参见第一篇。

 

两个线程可以使用全局变量作为数据交互的枢纽。

这里为了多线程使用了spin，不清楚spinOnce如何实现。

在每个callback函数内分别进行循环。

目前发现的问题是：

一、周期不准啊，我现在也不知道ROS到底可以精确到多少，感觉梦回VS的SetTimer，老爷车！两个callback观察输出，都不怎么精确，只是仗着有缓存没怎么丢自己的信息，数据相互传递的话，就看出数据丢了不少，可能ROS也就这样了。

二、我知道还有比较进阶的action之类的用法，追求实时性的话，可能只能选择进阶的方法了，不过这样就大大增加了工作量，毕竟上下位机一起调节，有点要命。看了几个教程，基本上说action还是单对单的，只是多了对单任务完成状态的各种反馈，是否可以加入多话题订阅呢，这是个没找到答案的问题。

三、还有一种强制消息同步的方法，可以参考这篇博文，其实就是多节点接收然后同步输出，适合特殊需求，参见：https://blog.csdn.net/start_from_scratch/article/details/52337689