Initial commit

sensors/beitian-bg620/CMakeLists.txt

@@ -0,0 +1,220 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(beitian-bg620)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  geometry_msgs
+  nmea_msgs
+  nav_msgs
+  roscpp
+  rospy
+  sensor_msgs
+  std_msgs
+  tf2
+  tf2_geometry_msgs
+  tf2_ros
+)
+find_package(PkgConfig)
+pkg_check_modules(SERIAL libserial)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   geometry_msgs#   nav_msgs#   sensor_msgs#   std_msgs#   tf2_geometry_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+ INCLUDE_DIRS include
+ LIBRARIES beitian-bg620
+ CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy sensor_msgs std_msgs tf2 tf2_geometry_msgs tf2_ros nmea_msgs serial
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/MKT4052A_DRIVER.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/MKT4052A_DRIVER_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+catkin_install_python(PROGRAMS
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_MKT4052A_DRIVER.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
+
+add_executable(bg620_serial_reader_node src/bg620_serial_reader_node.cpp  src/bg620_serial_reader_core.cpp)
+target_link_libraries(bg620_serial_reader_node ${catkin_LIBRARIES} ${SERIAL_LDFLAGS})
+
+add_executable(bg620_sentence_parse_node src/bg620_sentence_parse_node.cpp src/bg620_sentence_parse_core.cpp)
+target_link_libraries(bg620_sentence_parse_node ${catkin_LIBRARIES})

sensors/beitian-bg620/README.txt

@@ -0,0 +1 @@
+该包为北天BG-620 RTK组合导航驱动包。

sensors/beitian-bg620/include/bg620_sentence_parse_core.h

@@ -0,0 +1,106 @@
+#ifndef BG620_SENTENCE_PARSE_H
+#define BG620_SENTENCE_PARSE_H
+
+#include "ros/ros.h"
+#include "tf2_ros/buffer.h"
+#include "tf2_ros/transform_listener.h"
+#include "tf2_ros/transform_broadcaster.h"
+#include <tf2_eigen/tf2_eigen.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include "geometry_msgs/TransformStamped.h"
+#include "nmea_msgs/Sentence.h"
+#include "sensor_msgs/Imu.h"
+#include "nav_msgs/Path.h"
+#include "tf/tf.h"
+#include "Eigen/Core"
+#include "Eigen/Geometry"
+#include "gpsTools.hpp"
+
+// Added by dawei
+#include "sensor_msgs/NavSatFix.h"
+#include "nav_msgs/Odometry.h"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
+#include "Eigen/Dense"
+
+#define __APP_NAME__ "bg620_sentence_parse"
+
+class BG620SentenceParse{
+    public:
+        BG620SentenceParse();
+        ~BG620SentenceParse();
+
+    private:
+        ros::NodeHandle nh_;
+        ros::NodeHandle private_nh_;
+        tf2_ros::TransformListener listener;
+        tf2_ros::Buffer buffer;
+    
+    private:
+        std::string outputNavSatFixTopic_;
+        std::string outputNavSatFixFrameId;
+
+        std::string outputOdometryTopic_;
+        std::string outputOdometryFrameId;
+
+        std::string outputLidarOdometryTopic_;
+        std::string outputLidarOdometryFrameId;
+
+        std::string outputTransFromStampedTopic_;
+        std::string outputTransFromStampedFrameId;
+
+        std::string outputImuTopic_;
+        std::string outputImuFrameId;
+
+        std::string outputGpsPathTopic_;
+        std::string robotOdomFrameId;
+
+        bool publishDynamicTf;
+        std::string inputNmeaSentenceTopic_;
+
+        double orinLon;
+        double orinLat;
+        double orinAlt;
+        double e_bias;
+        double n_bias;
+
+    private:
+        ros::Publisher navSatFixPublisher;
+        ros::Publisher odometryPublisher;
+        ros::Publisher lidarOdometryPublisher;
+        ros::Publisher transformStampedPublisher;
+        ros::Publisher imuPublisher;
+        ros::Publisher gpsPathPublisher;
+        ros::Subscriber nmeaSentenceSubscriber;
+        tf2_ros::TransformBroadcaster transformBroadcaster;
+
+    private:
+        GpsTools gpsTools;
+        nav_msgs::Path gpsPath;
+
+    private:
+        std::vector<std::string> splitStr(std::string str, const std::string &pattern);
+        void nmeaSentenceCallback(const nmea_msgs::Sentence::ConstPtr& msg);
+        void processBG620Data();
+
+        // Addd by Dawei
+        struct {
+            sensor_msgs::NavSatFix nav_fix;
+            double speed = 0.0;
+            double track_angle = 0.0;
+            double heading_deg = 0.0;
+            geometry_msgs::Quaternion orientation;
+            bool has_gga = false;
+            bool has_rmc = false;
+            bool has_heading = false;
+            ros::Time stamp;
+        } bg620_data_;
+        geometry_msgs::Quaternion current_orientation_;
+        double current_speed_;
+        Eigen::Quaternionf q;
+        // 新增辅助函数
+        double dmToDeg(double dm, char direction);
+        ros::Time parseUTCTime(const std::string& utc_time, const std::string& date = "");
+        double deg2rad(double deg) { return deg * M_PI / 180.0; }
+};
+
+#endif

sensors/beitian-bg620/include/bg620_serial_reader_core.h

@@ -0,0 +1,26 @@
+#ifndef RTK_SERIAL_READER_JIAKAO_H
+#define RTK_SERIAL_READER_JIAKAO_H
+
+#include "ros/ros.h"
+#include "nmea_msgs/Sentence.h"
+
+#define __APP_NAME__ "rtk_serial_reader_bg620"
+
+class RtkSerialReaderBG620{
+    public:
+        RtkSerialReaderBG620();
+        ~RtkSerialReaderBG620();
+        void run();
+
+    private:
+        ros::NodeHandle nh_;
+        ros::NodeHandle private_nh_;
+        ros::Publisher sentence_publisher;
+
+    private:
+        std::string outPutSentenceTopic;
+        std::string gpsComChannel;
+        int gpsComBaudRate;
+};
+
+#endif

sensors/beitian-bg620/include/gpsTools.hpp

@@ -0,0 +1,143 @@
+//
+// Created by echo on 2019/11/26.
+//
+
+#ifndef PCD_COMPARE_GPSTOOLS_H
+#define PCD_COMPARE_GPSTOOLS_H
+#include <nav_msgs/Odometry.h>
+#include <sensor_msgs/NavSatFix.h>
+#include <boost/foreach.hpp>
+#include <geometry_msgs/QuaternionStamped.h>
+#include <Eigen/Core>
+#define DEG_TO_RAD 0.01745329252
+#define EARTH_MAJOR 6378137.0            ///< WGS84 MAJOR AXIS
+#define EARTH_MINOR 6356752.31424518    ///< WGS84 MINOR AXIS
+
+class GpsTools {
+ public:
+  GpsTools() { lla_origin_.setIdentity(); };
+
+
+  // Eigen::Vector3d LLA2ECEF(const Eigen::Vector3d &lla);
+  //  Eigen::Vector3d ECEF2LLA(const Eigen::Vector3d &ecef);
+  //  Eigen::Vector3d ECEF2ENU(const Eigen::Vector3d &ecef);
+  //  Eigen::Vector3d ENU2ECEF(const Eigen::Vector3d &enu);
+  //static Eigen::Vector3d GpsMsg2Eigen(const sensor_msgs::NavSatFix &gps_msgs);
+  // void updateGPSpose(const sensor_msgs::NavSatFix &gps_msgs);
+
+  /**
+   * ros msg to eigen
+   * @param gps_msgs
+   * @return
+   */
+  static Eigen::Vector3d GpsMsg2Eigen(const sensor_msgs::NavSatFix &gps_msgs) {
+    Eigen::Vector3d
+        lla(gps_msgs.latitude, gps_msgs.longitude, gps_msgs.altitude);
+    return lla;
+  }
+
+
+  /**
+   *  //2. LLA经度(longitude),纬度(latitude)和高度(altitude)经纬高坐标系 转(Earth-Centered, Earth-Fixed)
+   *  Z轴指向指向北，但不完全精确地与地球转动轴重合。转动轴有微小“摆动”，称之为“极运动(polar motion)”
+   *  X轴在球面上与格林威治线和赤道的交点
+   * @param lla
+   * @return
+   */
+  Eigen::Vector3d LLA2ECEF(const Eigen::Vector3d &lla) {
+    Eigen::Vector3d ecef;
+    double lat = deg2rad(lla.x());
+    double lon = deg2rad(lla.y());
+    double alt = lla.z();
+    double earth_r = pow(EARTH_MAJOR, 2)
+        / sqrt(pow(EARTH_MAJOR * cos(lat), 2) + pow(EARTH_MINOR * sin(lat), 2));
+    ecef.x() = (earth_r + alt) * cos(lat) * cos(lon);
+    ecef.y() = (earth_r + alt) * cos(lat) * sin(lon);
+    ecef.z() = (pow(EARTH_MINOR / EARTH_MAJOR, 2) * earth_r + alt) * sin(lat);
+
+    return ecef;
+  }
+
+  Eigen::Vector3d ECEF2LLA(const Eigen::Vector3d &ecef) {
+    double e =
+        sqrt((pow(EARTH_MAJOR, 2) - pow(EARTH_MINOR, 2)) / pow(EARTH_MAJOR, 2));
+    double e_ =
+        sqrt((pow(EARTH_MAJOR, 2) - pow(EARTH_MINOR, 2)) / pow(EARTH_MINOR, 2));
+    double p = sqrt(pow(ecef.x(), 2) + pow(ecef.y(), 2));
+    double theta = atan2(ecef.z() * EARTH_MAJOR, p * EARTH_MINOR);
+
+    double lon = atan2(ecef.y(), ecef.x());
+    double lat = atan2((ecef.z() + pow(e_, 2) * EARTH_MINOR * pow(sin(theta), 3)),
+                       p - pow(e, 2) * EARTH_MAJOR * pow(cos(theta), 3));
+    double earth_r = pow(EARTH_MAJOR, 2)
+        / sqrt(pow(EARTH_MAJOR * cos(lat), 2) + pow(EARTH_MINOR * sin(lat), 2));
+    double alt = p / cos(lat) - earth_r;
+    Eigen::Vector3d lla(rad2deg(lat), rad2deg(lon), alt);
+    return lla;
+  }
+
+  Eigen::Vector3d ECEF2ENU(const Eigen::Vector3d &ecef) {
+    double lat = deg2rad(lla_origin_.x());
+    double lon = deg2rad(lla_origin_.y());
+
+    Eigen::Vector3d t = -LLA2ECEF(lla_origin_);
+    Eigen::Matrix3d r;
+    r << -sin(lon), cos(lon), 0,
+        -cos(lon) * sin(lat), -sin(lat) * sin(lon), cos(lat),
+        cos(lon) * cos(lat), sin(lon) * cos(lat), sin(lat);
+
+    Eigen::Vector3d enu;
+    enu = ecef + t;
+    enu = r * enu;
+    return enu;
+  }
+
+  Eigen::Vector3d ENU2ECEF(const Eigen::Vector3d &enu) {
+    double lat = deg2rad(lla_origin_.x());
+    double lon = deg2rad(lla_origin_.y());
+
+    Eigen::Vector3d t = LLA2ECEF(lla_origin_);
+    Eigen::Matrix3d r;
+    r << -sin(lon), -cos(lon) * sin(lat), cos(lon) * cos(lat),
+        cos(lon), -sin(lon) * sin(lat), sin(lon) * cos(lat),
+        0, cos(lat), sin(lat);
+    Eigen::Vector3d ecef;
+    ecef = r * enu + t;
+    return ecef;
+  }
+
+  void updateGPSpose(const sensor_msgs::NavSatFix &gps_msgs) {
+    //检查状态4
+    if (gps_msgs.status.status == 4 || gps_msgs.status.status == 5 || gps_msgs.status.status == 1
+        || gps_msgs.status.status == 2) {
+      //第一个的时候设置为起点
+      if (lla_origin_ == Eigen::Vector3d::Identity()) {
+        Eigen::Vector3d lla = GpsMsg2Eigen(gps_msgs);
+        lla_origin_ = lla;
+        std::cout << "GPS origin: " << lla_origin_ << "\n status: " << gps_msgs.status.status << std::endl;
+      } else {
+        Eigen::Vector3d lla = GpsMsg2Eigen(gps_msgs);
+        Eigen::Vector3d ecef = LLA2ECEF(lla);
+        Eigen::Vector3d enu = ECEF2ENU(ecef);
+        gps_pos_ = enu;
+        std::cout << "GPS lla_origin_: " << lla_origin_ << "\n curr" << gps_pos_ << std::endl;
+      }
+    }
+  }
+
+//变量部分
+  //1.lla的起点
+  Eigen::Vector3d lla_origin_;
+  //2.enu下的坐标
+  Eigen::Vector3d gps_pos_;
+ private:
+  static inline double deg2rad(const double &deg) {
+    return deg * DEG_TO_RAD;
+  };
+  static inline double rad2deg(const double &rad) {
+    return rad / DEG_TO_RAD;
+  }
+
+};
+
+#endif //PCD_COMPARE_GPSTOOLS_H

sensors/beitian-bg620/launch/beitian_bg620_driver.launch

@@ -0,0 +1,4 @@
+<launch>
+    <include file="$(find beitian-bg620)/launch/include/beitian_bg620_serial_reader.launch" />
+    <include file="$(find beitian-bg620)/launch/include/beitian_bg620_sentence_parse.launch" />
+</launch>

sensors/beitian-bg620/launch/include/beitian_bg620_sentence_parse.launch

@@ -0,0 +1,69 @@
+<launch>
+    <arg name="outputNavSatFixTopic_" default="/bg620/gpsFix"/>
+    <arg name="outputNavSatFixFrameId" default="gps"/>
+
+    <arg name="outputOdometryTopic_" default="/bg620/gpsOdom"/>
+    <arg name="outputOdometryFrameId" default="gps"/>
+
+    <arg name="outputLidarOdometryTopic_" default="/bg620/lidarOdom"/>
+    <arg name="outputLidarOdometryFrameId" default="c32_L"/>
+
+    <arg name="outputTransFromStampedTopic_" default="/bg620/gpsTransFromStamped"/>
+    <arg name="outputTransFromStampedFrameId" default="gps"/>
+
+    <arg name="outputImuTopic_" default="/bg620/gpsImu"/>
+    <arg name="outputImuFrameId" default="gps"/>
+
+    <arg name="outputGpsPathTopic_" default="/bg620/gpsPath"/>
+    <arg name="robotOdomFrameId" default="base_link"/>
+
+    <arg name="inputNmeaSentenceTopic_" default="/bg620/nmeaSentence"/>
+    <arg name="publishDynamicTf" default="false"/>
+
+    <!-- origin of dd_yuanqu -->
+    <!-- <arg name="orinLon" default="116.178987"/>
+    <arg name="orinLat" default="39.903197"/>
+    <arg name="orinAlt" default="75.0848"/> -->
+
+    <!-- origin of haidian_park -->
+    <arg name="orinLon" default="116.2899854448"/>
+    <arg name="orinLat" default="39.98534"/>
+    <arg name="orinAlt" default="48.389"/>
+
+    <!-- origin of caihonghu_park -->
+    <!-- <arg name="orinLon" default="117.2874076873333"/>
+    <arg name="orinLat" default="36.67463832666667"/>
+    <arg name="orinAlt" default="115.1813"/> -->
+
+    <arg name="e_bias" default="0.0"/>
+    <arg name="n_bias" default="0.0"/>
+
+    <node name="bg620_sentence_parse_node" pkg="beitian-bg620" type="bg620_sentence_parse_node" output="screen">
+      <param name="outputNavSatFixTopic_" value="$(arg outputNavSatFixTopic_)" type="string"/>
+      <param name="outputNavSatFixFrameId" value="$(arg outputNavSatFixFrameId)" type="string"/>
+
+      <param name="outputOdometryTopic_" value="$(arg outputOdometryTopic_)" type="string"/>
+      <param name="outputOdometryFrameId" value="$(arg outputOdometryFrameId)" type="string"/>
+
+      <param name="outputLidarOdometryTopic_" value="$(arg outputLidarOdometryTopic_)" type="string"/>
+      <param name="outputLidarOdometryFrameId" value="$(arg outputLidarOdometryFrameId)" type="string"/>
+
+      <param name="outputTransFromStampedTopic_" value="$(arg outputTransFromStampedTopic_)" type="string"/>
+      <param name="outputTransFromStampedFrameId" value="$(arg outputTransFromStampedFrameId)" type="string"/>
+
+      <param name="outputImuTopic_" value="$(arg outputImuTopic_)" type="string"/>
+      <param name="outputImuFrameId" value="$(arg outputImuFrameId)" type="string"/>
+
+      <param name="outputGpsPathTopic_" value="$(arg outputGpsPathTopic_)" type="string"/>
+      <param name="robotOdomFrameId" value="$(arg robotOdomFrameId)" type="string"/>
+
+      <param name="inputNmeaSentenceTopic_" value="$(arg inputNmeaSentenceTopic_)" type="string"/>
+      <param name="publishDynamicTf" value="$(arg publishDynamicTf)" type="bool"/>
+
+      <param name="orinLon" value="$(arg orinLon)" type="double"/>
+      <param name="orinLat" value="$(arg orinLat)" type="double"/>
+      <param name="orinAlt" value="$(arg orinAlt)" type="double"/>
+      <param name="e_bias" value="$(arg e_bias)" type="double"/>
+      <param name="n_bias" value="$(arg n_bias)" type="double"/>
+  </node>
+</launch>

sensors/beitian-bg620/launch/include/beitian_bg620_serial_reader.launch

@@ -0,0 +1,11 @@
+<launch>
+  <arg name="outPutSentenceTopic" default="/bg620/nmeaSentence"/>
+  <arg name="gpsComChannel" default="/dev/ttyACM0"/>
+  <arg name="gpsComBaudRate" default="115200"/>
+
+  <node name="bg620_serial_reader_node" pkg="beitian-bg620" type="bg620_serial_reader_node" output="screen">
+      <param name="outPutSentenceTopic" value="$(arg outPutSentenceTopic)" type="string"/>
+      <param name="gpsComChannel" value="$(arg gpsComChannel)" type="string"/>
+      <param name="gpsComBaudRate" value="$(arg gpsComBaudRate)" type="int"/>
+  </node>
+</launch>

sensors/beitian-bg620/package.xml

@@ -0,0 +1,97 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>beitian-bg620</name>
+  <version>0.0.0</version>
+  <description>The beitian_bg620 package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="buaazs17@163.com">zhangshu</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/newton-m2</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>nav_msgs</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>tf2</build_depend>
+  <build_depend>tf2_geometry_msgs</build_depend>
+  <build_depend>tf2_ros</build_depend>
+  <build_depend>nmea_msgs</build_depend>
+  <build_depend>serial</build_depend>
+
+
+  <build_export_depend>geometry_msgs</build_export_depend>
+  <build_export_depend>nav_msgs</build_export_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>sensor_msgs</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <build_export_depend>tf2</build_export_depend>
+  <build_export_depend>tf2_geometry_msgs</build_export_depend>
+  <build_export_depend>tf2_ros</build_export_depend>
+  <build_export_depend>nmea_msgs</build_export_depend>
+  <build_export_depend>serial</build_export_depend>
+
+
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>nav_msgs</exec_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>tf2</exec_depend>
+  <exec_depend>tf2_geometry_msgs</exec_depend>
+  <exec_depend>tf2_ros</exec_depend>
+  <exec_depend>nmea_msgs</exec_depend>
+  <exec_depend>serial</exec_depend>
+
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

sensors/beitian-bg620/src/bg620_sentence_parse_core.cpp

@@ -0,0 +1,381 @@
+#include "bg620_sentence_parse_core.h"
+
+BG620SentenceParse::BG620SentenceParse():nh_(""), private_nh_("~"), listener(buffer), current_speed_(0.0){
+    // 参数初始化
+    private_nh_.param<std::string>("outputNavSatFixTopic_", outputNavSatFixTopic_, "");
+    private_nh_.param<std::string>("outputNavSatFixFrameId", outputNavSatFixFrameId, "");
+
+    // [Debug] Print parameters
+    ROS_INFO("[BG620] Parameters loaded:");
+    ROS_INFO("outputNavSatFixTopic: %s", outputNavSatFixTopic_.c_str());
+    ROS_INFO("outputOdometryTopic: %s", outputOdometryTopic_.c_str());
+    ROS_INFO("inputNmeaSentenceTopic_: %s", inputNmeaSentenceTopic_.c_str());
+    
+    private_nh_.param<std::string>("outputOdometryTopic_", outputOdometryTopic_, "");
+    private_nh_.param<std::string>("outputOdometryFrameId", outputOdometryFrameId, "");
+
+    private_nh_.param<std::string>("outputLidarOdometryTopic_", outputLidarOdometryTopic_, "");
+    private_nh_.param<std::string>("outputLidarOdometryFrameId", outputLidarOdometryFrameId, "");
+
+    private_nh_.param<std::string>("outputTransFromStampedTopic_", outputTransFromStampedTopic_, "");
+    private_nh_.param<std::string>("outputTransFromStampedFrameId", outputTransFromStampedFrameId, "");
+
+    private_nh_.param<std::string>("outputImuTopic_", outputImuTopic_, "");
+    private_nh_.param<std::string>("outputImuFrameId", outputImuFrameId, "");
+
+    private_nh_.param<std::string>("outputGpsPathTopic_", outputGpsPathTopic_, "");
+    private_nh_.param<std::string>("robotOdomFrameId", robotOdomFrameId, "");
+    
+    private_nh_.param<std::string>("inputNmeaSentenceTopic_", inputNmeaSentenceTopic_, "");
+    private_nh_.param<bool>("publishDynamicTf", publishDynamicTf, false);
+    
+    private_nh_.param<double>("orinLon", orinLon, 0.0);
+    private_nh_.param<double>("orinLat", orinLat, 0.0);
+    private_nh_.param<double>("orinAlt", orinAlt, 0.0);
+    
+    private_nh_.param<double>("e_bias", e_bias, 0.0);
+    private_nh_.param<double>("n_bias", n_bias, 0.0);
+
+    // 初始化发布器
+    imuPublisher = nh_.advertise<sensor_msgs::Imu>(outputImuTopic_, 1);
+    navSatFixPublisher = nh_.advertise<sensor_msgs::NavSatFix>(outputNavSatFixTopic_, 1);
+    odometryPublisher = nh_.advertise<nav_msgs::Odometry>(outputOdometryTopic_, 1);
+    lidarOdometryPublisher = nh_.advertise<nav_msgs::Odometry>(outputLidarOdometryTopic_, 1);
+    transformStampedPublisher = nh_.advertise<geometry_msgs::TransformStamped>(outputTransFromStampedTopic_, 1);
+    gpsPathPublisher = nh_.advertise<nav_msgs::Path>(outputGpsPathTopic_, 1);
+
+    ROS_INFO("[BG620] Publishers initialized");
+
+    // 初始化订阅器
+    nmeaSentenceSubscriber = nh_.subscribe<nmea_msgs::Sentence>(inputNmeaSentenceTopic_, 1, &BG620SentenceParse::nmeaSentenceCallback, this);
+    // 初始化GPS工具
+    gpsTools.lla_origin_<<orinLat, orinLon, orinAlt;
+}
+
+BG620SentenceParse::~BG620SentenceParse(){
+}
+
+// 度分转换实现
+double BG620SentenceParse::dmToDeg(double dm, char direction) {
+    int degree = static_cast<int>(dm / 100);
+    double minutes = dm - degree * 100;
+    double decimal_deg = degree + minutes / 60.0;
+    return (direction == 'S' || direction == 'W') ? -decimal_deg : decimal_deg;
+}
+
+// UTC时间解析实现
+ros::Time BG620SentenceParse::parseUTCTime(const std::string& utc_time, const std::string& date) {
+    try {
+        // 如果没有提供日期，使用当前日期
+        std::string use_date = date.empty() ? "010100" : date; // 默认2000-01-01
+
+        int day = std::stoi(use_date.substr(0,2));
+        int month = std::stoi(use_date.substr(2,2));
+        int year = 2000 + std::stoi(use_date.substr(4,2));
+
+        int hours = std::stoi(utc_time.substr(0,2));
+        int minutes = std::stoi(utc_time.substr(2,2));
+        int seconds = std::stoi(utc_time.substr(4,2));
+        int nsec = 0;
+
+        if (utc_time.length() > 6 && utc_time[6] == '.') {
+            double frac_sec = std::stod("0." + utc_time.substr(7));
+            nsec = static_cast<int>(frac_sec * 1e9);
+        }
+
+        tm time_struct = {};
+        time_struct.tm_year = year - 1900;
+        time_struct.tm_mon = month - 1;
+        time_struct.tm_mday = day;
+        time_struct.tm_hour = hours;
+        time_struct.tm_min = minutes;
+        time_struct.tm_sec = seconds;
+        time_t time_utc = timegm(&time_struct);
+
+        return ros::Time(time_utc, nsec);
+    } catch (...) {
+        ROS_WARN("Failed to parse UTC time: %s", utc_time.c_str());
+        return ros::Time::now();
+    }
+}
+
+// 字符串分割实现
+std::vector<std::string> BG620SentenceParse::splitStr(std::string str, const std::string &pattern){
+    std::string::size_type pos;
+    std::vector<std::string> result;
+    str += pattern;
+    int size = str.size();
+    for (int i = 0; i < size; i++)
+    {
+        pos = str.find(pattern, i);
+        if (pos < size)
+        {
+            std::string s = str.substr(i, pos - i);
+            result.push_back(s);
+            i = pos + pattern.size() - 1;
+        }
+    }
+    return result;
+}
+
+// 核心回调函数
+void BG620SentenceParse::nmeaSentenceCallback(const nmea_msgs::Sentence::ConstPtr& msg){
+    try{
+//        ROS_DEBUG("[BG620] Received NMEA: %s", msg->sentence.c_str());
+        std::string raw_msg = msg->sentence;
+
+        // 统一分隔符处理
+        std::vector<std::string> fields = splitStr(raw_msg.substr(0, raw_msg.find("*")), ",");
+
+        if (fields.empty()) {
+            ROS_WARN("[BG620] Empty NMEA message");
+            return;
+        }
+
+        // GNGGA消息处理（第一条）
+        if (fields[0] == "$GNGGA") {
+//            ROS_DEBUG("[BG620] Processing GNGGA message");
+            bg620_data_.has_gga = true;
+            bg620_data_.stamp = msg->header.stamp;
+
+            // 解析经纬度（度分格式）
+            bg620_data_.nav_fix.latitude = dmToDeg(std::stod(fields[2]), fields[3][0]);
+            bg620_data_.nav_fix.longitude = dmToDeg(std::stod(fields[4]), fields[5][0]);
+            bg620_data_.nav_fix.altitude = std::stod(fields[9]);
+
+            // 设置状态
+            bg620_data_.nav_fix.status.service = sensor_msgs::NavSatStatus::SERVICE_GPS;
+            int gps_status = std::stoi(fields[6]);  // 先将字段转换为整数
+            bg620_data_.nav_fix.status.status = gps_status;
+            // ROS_INFO("[BG620] GPS - Lat: %.6f, Lon: %.6f, Alt: %.2f",
+            //        bg620_data_.nav_fix.latitude,
+            //        bg620_data_.nav_fix.longitude,
+            //        bg620_data_.nav_fix.altitude);
+        }
+//        // GNRMC消息处理（第二条）
+//        else if (fields[0] == "$GNRMC") {
+////            ROS_DEBUG("[BG620] Processing GNRMC message");
+//            bg620_data_.has_rmc = true;
+//            bg620_data_.speed = std::stod(fields[7]) * 0.514444; // 节→m/s
+//            bg620_data_.track_angle = std::stod(fields[8]);      // 真北航向角
+//
+//            ROS_INFO("[BG620] Speed - %.2f m/s, Track: %.2f°",
+//                   bg620_data_.speed,
+//                   bg620_data_.track_angle);
+//        }
+        // HEADINGA消息处理（第三条）
+        else if (fields[0] == "#HEADINGA") {
+//            ROS_DEBUG("[BG620] Processing HEADINGA message");
+            bg620_data_.has_heading = true;
+
+            // 解析姿态角（单位：度）
+            double yaw = std::stod(fields[11]);
+            double pitch = std::stod(fields[12]);
+            double roll = std::stod(fields[13]);
+            // acturally fields[12] is the heading angle of rtk
+            bg620_data_.heading_deg = pitch;
+
+            double yaw_rad = yaw * M_PI / 180.0;
+            double pitch_rad = pitch * M_PI / 180.0;
+            double roll_rad = roll * M_PI / 180.0;
+
+            // 转换为四元数
+            geometry_msgs::Quaternion q;
+            q = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
+            bg620_data_.orientation = q;
+
+            // 三条消息都收到后处理
+//            if (bg620_data_.has_gga && bg620_data_.has_rmc && bg620_data_.has_heading) {
+            if (bg620_data_.has_gga && bg620_data_.has_heading) {
+//                ROS_INFO("[BG620] All required messages received, processing data");
+                processBG620Data();
+
+                // 重置标志位
+                bg620_data_.has_gga = false;
+//                bg620_data_.has_rmc = false;
+                bg620_data_.has_heading = false;
+            }
+
+            // ROS_INFO("[BG620] Orien - Yaw: %.2f, Pitch: %.2f, Roll: %.2f",
+            //        std::stod(fields[11]),
+            //        std::stod(fields[12]),
+            //        std::stod(fields[13]));
+        }
+    } catch (std::exception& e) {
+        ROS_ERROR("NMEA parsing error [%s]: %s", __APP_NAME__, e.what());
+    }
+}
+
+// 新增的统一处理函数
+void BG620SentenceParse::processBG620Data() {
+    // 设置消息头
+    const ros::Time current_frame_time = ros::Time::now();
+    bg620_data_.nav_fix.header.stamp = current_frame_time;
+    bg620_data_.nav_fix.header.frame_id = outputNavSatFixFrameId;
+
+    // 发布NavSatFix
+//    ROS_INFO("[BG620] Publishing NavSatFix");
+    navSatFixPublisher.publish(bg620_data_.nav_fix);
+
+    // 发布里程计
+    nav_msgs::Odometry gpsOdometry;
+    gpsOdometry.header.frame_id = "map";
+    gpsOdometry.header.stamp = current_frame_time;
+    gpsOdometry.child_frame_id = outputOdometryFrameId;
+    // 坐标系转换
+    Eigen::Vector3d lla = gpsTools.GpsMsg2Eigen(bg620_data_.nav_fix);
+    Eigen::Vector3d ecef = gpsTools.LLA2ECEF(lla);
+    Eigen::Vector3d enu = gpsTools.ECEF2ENU(ecef);
+
+    // ROS_INFO("[BG620] ENU - E: %.2f, N: %.2f, U: %.2f",
+    //        enu[0], enu[1], enu[2]);
+
+    gpsOdometry.pose.pose.position.x = enu[0]+e_bias;
+    gpsOdometry.pose.pose.position.y = enu[1]+n_bias;
+    gpsOdometry.pose.pose.position.z = enu[2];
+
+    // ROS_INFO("E=%.4f, N=%.4f, U=%.4f", enu[0],enu[1],enu[2]);
+    gpsOdometry.pose.pose.orientation.x = bg620_data_.orientation.x;
+    gpsOdometry.pose.pose.orientation.y = bg620_data_.orientation.y;
+    gpsOdometry.pose.pose.orientation.z = bg620_data_.orientation.z;
+    gpsOdometry.pose.pose.orientation.w = bg620_data_.orientation.w;
+    // just a trick to transpose solution status and yaw_angle
+    gpsOdometry.pose.covariance[1] = bg620_data_.nav_fix.status.status;
+    gpsOdometry.pose.covariance[2] = bg620_data_.heading_deg;
+    // 设置协方差
+    switch (bg620_data_.nav_fix.status.status) {
+    case 1:  // 单点定位
+        gpsOdometry.pose.covariance[0] = 1.0;  // x (m^2)
+        gpsOdometry.pose.covariance[7] = 1.0;  // y (m^2)
+        gpsOdometry.pose.covariance[14] = 10.0;  // z (m^2)
+        break;
+    case 2:  // 差分定位
+        gpsOdometry.pose.covariance[0] = 0.5;  // x (m^2)
+        gpsOdometry.pose.covariance[7] = 0.5;  // y (m^2)
+        gpsOdometry.pose.covariance[14] = 5.0;  // z (m^2)
+        break;
+    case 4:  // RTK固定解
+        gpsOdometry.pose.covariance[0] = 0.01;  // x (m^2)
+        gpsOdometry.pose.covariance[7] = 0.01;  // y (m^2)
+        gpsOdometry.pose.covariance[14] = 0.1;  // z (m^2)
+        break;
+    case 5:  // RTK浮点解
+        gpsOdometry.pose.covariance[0] = 0.1;  // x (m^2)
+        gpsOdometry.pose.covariance[7] = 0.1;  // y (m^2)
+        gpsOdometry.pose.covariance[14] = 1.0;  // z (m^2)
+        break;
+    case 6:  // 惯导模式
+        gpsOdometry.pose.covariance[0] = 0.2;  // x (m^2)
+        gpsOdometry.pose.covariance[7] = 0.2;  // y (m^2)
+        gpsOdometry.pose.covariance[14] = 2.0;  // z (m^2)
+        break;
+    default:  // 其他状态 0,3,7,8 等（含无效状态）
+        gpsOdometry.pose.covariance[0] = 1000.0;
+        gpsOdometry.pose.covariance[7] = 1000.0;
+        gpsOdometry.pose.covariance[14] = 1000.0;
+}
+    // 暂时不发布里程计的速度信息
+//    ROS_INFO("[BG620] Publishing gpsOdometry");
+    odometryPublisher.publish(gpsOdometry);
+
+    /* 发布geometry_msgs::TransFromed,用于标定组合导航与激光雷达的外参 */
+    geometry_msgs::TransformStamped ts;
+    ts.header.stamp = current_frame_time;
+    ts.header.frame_id = "map";
+    ts.child_frame_id = outputTransFromStampedFrameId;
+    ts.transform.translation.x = enu[0]+e_bias;
+    ts.transform.translation.y = enu[1]+n_bias;
+    ts.transform.translation.z =  enu[2];
+    ts.transform.rotation.x = bg620_data_.orientation.x;
+    ts.transform.rotation.y = bg620_data_.orientation.y;
+    ts.transform.rotation.z = bg620_data_.orientation.z;
+    ts.transform.rotation.w = bg620_data_.orientation.w;
+//    ROS_INFO("[BG620] Publishing ts");
+    transformStampedPublisher.publish(ts);
+
+    // 发布Path 测试gps性能时用，部署时注释
+    gpsPath.header.frame_id = "map";
+    gpsPath.header.stamp = current_frame_time;
+    geometry_msgs::PoseStamped pose;
+    if(gpsPath.poses.size()>=600){
+        gpsPath.poses.clear();
+    }
+    pose.header = gpsPath.header;
+    pose.pose.position.x = enu[0]+e_bias;
+    pose.pose.position.y = enu[1]+n_bias;
+    pose.pose.position.z = enu[2];
+    pose.pose.orientation.x = bg620_data_.orientation.x;
+    pose.pose.orientation.y = bg620_data_.orientation.y;
+    pose.pose.orientation.z = bg620_data_.orientation.z;
+    pose.pose.orientation.w = bg620_data_.orientation.w;
+    gpsPath.poses.push_back(pose);
+//    ROS_INFO("[BG620] Publishing gpsPath");
+    gpsPathPublisher.publish(gpsPath);
+
+    Eigen::Affine3f transform;
+    Eigen::Matrix4f mat_pose;
+    geometry_msgs::TransformStamped map_wrt_frame;
+    geometry_msgs::TransformStamped frame_wrt_odom;
+    Eigen::Matrix4f frame2odom;
+    geometry_msgs::TransformStamped map_wrt_odom;
+    tf2::Transform odom_wrt_map;
+    geometry_msgs::TransformStamped odom_trans;
+
+    /* 生成4阶矩阵 */
+    transform = Eigen::Affine3f::Identity();
+    transform.translation() << enu[0]+e_bias, enu[1]+n_bias, enu[2];
+    Eigen::Quaternionf q(bg620_data_.orientation.w,
+                    bg620_data_.orientation.x,
+                    bg620_data_.orientation.y,
+                    bg620_data_.orientation.z);
+    transform.rotate(q);
+    mat_pose = transform.matrix();
+
+    map_wrt_frame = tf2::eigenToTransform(Eigen::Isometry3d(mat_pose.inverse().cast<double>()));
+    map_wrt_frame.header.stamp = current_frame_time;
+    map_wrt_frame.header.frame_id = outputOdometryFrameId;
+    map_wrt_frame.child_frame_id = "map";
+
+    /* 发布激光里程计处的ODOM*/
+    nav_msgs::Odometry lidar_odom;
+    lidar_odom.header.frame_id = "map";
+    lidar_odom.header.stamp = current_frame_time;
+    lidar_odom.child_frame_id = outputLidarOdometryFrameId;
+
+    frame_wrt_odom = buffer.lookupTransform(outputLidarOdometryFrameId, outputOdometryFrameId, ros::Time(0));
+    frame2odom = tf2::transformToEigen(frame_wrt_odom).cast<float>().matrix();
+
+    tf2::doTransform(map_wrt_frame, map_wrt_odom, frame_wrt_odom);
+
+    tf2::fromMsg(map_wrt_odom.transform, odom_wrt_map);
+    odom_wrt_map = odom_wrt_map.inverse();
+
+    odom_trans.transform = tf2::toMsg(odom_wrt_map);
+
+    lidar_odom.pose.pose.position.x = odom_trans.transform.translation.x;
+    lidar_odom.pose.pose.position.y = odom_trans.transform.translation.y;
+    lidar_odom.pose.pose.position.z = odom_trans.transform.translation.z;
+    lidar_odom.pose.pose.orientation.x = odom_trans.transform.rotation.x;
+    lidar_odom.pose.pose.orientation.y = odom_trans.transform.rotation.y;
+    lidar_odom.pose.pose.orientation.z = odom_trans.transform.rotation.z;
+    lidar_odom.pose.pose.orientation.w = odom_trans.transform.rotation.w;
+//    ROS_INFO("[BG620] PUblishing lidar_odom");
+    lidarOdometryPublisher.publish(lidar_odom);
+
+    // 发布动态TF
+    if(publishDynamicTf && bg620_data_.nav_fix.status.status == sensor_msgs::NavSatStatus::STATUS_GBAS_FIX){
+        frame_wrt_odom = buffer.lookupTransform(robotOdomFrameId, outputOdometryFrameId, ros::Time(0));
+        frame2odom = tf2::transformToEigen(frame_wrt_odom).cast<float>().matrix();
+
+        tf2::doTransform(map_wrt_frame, map_wrt_odom, frame_wrt_odom);
+
+        tf2::fromMsg(map_wrt_odom.transform, odom_wrt_map);
+        odom_wrt_map = odom_wrt_map.inverse();
+
+        odom_trans.transform = tf2::toMsg(odom_wrt_map);
+        odom_trans.header.stamp = current_frame_time;
+        odom_trans.header.frame_id = "map";
+        odom_trans.child_frame_id = robotOdomFrameId;
+        transformBroadcaster.sendTransform(odom_trans);
+    }
+}

sensors/beitian-bg620/src/bg620_sentence_parse_node.cpp

@@ -0,0 +1,9 @@
+#include "bg620_sentence_parse_core.h"
+
+int main(int argc, char **argv){
+    setlocale(LC_CTYPE, "zh_CN.utf8");
+    ros::init(argc, argv, "bg620_nmea_sentence_parse");
+    BG620SentenceParse bg620SentenceParse;
+    ros::spin();
+    return 0;
+}

sensors/beitian-bg620/src/bg620_serial_reader_core.cpp

@@ -0,0 +1,72 @@
+#include "bg620_serial_reader_core.h"
+
+#include <cstdlib>
+#include <cstring>
+#include <unistd.h>
+
+#include <libserial/SerialPort.h>
+
+RtkSerialReaderBG620::RtkSerialReaderBG620():nh_(""), private_nh_("~"){
+    private_nh_.param<std::string>("outPutSentenceTopic", outPutSentenceTopic, "");
+    private_nh_.param<std::string>("gpsComChannel", gpsComChannel, "");
+    private_nh_.param<int>("gpsComBaudRate", gpsComBaudRate, 115200);
+}
+
+RtkSerialReaderBG620::~RtkSerialReaderBG620(){
+}
+
+void RtkSerialReaderBG620::run(){
+    sentence_publisher = nh_.advertise<nmea_msgs::Sentence>(outPutSentenceTopic, 1);
+    LibSerial::SerialPort serial_port;
+    try {
+        serial_port.Open(gpsComChannel);
+        switch (gpsComBaudRate) {
+            case 9600:
+                serial_port.SetBaudRate(LibSerial::BaudRate::BAUD_9600);
+                break;
+            case 19200:
+                serial_port.SetBaudRate(LibSerial::BaudRate::BAUD_19200);
+                break;
+            case 38400:
+                serial_port.SetBaudRate(LibSerial::BaudRate::BAUD_38400);
+                break;
+            case 57600:
+                serial_port.SetBaudRate(LibSerial::BaudRate::BAUD_57600);
+                break;
+            case 115200:
+                serial_port.SetBaudRate(LibSerial::BaudRate::BAUD_115200);
+                break;
+            default:
+                serial_port.SetBaudRate(LibSerial::BaudRate::BAUD_115200);
+        }
+        serial_port.SetCharacterSize(LibSerial::CharacterSize::CHAR_SIZE_8);
+        serial_port.SetFlowControl(LibSerial::FlowControl::FLOW_CONTROL_NONE);
+        serial_port.SetParity(LibSerial::Parity::PARITY_NONE);
+        serial_port.SetStopBits(LibSerial::StopBits::STOP_BITS_1);
+    } catch (const LibSerial::OpenFailed &) {
+        ROS_ERROR("The serial port did not open correctly.");
+        return;
+    }
+    const size_t ms_timeout = 150;
+    const char line_terminator = '\n';
+    while(ros::ok()){
+        std::string recvMsg;
+        try {
+            serial_port.ReadLine(recvMsg, line_terminator, ms_timeout);
+        } catch (const LibSerial::ReadTimeout &) {
+            std::cerr << "\nThe ReadLine() call has timed out." << std::endl;
+        }
+        recvMsg.erase(std::remove_if(recvMsg.begin(), recvMsg.end(), ::isspace), recvMsg.end());
+        if(recvMsg.empty()){
+            continue;
+        }
+        nmea_msgs::Sentence sentence;
+        sentence.header.frame_id = "rtk";
+        sentence.header.stamp = ros::Time().now();
+        sentence.sentence = recvMsg;
+        sentence_publisher.publish(sentence);
+        ros::spinOnce();
+    }
+    serial_port.Close();
+    return;
+}

sensors/beitian-bg620/src/bg620_serial_reader_node.cpp

@@ -0,0 +1,9 @@
+#include "bg620_serial_reader_core.h"
+
+int main(int argc, char **argv){
+    setlocale(LC_CTYPE, "zh_CN.utf8");
+    ros::init(argc, argv, "rtk_bg620");
+    RtkSerialReaderBG620 rtkSerialReaderBG620;
+    rtkSerialReaderBG620.run();
+    return 0;
+}