to input data from a csv file in cpp
How to input data(numerical values) in a csv file(values seperated by commas), to a cpp file-executable to be run in terminal to see the motion of robot in rviz??Data contains the position of robot joints at different times,part of the csv file contains values like these,each column representing each joint.
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
1.222005 -0.618795 -1.484478 -1.458752 1.548821 -88.940772 20.628733 -46.40773 -30.980427 10.593311 1.577927 -28.24077 -89.131682 -20.840441 21.234292 22.269685
0.912665 -0.666179 -1.409592 -1.440303 1.48104 -88.842338 21.44113 -47.07769 -30.874201 7.402907 1.658714 -26.159671 -89.100513 -21.301901 22.968179 22.688747
0.483921 -0.731897 -1.305772 -1.414634 1.387132 -88.705916 22.56705 -48.006169 -30.72695 3.01525 1.773535 -23.261877 -89.057369 -21.94138 25.371108 23.26944
-0.040279 -0.812168 -1.178862 -1.38335 1.272311 -88.539071 23.943755 -49.141486 -30.546927 -2.300542
-1.279702 -1.002047 -0.878746 -1.309267 1.000844 -88.144647 27.198731 -51.825795 -30.121219 -14.640345 2.279228 -11.180994 -88.879752 -24.572118 35.256071 25.658447
-1.947199 -1.10432 -0.717115 -1.269332 0.854625
following 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;
}