How to subscribe many topic in one code and save data in one txt file

asked 2017-07-03 07:39:12 -0500

hudaming gravatar image

updated 2017-07-03 07:40:08 -0500

I hope to subscribe many topics in one code, and I hope save the data in a txt file, so I write the code:

void receiveGPS(const sensor_msgs::NavSatFix::ConstPtr &msg) {

ofstream outfile("/home/humingming/position.csv",ios::out|ios::app);

  if(!outfile){

      cerr<<"open error!"<<endl;

exit(1);
  }

gps.header.stamp=msg->header.stamp;

string t=receivetime(gps.header.stamp.sec);

gps.status.status=msg->status.status;

gps.latitude=msg->latitude;

gps.longitude=msg->longitude;

gps.altitude=msg->altitude;

gps.position_covariance[0]=msg->position_covariance[0];

gps.position_covariance[4]=msg->position_covariance[4];

gps.position_covariance[8]=msg->position_covariance[8];

outfile<<t&lt;&lt;","&lt;<gps.latitude&lt;&lt;","&lt;<gps.longitude&lt;&lt;","&lt;<gps.altitude&lt;<endl;< p="">

outfile.close();

// ROS_INFO("The location is [%f][%f][%f]",gps.latitude,gps.longitude,gps.altitude);

}

void receiveIMU(const sensor_msgs::Imu::ConstPtr& msg) {

ofstream outfile("/home/hu/imu.csv",ios::out|ios::app);

  if(!outfile){

      cerr<<"open error!"<<endl;

      exit(1);
  }

string t=receivetime(gps.header.stamp.sec);

imu_accel.linear.x=msg->linear_acceleration.x;

imu_accel.linear.y=msg->linear_acceleration.y;

imu_accel.linear.z=msg->linear_acceleration.z;

imu_accel_cov.accel.covariance[0]=msg->linear_acceleration_covariance[0];

imu_accel_cov.accel.covariance[4]=msg->linear_acceleration_covariance[4];

imu_accel_cov.accel.covariance[8]=msg->linear_acceleration_covariance[8];

imu_vel.angular.x=msg->angular_velocity.x;

imu_vel.angular.y=msg->angular_velocity.y;

imu_vel.angular.z=msg->angular_velocity.z;

imu_vel_cov.twist.covariance[0]=msg->angular_velocity_covariance[0];

imu_vel_cov.twist.covariance[4]=msg->angular_velocity_covariance[4];

imu_vel_cov.twist.covariance[8]=msg->angular_velocity_covariance[8];

imu_ori.w=msg->orientation.w;

imu_ori.x=msg->orientation.x;

imu_ori.y=msg->orientation.y;

imu_ori.z=msg->orientation.z;

imu_ori_cov.twist.covariance[0]=msg->orientation_covariance[0];

imu_ori_cov.twist.covariance[4]=msg->orientation_covariance[4];

imu_ori_cov.twist.covariance[8]=msg->orientation_covariance[8];

outfile<<t&lt;&lt;","&lt;<imu_accel.linear.x&lt;&lt;","&lt;<imu_accel.linear.y&lt;&lt;","&lt;<imu_accel.linear.z&lt;&lt;","&lt;<imu_accel.angular.x&lt;&lt;","< p="">

<<imu_accel.angular.y&lt;&lt;","&lt;<imu_accel.angular.z&lt;&lt;","&lt;<imu_ori.w&lt;&lt;","&lt;<imu_ori.x&lt;&lt;","&lt;<imu_ori.y&lt;&lt;","< p="">

<<imu_ori.z&lt;<endl;< p="">

outfile.close();

ROS_INFO("I heard:linear_accleration[%f]/[%f]/[%f]", imu_accel.linear.x, imu_accel.linear.y, imu_accel.linear.z);

} void receiveVEL(const geometry_msgs::TwistWithCovarianceStamped::ConstPtr &msg) {

ofstream outfile("/home/hu/vel.csv",ios::out|ios::app);

  if(!outfile){

      cerr<<"open error!"<<endl;

      exit(1);
  }

string t=receivetime(gps.header.stamp.sec);

linear_vel.linear.x=msg->twist.twist.linear.x;

linear_vel.linear.y=msg->twist.twist.linear.y;

linear_vel.linear.z=msg->twist.twist.linear.z;

linear_vel_cov.covariance[0]=msg->twist.covariance[0];

linear_vel_cov.covariance[7]=msg->twist.covariance[7];

linear_vel_cov.covariance[14]=msg->twist.covariance[14];

outfile<<t&lt;&lt;","&lt;<linear_vel.linear.x&lt;&lt;","&lt;<linear_vel.linear.y&lt;&lt;","&lt;<linear_vel.linear.z&lt;<endl;< p="">

outfile.close();

}

private:

};

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

// Set up ROS.

ros::init(argc, argv, "receiveGPS");

ros::NodeHandle n;

Foo foo_object;

ros::Subscriber sub_gps = n.subscribe("gps/fix", 2, &Foo::receiveGPS,&foo_object);

ros::Subscriber sub_imu = n ... (more)

edit retag flag offensive close merge delete