六、ROS小车闭环控制：实现过程中的一些经验总结（C语言微积分，读写文档）

--------------------------------------------------------------------闲白区----------------------------------------------------------------------------

继续围绕闭环谈谈遇到的一些问题和解决办法。由于我的工作还没做完，虽然不是很复杂，也暂时不能公开所有内容，希望对有类似问题的朋友有所帮助。

我个人理解工程应用方面的编程，就好像搭积木，你学习别人的搭建方法，临摹别人的积木块变成自己的，自己又一路积累过来。

当需要搭建新的东西时，打磨一下已有的积木块就可以搭建了。最重要的是理解、分解之后，积累到自己的知识库。时间久了，直觉也就有了。

工程应用或者实验室使用常用的功能有：

1、微积分

2、通讯

3、文档记录

4、滤波

这四个百度一下或者github上翻翻就有很多成熟的东西，我们这里针对自己编写的ROS程序，对1，3进行进行一下分析。

而通讯部分，ROS已经帮你解决了linux内部的问题，如果拉windows入伙，那就是另一个问题了，我的方式是采用socket进行，这里面涉及到粘包之类的复杂问题，windows有很多成熟的库，用起来比较简单，而linux我翻了几天没有特别好的简单模式可循，等我后续找到再说。滤波部分，无外乎均值，中值，高通，低通，卡尔曼，等等，其实也是C程序的通用问题，我以往都是百度+github上找，目前还没涉及到，也暂且搁置。

因此，本篇只说微积分和文档记录，其他有时间再说。其实，也不算是新鲜的，跟我在windows里面实现的大同小异，只是这里要结合ROS小车来谈。（为啥不能把字体改小了呢，莫名其妙？！）

--------------------------------------------------------------------正文分割线----------------------------------------------------------------------------

相关程序已经可有可无的开源了，附带实现了一下基本控制算法，参见下一篇（七）。

〇、准备工作

在roboware中增加“.h”头文件，将你的变量、函数在这里声明，可以节约很多时间。

在include中，右键点击你的节点名字，选择添加头文件，然后点击任意一个地方，命名你的头文件，在出现的下拉栏里面选择添加到库文件，roboware就帮你做好工作了，可以去你的cmakelist中看看到底发生了什么变化。

在你想使用这个头文件的时候，直接：

#include <你的项目名/你的头文件.h>

例如我的：#include <learning_com/Data_def.h>，不需要把include这个路径写出来

涉及到的步骤截图如下：

步骤1：

步骤2：

一、微积分的C语言实现

本部分主要参考：《计算方法（C语言版）》。

1、保证程序有循环，ROS的话，在某个callback里面做就行了，将数据循环写入一个足够大的数组，用一个for循环，每次调用的时候将每个位置的数据依次移位就行；

2、在随时更新数组的情况下，使用书中的中心微分法和梯形积分法的函数为：

/*********************************************************/
/* 此处为中心微分法，给定i为0即为x0，                         */
/* 则Storage[0]为f(x0),此方法为求f(x)在x1处的微分值;         */
/* 数组中第0位为最新数据！                                  */
/* 形参分别代表：微分时间t，默认为0，标志位                   */
/********************************************************/
double CentralDerivative(double time, int i, int signature) {double def = 0;if (signature == 0) {def = (Storage[i] - Storage[i + 2]) / (2 * time);} else if (signature == 1) {} return def;
}/***************************************************************/
/* 此处为梯形积分法，给定i为0即为x0，                               */
/* 则Storage[0]为f(x0),此方法为求f(x)在x0~x1处的积分值;            */
/* 数组中0位为最新数据，注意积分是累加的                             */
/* 形参分别代表：积分时间，默认为0，标志位                          */
/*************************************************************/
double TrapezoidIntegral(double time, int i, int signature) {double intergral = 0;if (signature == 0) {intergral = (Storage[i] + Storage[i + 1]) *(time * 0.5); /// f(x0)~f(x1)的积分,1/2=0.5} else if (signature == 1) {}return intergral;
}

这里需要注意的是：微分是对数组中的第二个数进行微分，因此实际应用中要注意数据的对齐，通常来说，如果频率很高，由于数据的是连续的，这个影响并不大；这里面的积分实际上就是梯形的面积，因此是有误差的，而且这个面积需要进行累加，才是最后的积分，累加操作相信你自己就能实现。

显然，这都是近似计算，是有一定误差的，数据变化越剧烈，这个误差就越大。或许还有更高级的手段，感兴趣的朋友可以继续研究。

二、轨迹的设定和分解

这里由于程序结构略微复杂一点，不能很好写出来，能看懂意思，自己实现就好。

移动小车的闭环控制，没有期望轨迹就如同盲人摸象。

这里就需要说说ROS小车的运动方式了。

我们知道，核心的控制实现是我们发给TWIST信息中的“/cmd_vel”，如何给定请参考第一篇：

pub_ = n_.advertise<geometry_msgs::Twist>("/cmd_vel", 10);

这里面要注意的是，/cmd_vel有如下控制量：

cmd_vel.linear.x//x方向的线速度，对于一般的轮式机器人运动学就是V。单位m/s
cmd_vel.linear.y//差速转向的机器人此项无效，只对全向机器人有效
cmd_vel.linear.z//地上跑的肯定不行了，这个z轴是朝天的
cmd_vel.angular.x//欧拉角中的roll角转角速度,单位rad/s
cmd_vel.angular.y//pitch角转角速度
cmd_vel.angular.z//yaw角转角速度，偏航角速度，对于一般的轮式机器人运动学就是Omega

对于轮式机器人，受其运动能力限制，最多只有x，y的线速度和yaw偏航角可控。

欧拉角的解释，请百度一下。

那么我们知道了，对于我目前的差速ROS小车来说，只要控制前进速度V和转向速度Omega就好（也就是cmd_vel.linear.x和cmd_vel.angular.z），这里面涉及一些移动机器人运动学，这里面没什么理论创新，大同小异。

那么怎么得到想要的轨迹呢。

这里以圆形为例，由于x=r*cos(t),y=r*sin(t),r为半径，t为变化的时间：

//这是一个在某处被循环调用的函数，假设更新频率为30hz，Robo_State_cur为存储机器人运动学信息的结构体，
//在头文件中声明和初始化，而v代表速度，w代表角速度//获取期望位姿函数
void get_desired_pose() {//给入每个时刻的期望位置，通过期望位置得到得到期望的速度// traj_counter/30*pi*2 代表1秒跑完一圆,r=1为圆的半径,t=60,60秒一圈double r = 1;double t = 60;Robo_State_cur.X_c = r * cos(traj_counter / 30 * 2 * pi / 2 / t);//机器人X方向的期望位置Robo_State_cur.Y_c = r * sin(traj_counter / 30 * 2 * pi / 2 / t);//机器人Y方向的期望位置TransStorage(4); //更新相关数组time_interval = 1/30;double X_d = 0;double Y_d = 0;X_d = CentralDerivative(time_interval, 0, 0);//调用微分函数，从位置得到速度Y_d = CentralDerivative(time_interval, 0, 1);Robo_State_cur.V_c =sqrt(X_d * X_d + Y_d * Y_d); //m/s，机器人X方向的期望速度Robo_State_cur.W_c = Robo_State_cur.V_c / r;  //rad/s，机器人yaw角的期望角速度，因为是圆形期望轨迹，v=wrdouble temp = Robo_State_cur.Theta_c;Robo_State_cur.Theta_c = temp + TrapezoidIntegral(time_interval, 0, 2); //rad，机器人yaw角的累积角速度TransStorage(5); //更新相关数组traj_counter++;
}

三、读文档和写文档

读文档可以得到我们想要的数据，作为参数输入，参考这个就可以，当然涉及到数据分段，那又复杂了，暂且不谈：

https://blog.csdn.net/stpeace/article/details/12404925

写文档的话，虽然有bag文件，但是这样并不方便，因此这里参考以下两个整合一下：

https://blog.csdn.net/snoopy_neu/article/details/80962217

https://blog.csdn.net/Start_From_Scratch/article/details/52337722

文件头写成当前系统时间的方式，保证每次写文档都是新的名字，还是结合第一篇文章进行比照，会比较好理解：

//文档相关头文件
#include <fstream>
#include <iostream>
#include <string>#include <sys/time.h>
#include <time.h>///其他的东西
void SubscribeAndPublish::callback(const acl_msgs::ViconState &Vi_input) {ros::Rate loop_rate(30);geometry_msgs::Twist output;///其他的程序//***************写文档开始********************************************//if (counter == 0) {time_t t = std::time(0);struct tm *now = std::localtime(&t);std::string file_name;std::string file_path;file_path = "//home//peterli//catkin_ws_com//data_history//";//你想存储的绝对路径// the name of bag file is better to be determined by the system timefile_name = int2string(now->tm_year + 1900) + '-' +int2string(now->tm_mon + 1) + '-' + int2string(now->tm_mday) +'-' + int2string(now->tm_hour) + '-' + int2string(now->tm_min) +'-' + int2string(now->tm_sec) + ".txt";file_name = file_path + file_name; // 这样就是路径+文件名in.open(file_name,std::ios::trunc); // ios::trunc表示在打开文件前将文件清空,由于是写入,文件不存在则创建time_file = 0;} else {if (n_.ok()) {ROS_INFO("FILE is writing line: %d ", counter);time_file = time_file + time_interval;in << std::fixed << std::setprecision(6) << time_file / 1000 << "\t"<< Robo_State_des.V_c << "\t" << Robo_State_des.W_c << "\t"<< Robo_State_cur.V_a << "\t" << Robo_State_cur.W_a << std::endl;
//这里面包括了保留六位小数和前后对齐的操作，这样直接导入matlab就可以很方便绘图分析等} else {ROS_INFO("FILE is closed ");in.close(); //关闭文件，实际上我发现这个并没起作用，不过并不影响……}}//***************写文档结束********************************************//counter++;pub_.publish(output);loop_rate.sleep();
}//写文档字符转化
std::string int2string(int value) {std::stringstream ss;ss << value;return ss.str();
}

四、小结

这里没有说很多C++的基础用法和概念，比如定义、声明，如何定义结构体，如何定义全局变量，如何定义宏变量等等，建议初学者去看鸡啄米大神的博客，百度一下就有了。

附录：

还是稍微说一下Linux中的结构体定义，这个我也查了一下才成功。跟Windows的略微有点区别，感觉更随意了。

在前面说的头文件中（当然，放在CPP的最上面也可以）这样直接定义加上初始化即可：

typedef struct _paras{
double a;
int b;
string c;
}paras;paras paras1 = {0,0,a};

在你的CPP中就可以直接使用了，这已经是全局变量。

后面还有很多工作要做，感觉有什么有价值和共性的问题，再分享一下，希望可以帮助别人少走点弯路。