前面第一篇基本搞出了闭环的样子,也搞懂了订阅和发布节点信息的方法。
第二和第三篇搞定了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