前面第一篇基本搞出了闭环的样子，也搞懂了订阅和发布节点信息的方法。
第二和第三篇搞定了IMU信息的输入，那么，我们想要结合里程仪信息（目前是轮式的，我后面要做视觉的）和IMU信息，怎么实现闭环呢。
首先还是参考这个多线程的例子：
https://blog.csdn.net/cyliujc/article/details/78707583
因为按照第一篇的架构，多输入好像实现不了，那么就搞复杂一点吧。
基本的操作第一篇说的差不多了，很多ROS小车大同小异，不多说了。
这里先搞个异步的多接收，用多线程实现，也就是接收的频率可以不同。
下一篇再尝试一下同步接收方式，按照官网的说明，同步接收方式是所有消息全都收到了才一起输出。
上程序：
#include "std_msgs/String.h"
#include <geometry_msgs/Twist.h>
#include <math.h>
#include <nav_msgs/Odometry.h>
#include <ros/ros.h>
#include <sstream>
#include <tf/transform_broadcaster.h>
#include <sensor_msgs/Imu.h>
#include <boost/thread.hpp>
int counter;
const double pi = 3.141592653;
double imu_test;
class SubscribeAndPublish {
public:
SubscribeAndPublish() {
// Topic you want to publish 这里发布cmd_vel控制小车
pub_ = n_.advertise<geometry_msgs::Twist>("/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