Question

How to input data (numerical values) in a CSV file (values separated by commas), to a C++ file-executable to be run in a terminal to see the motion of a robot in rviz? The data contains the position of robot joints at different times; part of the CSV file contains values like these, each column representing each joint. Each line of the file contains 17 floating point numbers separated by commas, and there are over 100 lines in my sample data file. One line is:

 1.388106,-0.593356,-1.524699,-1.468721,1.585204,-88.993656,20.192482,-46.047969,-31.037494,12.317457,1.535528,-29.353799,-89.148412,-20.592636,20.303178,22.044684,19.933448

The following code is the publishJoint.cpp file:

    #include <string>
    #include <ros/ros.h>
    #include <sensor_msgs/JointState.h>

    int main(int argc, char** argv) 
    {
        ros::init(argc, argv, "publishJoints");
        ros::NodeHandle n;
        ros::Publisher joint_pub = n.advertise<sensor_msgs::JointState>("joint_states", 100);
        ros::Rate loop_rate(1000000);

        const double degree = M_PI/180;

        // robot state
        double swivel=0;
        double tilt=0;


       // message declarations
       sensor_msgs::JointState joint_state;

       joint_state.name.resize(17);
       joint_state.position.resize(17);

       while (ros::ok()) 
       {
           //update joint_state
           joint_state.header.stamp = ros::Time::now();

           swivel=0;
           joint_state.name[0] ="m3joint_mt4_j0";
           joint_state.position[0] = swivel;

           joint_state.name[1] ="m3joint_mt4_j1";
           joint_state.position[1] = tilt;

           joint_state.name[2] ="m3joint_slave_mt4_j2";
           joint_state.position[2] = swivel;

           joint_state.name[3] ="left_shoulder_pan_joint";
           joint_state.position[3] = tilt;

           joint_state.name[4] ="left_shoulder_lift_joint";
           joint_state.position[4] = tilt;

           joint_state.name[5] ="left_elbow_pan_joint";
           joint_state.position[5] = tilt;

           joint_state.name[6] ="left_elbow_lift_joint";
           joint_state.position[6] = tilt;

           joint_state.name[7] ="m3joint_ma14_j4";
           joint_state.position[7] = tilt;

           joint_state.name[8] ="m3joint_ma14_j5";
           joint_state.position[8] = tilt;

           joint_state.name[9] ="m3joint_ma14_j6";
           joint_state.position[9] = tilt;

           joint_state.name[10] ="right_shoulder_pan_joint";
           joint_state.position[10] = swivel;

           joint_state.name[11] ="right_shoulder_lift_joint";
           joint_state.position[11] = swivel;

           joint_state.name[12] ="right_elbow_pan_joint";
           joint_state.position[12] = swivel;

           joint_state.name[13] ="right_elbow_lift_joint";
           joint_state.position[13] = swivel;

           joint_state.name[14] ="m3joint_ma12_j4";
           joint_state.position[14] = swivel;

           joint_state.name[15] ="m3joint_ma12_j5";
           joint_state.position[15] = swivel;

           joint_state.name[16] ="m3joint_ma12_j6" ;
           joint_state.position[16] = swivel;

           tilt += 0.000001;

           //send the joint state and transform
           joint_pub.publish(joint_state);

           // This will adjust as needed per iteration
           loop_rate.sleep();
       }
   return 0;
}

How can I read the data from the file into my program?

Answer 1

Since there are supposed to be 17 entries per line, I would read each line into a string, and then carve the string into 17 values using a string stream. Here is some outline code that does the job; it can certainly be enhanced and would have to be adapted to fit your program.

#include <iostream>
#include <sstream>
#include <string>
#include <cstdlib>
using namespace std;

static void err_exit(int line, int field, const char *tag, string &s)
{
    cerr << "Format error: line " << line << ", field " << field << ": " << tag << '\n';
    cerr << "Data: " << s << endl;
    exit(1);
}

int main(void)
{
    string s;
    int lineno = 0;
    while (cin >> s)
    {
        cout << "<<" << s << ">>" << endl;
        lineno++;
        stringstream ss(s);
        enum { NUM_ENTRIES = 17 };
        double v[NUM_ENTRIES];
        char delim;
        for (int i = 0; i < NUM_ENTRIES; i++)
        {
            if (!(ss >> v[i]))
                err_exit(lineno, i, "extract failed", s);
            else if (i < NUM_ENTRIES - 1 && !((ss >> delim) && delim == ','))
                err_exit(lineno, i, "delimiter incorrect", s);
            else if (i == NUM_ENTRIES - 1 && (ss >> delim))
                err_exit(lineno, i, "extra data at end of line", s);
        }
        // Process v
        cout << "Line: " << lineno << '\n';
        for (int i = 0; i < NUM_ENTRIES; i++)
        {
            cout << "Entry " << i << ": " << v[i] << '\n';
        }
        cout << endl;
    }
}

Given your input line above, it produces the output:

<<1.388106,-0.593356,-1.524699,-1.468721,1.585204,-88.993656,20.192482,-46.047969,-31.037494,12.317457,1.535528,-29.353799,-89.148412,-20.592636,20.303178,22.044684,19.933448>>
Line: 1
Entry 0: 1.38811
Entry 1: -0.593356
Entry 2: -1.5247
Entry 3: -1.46872
Entry 4: 1.5852
Entry 5: -88.9937
Entry 6: 20.1925
Entry 7: -46.048
Entry 8: -31.0375
Entry 9: 12.3175
Entry 10: 1.53553
Entry 11: -29.3538
Entry 12: -89.1484
Entry 13: -20.5926
Entry 14: 20.3032
Entry 15: 22.0447
Entry 16: 19.9334