#include "ros/ros.h"
//#include "CameraVideoTopic/NavSatFix.h"
#include "sensor_msgs/NavSatFix.h"
#include <opencv2/highgui.hpp>
#include <opencv2/calib3d.hpp>
#include <iostream>
#include <fstream>
using namespace ros;
using namespace std;
int KeyNum;
bool bSaveLoc;
const string filename = "example.txt";
ofstream myfile(filename.c_str());
void SubscribeTopicCallBack(const sensor_msgs::NavSatFix::ConstPtr &msg) {
// ROS_INFO("SubscribeTopic:[latitude:%lf longitude:%lf altitude:%lf covariance_type:%d]",
// msg->latitude, msg->longitude, msg->altitude, msg->position_covariance_type);
// ROS_INFO("SubscribeTopic:[position_covariance:%lf %lf %lf %lf %lf %lf %lf %lf %lf]",
// msg->position_covariance[0], msg->position_covariance[1], msg->position_covariance[2],
// msg->position_covariance[3], msg->position_covariance[4], msg->position_covariance[5],
// msg->position_covariance[6], msg->position_covariance[7], msg->position_covariance[8]);
cout << "bSaveLoc" << bSaveLoc << endl;
if (bSaveLoc) {
ROS_INFO("SubscribeTopic:[latitude:%lf longitude:%lf altitude:%lf covariance_type:%d]",
msg->latitude, msg->longitude, msg->altitude, msg->position_covariance_type);
myfile << "T\n" << endl;
// myfile << msg->latitude << msg->longitude << msg->altitude << msg->position_covariance_type << endl;
// myfile << msg->position_covariance[0] << msg->position_covariance[1] << msg->position_covariance[2] << endl;
// myfile << msg->position_covariance[3] << msg->position_covariance[4] << msg->position_covariance[5] << endl;
// myfile << msg->position_covariance[6] << msg->position_covariance[7] << msg->position_covariance[8] << endl
// << endl;
bSaveLoc = false;
}
}
int main(int argc, char **argv) {
if (!myfile) {
std::cerr << "file open failed: " << std::strerror(errno) << "\n";
return 1;
}
myfile << fixed;
myfile.precision(10); // 设置精度 2
cv::namedWindow("SubscribeTopic");
init(argc, argv, "SubscribeTopic");
NodeHandle node;
Subscriber sbu = node.subscribe("/mavros/global_position/raw/fix", 100, SubscribeTopicCallBack);
ros::Rate loop_rate(60);
while (ros::ok()) {
KeyNum = cv::waitKey(2);//https://www.asciitable.com/
if (KeyNum == 'r' || KeyNum == 'R') {
bSaveLoc = true;
} else if (KeyNum == 'q' || KeyNum == 'Q') {
break;
}
ros::spinOnce();
loop_rate.sleep();
}
myfile << "KK\n" << endl;
myfile.close();
cv::destroyWindow("SubscribeTopic");
return 0;
}
解决了,输出到根目录文件夹了。