--------------------------------------------------------------------闲白区----------------------------------------------------------------------------
继续围绕闭环谈谈遇到的一些问题和解决办法。由于我的工作还没做完，虽然不是很复杂，也暂时不能公开所有内容，希望对有类似问题的朋友有所帮助。
我个人理解工程应用方面的编程，就好像搭积木，你学习别人的搭建方法，临摹别人的积木块变成自己的，自己又一路积累过来。
当需要搭建新的东西时，打磨一下已有的积木块就可以搭建了。最重要的是理解、分解之后，积累到自己的知识库。时间久了，直觉也就有了。
工程应用或者实验室使用常用的功能有：
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=wr
double 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 time
file_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中就可以直接使用了，这已经是全局变量。
后面还有很多工作要做，感觉有什么有价值和共性的问题，再分享一下，希望可以帮助别人少走点弯路。