十、ROS小车闭环控制：从txt文档读取复杂轨迹控制值、期望值指令

时间不多，说重点，以往基础不再赘述。

因为：

1、小车需要/cmd_vel 才能动。

所以，我们要从txt读出线速度和角速度。

2、小车需要全局的位置，转角期望值才能做闭环。

所以，我们要从txt读出位置，转角期望。

3、复杂轨迹不好在linux搞定

所以，需要读文档。提前用matlab等算好速度写入，注意根据时间与位置的匹配。

4、按照逐行读取，会因为有很多的空格、换行，实际处理起来很要命，可以百度一下，需要string转char然后再各种检查空格进行数字拼接（有小数点更要命）。

所以，干错交错写成一列！例如10行3列：

数据1

数据2

数据3

...

然后ROS逐行读取并依次写入：

所有数据[10][3]

 

基于以上便利性假设，给个简单例子，按照前面博文的最简闭环加入如何读取一个97行2列的txt数据存入数组

核心思路---逐行读取，逐行string转为double，然后依次给入提前设置好的数组：

#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 <iostream>
#include <fstream>
#include <sstream> //使用stringstream需要引入这个头文件

//从文档读取控制目标线速度v_c和角速度oemag_c
const int txt_control_data_row = 97;//行
const int txt_control_data_col = 2;//列  v_c，oemag_c
double txt_control_data[txt_control_data_row][txt_control_data_col];

int txt_counter;//测试用的计数器
// const double pi = 3.141592653;//应该还有更优雅的方式


using namespace std;

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



//文档读取核心函数思路：每行一个数据，判断空格小数点太麻烦
//逐行读取，逐行string转为double，然后依次给入提前设置好的数组
template <class Type>
Type stringToNum(const string& str)
{
	istringstream iss(str);
	Type num;
	iss >> num;
	return num;    
}

//控制指令txt，从文档读取控制目标线速度v_c（每行第1个）和角速度oemag_c（每行第2个）
int fileread_command() {
  ifstream fin;
  string line;
  fin.open("/home/peterli/catkin_ws_SMC_lidar/src/learning_com/src/"
           "command_v_omage.txt");

  int row = 0;
  int col = 0;

  if (fin) // 有该文件

  {
    while (getline(fin, line)) // line中不包括每行的换行符
    {
      //  cout << line << endl;
      //  cout << stringToNum<double>(line) << endl;
      //根据实际写文档的顺序1,2,1,2,1,2....交错存储
      if (row < 97) {
        if (col < 2) {
          txt_control_data[row][col] = stringToNum<double>(line);
        } else {
          //数据1,2写完一组，另起一行
          col = 0;
          row++;
          txt_control_data[row][col] = stringToNum<double>(line);
        }
        col++;
      }
    }
  } else // 没有该文件
  {
    cout << "no such file" << endl;
  }

  //测试写的顺序是否正确
  for (int i = 0; i < 97; i++) {

    cout << i << " " << txt_control_data[i][0] << " " << txt_control_data[i][1]
         << " " << endl;
  }

  return 0;
}

int main(int argc, char **argv) {
  // Initiate ROS
  ros::init(argc, argv, "readfiletest");
 
  // Create an object of class SubscribeAndPublish that will take care of
  // everything
  SubscribeAndPublish SAPObject;
  ros::Rate rate(30.0); 
  counter = 0;

 //执行读取的位置
 fileread_command();


  while (SAPObject.node_ok()) {
    /* code for loop body */
   ros::spinOnce();
    rate.sleep();

      counter ++;
  }
  return 0;
}

OK，读出来了97行2列的数据。这里只给了控制指令的例子，期望数值也同理，为了省事都可以利用matlab之类写成一列。

下一步就可以根据ROS的机制，作为控制信息，设计好控制频率作为控制指令发送。

关于逐行读取带空格的小数数据，我研究了一下感觉很费力就没有深入，有兴趣的朋友可以继续深入，就是在上文所示的逐行读入那里处理一下再写入数组就好了。

之前分享的开源程序实际上已经给出了按照时序给定x，y期望位置就可以计算速度和转角的程序实现。理论上还可以更加复杂的实现matlab与ros的沟通。我个人倾向于简单好用为主，现在的思路应该是对懒人的不错的办法。matlab直接插值真的方便。

希望对大家有帮助。参考了很多的关于C++读文档的博文，难以一一指出道谢。不过说实话这个话题80%的博文看的脑壳痛不知所云，希望这个不是那80%吧。

 

PS：

matlab写文档

% 写成一列，注意换行之后无空格，写多少种数据进去随意，便于理解修改就行
fid = fopen('command_v_omage.txt','wt'); %写的方式打开文件（若不存在，建立文件）；
for i = 1:len-2 %根据实际情况，v_c，omega_c为列或者行向量。
fprintf(fid,'%f \n%f \n',v_c(i),omega_c(i));  % 整数就%d
end
fclose(fid);  %关闭文件；