ROS编程 csv文件读取/输出 与 YAML文件生成

前言

代码是抄的, 感想是真的. cpp的文件操作对比起python来说是比较繁复的. 而每次学习cpp的时候都会去刻意留文件操作的教程. 其实直接做一次实验就可以理解了. 下面的代码是抄回来， 分别是做手眼标定与力传感器标定过程中需要将采集的数据输出与读取过程中遇到的文件操作情况，记录下来防止自己忘记.

代码

1. csv文件生成. 代码场景 手眼标定 记录回传数据 生成csv文件

#include <ros/ros.h>
#include <iostream>
#include <fstream>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <sensor_msgs/JointState.h>
#include <geometry_msgs/PoseStamped.h>

using namespace sensor_msgs;
using namespace message_filters;
using namespace geometry_msgs;


void callback(const sensor_msgs::JointState::ConstPtr& msgs , const geometry_msgs::PoseStamped::ConstPtr& marker_msgs, std::fstream& fout_ref){
    //do something
    fout_ref << msgs->position[0] << "," << msgs->position[1] << ","
             << msgs->position[2] << "," << msgs->position[3] << ","
             << msgs->position[4] << "," << msgs->position[5] << ","
             << msgs->position[6] << "," << marker_msgs->pose.position.x << ","
             << marker_msgs->pose.position.y << "," << marker_msgs->pose.position.z << ","
             << marker_msgs->pose.orientation.x << "," << marker_msgs->pose.orientation.y << ","
             << marker_msgs->pose.orientation.z << "," << marker_msgs->pose.orientation.w << "\n";
};

int main(int argc, char* argv[]){
    ros::init(argc,argv,"receiver_node");
    ros::NodeHandle nh;

    // 创建文件
    std::fstream fout;
    fout.open("/home/xxx/path_to_package/data.csv",std::ios::out|std::ios::app);
    fout << "joint_1" << "," << "joint_2" << "," 
         << "joint_3" << "," << "joint_4" << ","
         << "joint_5" << "," << "joint_6" << ","
         << "joint_7" << "," << "x_marker" << ","
         << "y_marker" << "," << "z_marker" << ","
         << "rx_marker" << "," << "ry_marker" << ","
         << "rz_marker" << "," << "rw_marker" << "\n";

    message_filters::Subscriber<JointState> sub(nh,"joint_states",1);
    message_filters::Subscriber<PoseStamped> marker_sub(nh, "vrpn_client_node/marker/pose",5);
    //TimeSynchronizer<msgs::JointPosition, PoseStamped> sync(sub, marker_sub, 10);
    typedef sync_policies::ApproximateTime<JointState,PoseStamped> my_syncPolicy;

    Synchronizer<my_syncPolicy> sync(my_syncPolicy(10),sub,marker_sub);
    sync.registerCallback(boost::bind(&callback,_1,_2,boost::ref(fout)));

    ros::spin();
    return 0;
}

2. csv文件读取. 代码场景 从csv文件中读取数据加载到计算程序中

#include <ros/ros.h>
#include <iostream>
#include <fstream>
#include <vector>
#include <string>

#include <sensor_msgs/JointState.h>
#include <geometry_msgs/PoseStamped.h>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include "sophus/so3.hpp"
#include "sophus/se3.hpp"

#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_state/robot_state.h>

// 1. 获取marker在mocap系统下的位姿
// 2. 获取机器人末端位姿参数

int main(int argc, char* argv[]){

    ros::init(argc, argv, "calibration_main");
    ros::NodeHandle node_handle;

    ros::AsyncSpinner spinner(1);
    spinner.start();
   
    std::ifstream fin("/home/xxxx/path_to_package/data.csv",std::ios::in);
    std::vector<double> data;
    std::string line, word, temp;
    int flag = 1;
    while(getline(fin,line)){

        // used for breaking words
        std::stringstream s(line);

        if (flag == 1) {
            flag++;
            continue;
        }
        // read every column data of a row and store it in a string variable 'word'
        while(std::getline(s,word,',')){
            // add all the column data 
            // of a row to a vector
            data.push_back(stod(word));
        }
        flag++;

       
        data.clear();
    }

3. yaml-cpp文件生成 力传感器辨识后参数记录
   需要在CMakeLists.txt中加入

   find_package(yaml-cpp REQUIRED)
   include_directories(
   	include
   	${YAML_CPP_LIBRARIES}
   )
   
   add_executable(gravity_identify src/gravity_identify.cpp)
   target_link_libraries(gravity_identify ${catkin_LIBRARIES} 		${YAML_CPP_LIBRARIES})
   

   #include <yaml-cpp/yaml>
   #include <ros/package.h>
   //....
    std::string dir_package, dir_param_file;
   dir_package = ros::package::getPath("xxxxx");
   dir_param_file = dir_package + "/config/param.yaml";
   std::ofstream fout(dir_param_file);
   
   YAML::Emitter out(fout);
   out << YAML::BeginMap;
   out << YAML::Key << "l";
   out << YAML::BeginSeq;
   out << YAML::Value << h(0,0);
   out << YAML::Value << h(1,0);
   out << YAML::Value << h(2,0);
   out << YAML::EndSeq;
   out << YAML::Key << "G_b";
   out << YAML::BeginSeq;
   out << YAML::Value << g(0,0);
   out << YAML::Value << g(1,0);
   out << YAML::Value << g(2,0);
   out << YAML::EndSeq;
   out << YAML::Key << "F0";
   out << YAML::BeginSeq;
   out << YAML::Value << g(3,0);
   out << YAML::Value << g(4,0);
   out << YAML::Value << g(5,0);
   out << YAML::EndSeq;
   out << YAML::Key << "T0";
   out << YAML::BeginSeq;
   out << YAML::Value << h(3,0);
   out << YAML::Value << h(4,0);
   out << YAML::Value << h(5,0);
   out << YAML::EndSeq;
   out << YAML::EndMap;
   fout.close();