五、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进行一次输出,我觉得如果输入再多一些,说不定会更慢,毕竟是要等到到齐了才统一输入数据,因此如果提高消息发送节点的频率,应该是会使最终的整体频率上升,然而具体跟设定值差多少,似乎看运气。

各有各的好?

 

你可能感兴趣的:(ROS小车闭环实现,ROS)