gps的定位原始数据的消息结构为vd_msgs::GpsRaw
将其转换为gps里程计,作为groundtruth
#include <deque>
#include <mutex>
#include <ros/ros.h>
#include <sensor_msgs/NavSatFix.h>
#include <sensor_msgs/Imu.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Twist.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_datatypes.h>
#include "GeographicLib/LocalCartesian.hpp"
#include <vd_msgs/GpsRaw.h>
GeographicLib::LocalCartesian geo_converter;
ros::Publisher odom_pub_;
ros::Publisher imu_pub_;
vd_msgs::GpsRaw gps_raw;
double lat = 0;
double lon = 0;
double alt = 0;
bool enable_z = false;
void gpsRawCalback(const vd_msgs::GpsRaw msg)
{
gps_raw = msg;
nav_msgs::Odometry real_gps_odom;
real_gps_odom.header.stamp = gps_raw.header.stamp;
real_gps_odom.header.frame_id = "odom";
real_gps_odom.child_frame_id = "gps_link_compare";
geometry_msgs::Quaternion catch_orientation;
catch_orientation = tf::createQuaternionMsgFromRollPitchYaw
(gps_raw.Vector.x, gps_raw.Vector.y, gps_raw.Vector.z);
/* catch_orientation = tf::createQuaternionMsgFromRollPitchYaw
(0, 0, gps_raw.Vector.z); */
real_gps_odom.pose.pose.orientation = catch_orientation;
double local_E = 0;
double local_N = 0;
double local_U = 0;
geo_converter.Forward(gps_raw.Latitude, gps_raw.Longitude, gps_raw.Altitude, local_E
9055



被折叠的 条评论
为什么被折叠?



