废话：网上查了很多+看了一本ROS书，觉得很多知识都是在反复做基础工作或者wiki搬运，毕竟大家都是一边学一边弄，无可厚非，感受就是：为什么我想要的那么难找，no silver bullet。
ROS感觉上更适用于有一定编程基础的人作为得心应手的工具，拿来作为零基础的人学习或者思维训练的材料或者入门C++/Python/机器人控制有些不合适，毕竟为了通用性和复用性用法定死了。当然，很多时候我们就是想要个工具就是了。
好了，既然是工具，那我们做控制的就得做个闭环出来对吧。
正文：
一、准备工作
ROS小车当做移动机器人来看，自然就分机械部分（含电机），驱动器，控制器，主控电脑。
目前看到小车的底盘搭建有这样几种方式：
1、淘宝买；
2、stm32或其他做驱动板+Arduino或其他做控制器+树莓派等做主控（下位机）；
3、CAN驱动器+CAN主站设备（CAN卡等）+Ubuntu电脑。
如果自行搭建小车推荐这个博客：https://blog.csdn.net/forrest_z/article/category/6703051
有些小问题刚好当做课后题了。
由于我时间有限，选择了淘宝买这个路线，省了很多麻烦，因此底盘这里不多说了。
ROS的闭环当然逃不开ROS的消息机制，重点了解TOPIC的发布和订阅就好，一些topic的数据类型基本是固定的。还有一些小技巧如：
rostopic list //查看所有topic rostopic echo [topic] //可以显示在某个话题[topic]上发布的数据。
ROS系统的实时性我没查到确凿的数据，暂时不追求了，差不多就好。
参考了这个：https://answers.ros.org/question/216509/subscribe-and-publish-using-a-class/
也有朋友使用了还加了多线程，可以在本篇基础上进阶使用：https://blog.csdn.net/cyliujc/article/details/78707583
编程工具推荐Roboware，省去/剥夺了学习makelist等的体验，自动补完很好用，顺路熟悉下VScode为以后做准备。
二、闭环实现框架
我采用的是上位机ubuntu作为主控，下位机树莓派小车作为执行器的方式，因为我还要加一些复杂的东西进去，树莓派的算力肯定是不够的，树莓派拿来作为信息接收和执行的机构就好。
程序内容 SAPcontrol：
#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>
int counter;//测试用的计数器
const double pi = 3.141592653;//应该还有更优雅的方式
class SubscribeAndPublish {
public:
SubscribeAndPublish() {
// Topic you want to publish 这里发布cmd_vel控制小车，10代表了数据缓冲的量（次数）
pub_ = n_.advertise<geometry_msgs::Twist>("/cmd_vel", 10);
// Topic you want to subscribe 这里订阅odem ，wiki有详细的教程，10的含义同上
sub_ = n_.subscribe("/odom", 10, &SubscribeAndPublish::callback, this);
}
void callback(const nav_msgs::Odometry &input) {
geometry_msgs::Twist output;
//.... do something with the input and generate the output...
// 这里是控制闭环
//目前小车坐标系不明，复杂的控制应该要考虑的，需要使用的话，从input里面取出就好，使用rostopic echo /odom观察你要用的东西
output.angular.z = 0;//旋转备用
output.linear.x = 0.1 * sin(counter*pi/50);//向前
output.linear.y = 0;
ROS_INFO("test is: %lf", (double)input.twist.twist.linear.x);//强制转化了一下为了显示
//控制闭环结束
pub_.publish(output);
}
int node_ok() { return n_.ok(); }//放入public取代原本的ok（），因为外部不能直接调用private定义的节点
private:
ros::NodeHandle n_;
ros::Publisher pub_;
ros::Subscriber sub_;
}; // 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;
ros::Rate rate(30.0); // Hz，请跟底盘匹配
counter = 0;
while (SAPObject.node_ok()) {
/* code for loop body */
ros::spinOnce();//注意跟ros::spin()的区别，spin会程序只spin并执行发布/订阅操作不干别的，直到不ok才退出，不太灵活，适合作为消息发送的节点，once就如同字面。
rate.sleep();//补全循环周期满足上面的设定。
//以下为更进一步理解ROS的错误机制之后使用，我还不懂
/* try {
ros::spinOnce();
rate.sleep();
}
catch(错误)
{
// ROS_ERROR("%s", 错误.what());
ROS_INFO("Something is wrong!");
ros::Duration(0.5).sleep(); //出错就停0.5s
continue;
}
*/
counter ++;
}
return 0;
}
注意以上实在Roboware中做的，已经自动生成好cmakelist等等，适合懒人。
三、一些固定操作
1、编辑好你想要的闭环，必要的时候拿出C++的基础。然后找到[当前的ROS工作空间]之后catkin_make。
2、树莓派端，以我的为例：
source /opt/ros/kinetic/setup.bash export ROS_MASTER_URI=http://192.168.50.209:11311 //树莓派的IP export ROS_IP=192.168.50.209 //切换到你写好的launch目录，启动接收cmd_vel和发布odom的launch roslaunch 你的.launch
3、Ubuntu端，我的为例：
source catkin_ws_com/devel/setup.bash //前文程序所在的ROS工作空间 export ROS_MASTER_URI=http://192.168.50.209:11311//master只有一个 export ROS_IP=192.168.50.170 //这个写你的PC rosrun learning_com SAPcontrol // learning_com 是你的pkg ；SAPcontrol 是你的这个pkg中的控制节点也就是前文的程序，官方的ros——tutorial那个样子就对了 //当然如果东西都做好了，一起launch就好
4、如果没有意外，小车会开始前后加减速运动（正弦的感觉），更复杂的运动也是同理。
目前这个闭环，只能针对一个输入节点和一个输出节点，前面给了多线程的例子，后面第四篇我会用一下解决多输入节点单输出的节点的问题。
下一篇说一下MPU6050与树莓派的接法和用法。