假設已經接收到GPS信息，並且提取到了經緯度和高程信息（之後稱lla信息），經國此節點轉換爲/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,z
geo.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.hpp
if (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 COMPONENTS
roscpp
geometry_msgs
std_msgs
nmea_msgs
gnss
tf
)
catkin_package()
SET(CMAKE_CXX_FLAGS "-O2 -g -Wall ${CMAKE_CXX_FLAGS}")
include_directories(
${catkin_INCLUDE_DIRS}
)
add_executable(fix2tfpose
nodes/fix2tfpose/fix2tfpose.cpp
)
target_link_libraries(fix2tfpose ${catkin_LIBRARIES})
add_dependencies(fix2tfpose ${catkin_EXPORTED_TARGETS})
add_executable(nmea2tfpose
nodes/nmea2tfpose/nmea2tfpose_core.cpp
nodes/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 fix2tfpose
ARCHIVE 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}/launch
PATTERN ".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;
