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

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

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

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

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

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

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

那么开始实现！

代码：

#include "std_msgs/String.h"
#include <geometry_msgs/Twist.h>
#include <math.h>
#include <nav_msgs/Odometry.h>
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <sstream>
#include <tf/transform_broadcaster.h>

#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/synchronizer.h>

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::Subscriber<nav_msgs::Odometry>odom_sub(nh, "/odom",1); //订阅odom
  message_filters::Subscriber<sensor_msgs::Imu>imu_sub(nh, "/imu/data",1); //订阅imu

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

  typedef message_filters::sync_policies::ApproximateTime<nav_msgs::Odometry,sensor_msgs::Imu> MySyncPolicy; //定义这个是为了后面少写一点容易看懂

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

  message_filters::Synchronizer<MySyncPolicy> 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，丢数丢的更多了！！

各有各的好？

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

猜你喜欢