假设已经接收到GPS信息，并且提取到了经纬度和高程信息，经此节点转换为·/map·坐标系下的XYZ和yaw数值
Node 接口：
- name: /fix2tfpose
publish: [/gnss_pose, /gnss_stat]
subscribe: [/fix]
先来康康经过nmea结算后的/fix消息包括哪些内容
sensor_msgs/NavSatFix Message
File: sensor_msgs/NavSatFix.msg
Raw Message Definition
# Navigation Satellite fix for any Global Navigation Satellite System
# Specified using the WGS 84 reference ellipsoid
# header.stamp specifies the ROS time for this measurement (the
# corresponding satellite time may be reported using the
# sensor_msgs/TimeReference message).
# header.frame_id is the frame of reference reported by the satellite
# receiver, usually the location of the antenna. This is a
# Euclidean frame relative to the vehicle, not a reference
# ellipsoid.
Header header
# satellite fix status information
NavSatStatus status
# Latitude [degrees]. Positive is north of equator; negative is south.
# 赤道以北为正，以南为负
float64 latitude
# Longitude [degrees]. Positive is east of prime meridian; negative is west.
正数位于本初子午线以东；负面是西方。
float64 longitude
# Altitude [m]. Positive is above the WGS 84 ellipsoid
# (quiet NaN if no altitude is available).
float64 altitude
# Position covariance [m^2] defined relative to a tangential plane
# through the reported position. The components are East, North, and
# Up (ENU), in row-major order.
#
# Beware: this coordinate system exhibits singularities at the poles.
注意:这个坐标系在两极表现出奇异性。
float64[9] position_covariance
# If the covariance of the fix is known, fill it in completely. If the
# GPS receiver provides the variance of each measurement, put them
# along the diagonal. If only Dilution of Precision is available,
# estimate an approximate covariance from that.
如果修正的协方差是已知的，就把它完全填上。如果GPS接收器提供了每次测量的
方差，把它们放好沿着对角线。只要能提高精确度，估计一个近似的协方差。
uint8 COVARIANCE_TYPE_UNKNOWN = 0
#近似标准差
uint8 COVARIANCE_TYPE_APPROXIMATED = 1
#当只知道对角线协方差矩阵时
uint8 COVARIANCE_TYPE_DIAGONAL_KNOWN = 2
#当知道全部协方差矩阵时
uint8 COVARIANCE_TYPE_KNOWN = 3
uint8 position_covariance_type
Compact Message Definition
uint8 COVARIANCE_TYPE_UNKNOWN=0
uint8 COVARIANCE_TYPE_APPROXIMATED=1
uint8 COVARIANCE_TYPE_DIAGONAL_KNOWN=2
uint8 COVARIANCE_TYPE_KNOWN=3
std_msgs/Header header
sensor_msgs/NavSatStatus status
float64 latitude
float64 longitude
float64 altitude
float64[9] position_covariance
uint8 position_covariance_type
主程序：
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>
#include <ros/ros.h>
#include <sensor_msgs/NavSatFix.h>
#include <std_msgs/Bool.h>
#include <tf/transform_broadcaster.h>#include <iostream>
//GPS坐标转换程式包
#include <gnss/geo_pos_conv.hpp>
//创建一个pose发布node
static ros::Publisher pose_publisher;
//创建是否接收到FIX信息
static ros::Publisher stat_publisher;
//创建是否接收到FIX信息
static std_msgs::Bool gnss_stat_msg;
//用来判断是否移动
static geometry_msgs::PoseStamped _prev_pose;
//四元数是ROS中的通用表达形式
static geometry_msgs::Quaternion _quat;
//车辆篇航值
static double yaw;
// true if position history is long enough to compute orientation
static bool _orientation_ready = false;
//GPS坐标分为不同的PLANE
# Position covariance [m^2] defined relative to a tangential plane
# through the reported position. The components are East, North, and
# Up (ENU), in row-major order.
static int _plane;
//回调函数
static void GNSSCallback(const sensor_msgs::NavSatFixConstPtr &msg)
{//大地转换库设置geo_pos_conv geo;geo.set_plane(_plane);//latitude....转为x,y,zgeo.llh_to_xyz(msg->latitude, msg->longitude, msg->altitude);
//下面是创建一些列TF发布，并且填充pose消息帧static tf::TransformBroadcaster pose_broadcaster;tf::Transform pose_transform;tf::Quaternion pose_q;geometry_msgs::PoseStamped pose;pose.header = msg->header;// pose.header.stamp = ros::Time::now();pose.header.frame_id = "map";pose.pose.position.x = geo.y();pose.pose.position.y = geo.x();pose.pose.position.z = geo.z();// set gnss_stat，判断GPS信号结接收状态if (pose.pose.position.x == 0.0 || pose.pose.position.y == 0.0 || pose.pose.position.z == 0.0){gnss_stat_msg.data = false;}else{gnss_stat_msg.data = true;}double distance = sqrt(pow(pose.pose.position.y - _prev_pose.pose.position.y, 2) +pow(pose.pose.position.x - _prev_pose.pose.position.x, 2));std::cout << "distance : " << distance << std::endl;
//计算篇行角通过atan，math.hppif (distance > 0.2){yaw = atan2(pose.pose.position.y - _prev_pose.pose.position.y, pose.pose.position.x - _prev_pose.pose.position.x);_quat = tf::createQuaternionMsgFromYaw(yaw);_prev_pose = pose;_orientation_ready = true;}
//进行最后的话题发布if (_orientation_ready){pose.pose.orientation = _quat;pose_publisher.publish(pose);stat_publisher.publish(gnss_stat_msg);static tf::TransformBroadcaster br;tf::Transform transform;tf::Quaternion q;transform.setOrigin(tf::Vector3(pose.pose.position.x, pose.pose.position.y, pose.pose.position.z));q.setRPY(0, 0, yaw);transform.setRotation(q);br.sendTransform(tf::StampedTransform(transform, msg->header.stamp, "map", "gps"));}
}
//main
int main(int argc, char **argv)
{ros::init(argc, argv, "fix2tfpose");ros::NodeHandle nh;ros::NodeHandle private_nh("~");private_nh.getParam("plane", _plane);pose_publisher = nh.advertise<geometry_msgs::PoseStamped>("gnss_pose", 1000);stat_publisher = nh.advertise<std_msgs::Bool>("/gnss_stat", 1000);ros::Subscriber gnss_pose_subscriber = nh.subscribe("fix", 100, GNSSCallback);ros::spin();return 0;
}
launch文件
<!-- -->
<launch><arg name="plane" default="7"/><node pkg="gnss_localizer" type="fix2tfpose" name="fix2tfpose" output="log"><param name="plane" value="$(arg plane)"/></node></launch>
CMake文件
cmake_minimum_required(VERSION 2.8.3)
project(gnss_localizer)find_package(autoware_build_flags REQUIRED)
find_package(catkin REQUIRED COMPONENTSroscppgeometry_msgsstd_msgsnmea_msgsgnsstf
)catkin_package()SET(CMAKE_CXX_FLAGS "-O2 -g -Wall ${CMAKE_CXX_FLAGS}")include_directories(${catkin_INCLUDE_DIRS}
)add_executable(fix2tfposenodes/fix2tfpose/fix2tfpose.cpp
)
target_link_libraries(fix2tfpose ${catkin_LIBRARIES})
add_dependencies(fix2tfpose ${catkin_EXPORTED_TARGETS})add_executable(nmea2tfposenodes/nmea2tfpose/nmea2tfpose_core.cppnodes/nmea2tfpose/nmea2tfpose_node.cpp
)target_include_directories(nmea2tfpose PRIVATE nodes/nmea2tfpose ${catkin_INCLUDE_DIRS})
target_link_libraries(nmea2tfpose ${catkin_LIBRARIES})
add_dependencies(nmea2tfpose ${catkin_EXPORTED_TARGETS})if(CATKIN_ENABLE_TESTING)find_package(rostest REQUIRED)find_package(rosunit REQUIRED)add_rostest_gtest(nmea_test test/nmea_test.test test/nmea_test.cpp)target_link_libraries(nmea_test ${catkin_LIBRARIES})add_dependencies(nmea_test ${catkin_EXPORTED_TARGETS})
endif()install(TARGETS nmea2tfpose fix2tfposeARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})install(DIRECTORY launch/DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launchPATTERN ".svn" EXCLUDE)
其他：
ROS中智能指针定义方式
#ifndef GEOMETRY_MSGS_MESSAGE_POSESTAMPED_H
#define GEOMETRY_MSGS_MESSAGE_POSESTAMPED_H#include <string>
#include <vector>
#include <map>#include <ros/types.h>
#include <ros/serialization.h>
#include <ros/builtin_message_traits.h>
#include <ros/message_operations.h>#include <std_msgs/Header.h>
#include <geometry_msgs/Pose.h>namespace geometry_msgs
{
template <class ContainerAllocator>
struct PoseStamped_
{typedef PoseStamped_<ContainerAllocator> Type;PoseStamped_(): header(), pose() {}PoseStamped_(const ContainerAllocator& _alloc): header(_alloc), pose(_alloc) {(void)_alloc;}typedef ::std_msgs::Header_<ContainerAllocator> _header_type;_header_type header;typedef ::geometry_msgs::Pose_<ContainerAllocator> _pose_type;_pose_type pose;typedef boost::shared_ptr< ::geometry_msgs::PoseStamped_<ContainerAllocator> > Ptr;typedef boost::shared_ptr< ::geometry_msgs::PoseStamped_<ContainerAllocator> const> ConstPtr;}; // struct PoseStamped_typedef ::geometry_msgs::PoseStamped_<std::allocator<void> > PoseStamped;typedef boost::shared_ptr< ::geometry_msgs::PoseStamped > PoseStampedPtr;
typedef boost::shared_ptr< ::geometry_msgs::PoseStamped const> PoseStampedConstPtr;