小编是将欧拉角与gps数据时间戳对齐后保存到txt文档里,然后通过matlab读取数据画空间运动轨迹及欧拉角变化折线图
【matlab部分:https://blog.csdn.net/xx970829/article/details/115271215】
时间戳对齐
//message_filters::Subscriber<sensor_msgs::PointCloud2> points_sub(n, "/velodyne_points", 2000);
message_filters::Subscriber<sbg_driver::SbgEkfNav> gps_sub(n, "/sbg/ekf_nav", 100);
message_filters::Subscriber<sbg_driver::SbgEkfEuler> quat_sub(n, "/sbg/ekf_euler", 100);
typedef message_filters::sync_policies::ApproximateTime<sbg_driver::SbgEkfNav,sbg_driver::SbgEkfEuler> MySyncPolicy;//sensor_msgs::PointCloud2,
message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(100),gps_sub, quat_sub); // 同步points_sub,
保存到txt文件
FILE *fpWrite=fopen("data.txt","a");//a续写,w清除后写入
fprintf(fpWrite,"%16.10f %16.10f %16.10f %16.10f %16.10f %16.10f \n",quatmsg->angle.x,quatmsg->angle.y,quatmsg->angle.z,x,y,gpsmsg->position.z);
fclose(fpWrite);
将经纬度转换到墨卡托
void lonLat2WebMercator(double jing,double wei,double *x,double *y)
{
*x = jing * 20037508.34 / 180;
double ly = log(tan((90+ wei)*PI/360))/(PI/180);
*y = ly *20037508.34/180;
}
完整程序
// rosrun pcl_ros bag_to_pcd 555.bag /velodyne_points /home/xx/catkin_my/pcd
#include<ros/ros.h>
#include<iostream>
#include<fstream>
#include<sstream>
#include<string>
#include<vector>
#include <math.h>
#include <stdio.h>
#include<sensor_msgs/PointCloud2.h>
#include "std_msgs/String.h"
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_types.h>
#include <pcl/PCLPointCloud2.h>
#include <pcl/conversions.h>
#include <pcl_ros/transforms.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/filter.h>
#include "tf/transform_datatypes.h"
#include <tf2_msgs/TFMessage.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/common/impl/io.hpp>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/filters/passthrough.h> //直通滤波
#include <sbg_driver/SbgImuData.h>
#include <sbg_driver/SbgEkfQuat.h>
#include <sbg_driver/SbgEkfEuler.h>
#include <sbg_driver/SbgGpsPos.h>
#include <sbg_driver/SbgEkfNav.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/sync_policies/exact_time.h>
#include <message_filters/time_synchronizer.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/visualization/cloud_viewer.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/PointCloud.h>
#define PI 3.141592653
using namespace std;
using namespace pcl;
void lonLat2WebMercator(double jing,double wei,double *x,double *y)
{
*x = jing * 20037508.34 / 180;
double ly = log(tan((90+ wei)*PI/360))/(PI/180);
*y = ly *20037508.34/180;
}
void filter_point_cloud(const pcl::PointCloud<pcl::PointXYZ>&original_cloud,pcl::PointCloud<pcl::PointXYZ>&tmp_cloud_filtered)
{
tmp_cloud_filtered.clear();
float x_pass;
float y_pass;
float z_pass;
for (int i = 0; i < original_cloud.size(); i++)
{
x_pass=original_cloud.points[i].x;
y_pass=original_cloud.points[i].y;
z_pass=original_cloud.points[i].z;
tmp_cloud_filtered.push_back(original_cloud.points[i]);
}
}
int i=0;
//pcl::visualization::CloudViewer viewer("pcd viewer");
//const boost::shared_ptr<const sensor_msgs::PointCloud2>& pointmsg,
void chatterCallback(const boost::shared_ptr<const sbg_driver::SbgEkfNav>& gpsmsg,const boost::shared_ptr<const sbg_driver::SbgEkfEuler>& quatmsg)
{
/*
pcl::PCLPointCloud2 pcl_pc2;
pcl_conversions::toPCL(*pointmsg,pcl_pc2);
pcl::PointCloud<pcl::PointXYZ>::Ptr tmp_cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromPCLPointCloud2(pcl_pc2,*tmp_cloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr tmp_cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
filter_point_cloud(*tmp_cloud,*tmp_cloud_filtered);
viewer.showCloud(tmp_cloud_filtered);
pcl::io::savePCDFileASCII("test_pcd.pcd",*tmp_cloud_filtered);
//pcl::io::savePCDFileASCII("write_pcd_test.pcd", pcl_pc2);//保存pcd
*/
/*
imu_data.orientation.x = quatmsg->quaternion.x;
imu_data.orientation.y = quatmsg->quaternion.y;
imu_data.orientation.z = quatmsg->quaternion.z;
imu_data.orientation.w = quatmsg->quaternion.w;
imu_data.angular_velocity.x = gpsmsg->position.x;
imu_data.angular_velocity.y = gpsmsg->position.y;
imu_data.angular_velocity.z = gpsmsg->position.z;
*/
//cout<< setprecision(10) <<quatmsg->quaternion.w<<endl;
//cout<< setprecision(10) <<quatmsg->quaternion.x<<endl;
//cout<< setprecision(10) <<quatmsg->quaternion.y<<endl;
//cout<< setprecision(10) <<quatmsg->quaternion.z<<endl;
double x,y=0;
lonLat2WebMercator(gpsmsg->position.y,gpsmsg->position.x,&x,&y);
cout<< setprecision(10) <<quatmsg->angle.x<<endl;
cout<< setprecision(10) <<quatmsg->angle.y<<endl;
cout<< setprecision(10) <<quatmsg->angle.z<<endl;
cout<< setprecision(10) <<x<<endl;
cout<< setprecision(10) <<y<<endl;
cout<< setprecision(10) <<gpsmsg->position.z<<endl;
FILE *fpWrite=fopen("data.txt","a");//a续写,w清除后写入
fprintf(fpWrite,"%16.10f %16.10f %16.10f %16.10f %16.10f %16.10f \n",quatmsg->angle.x,quatmsg->angle.y,quatmsg->angle.z,x,y,gpsmsg->position.z);
fclose(fpWrite);
i++;
}
int main(int argc,char** argv)
{
ros::init (argc, argv, "ros_points");
ros::NodeHandle n;
//message_filters::Subscriber<sensor_msgs::PointCloud2> points_sub(n, "/velodyne_points", 2000);
message_filters::Subscriber<sbg_driver::SbgEkfNav> gps_sub(n, "/sbg/ekf_nav", 100);
message_filters::Subscriber<sbg_driver::SbgEkfEuler> quat_sub(n, "/sbg/ekf_euler", 100);
typedef message_filters::sync_policies::ApproximateTime<sbg_driver::SbgEkfNav,sbg_driver::SbgEkfEuler> MySyncPolicy;//sensor_msgs::PointCloud2,
message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(100),gps_sub, quat_sub); // 同步points_sub,
sync.registerCallback(boost::bind(&chatterCallback, _1,_2)); // _1,
ros::spin ();
}