Initial commit

sensors/newton-m2/CMakeLists.txt

@@ -0,0 +1,219 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(newton-m2)
+
+## 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
+  serial
+)
+
+## 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 newton-m2
+ 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(serial_reader_node src/serial_reader_node.cpp  src/serial_reader_core.cpp)
+target_link_libraries(serial_reader_node ${catkin_LIBRARIES})
+
+add_executable(m2_sentence_parse_node src/m2_sentence_parse_node.cpp src/m2_sentence_parse_core.cpp)
+target_link_libraries(m2_sentence_parse_node ${catkin_LIBRARIES})

sensors/newton-m2/README.txt

@@ -0,0 +1 @@
+该包为星网宇达组合导航驱动包。

sensors/newton-m2/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/newton-m2/include/m2_sentence_parse_core.h

@@ -0,0 +1,81 @@
+#ifndef M2_SENTENCE_PARSE_H
+#define M2_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"
+
+#define __APP_NAME__ "m2_sentence_parse"
+
+class M2SentenceParse{
+    public:
+        M2SentenceParse();
+        ~M2SentenceParse();
+
+    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 gphdpOdometryTopic_;
+        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 gphpdOdometryPublisher;
+        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);
+};
+
+#endif

sensors/newton-m2/include/serial_reader_core.h

@@ -0,0 +1,27 @@
+#ifndef RTK_SERIAL_READER_JIAKAO_H
+#define RTK_SERIAL_READER_JIAKAO_H
+
+#include "ros/ros.h"
+#include "serial/serial.h"
+#include "nmea_msgs/Sentence.h"
+
+#define __APP_NAME__ "rtk_serial_reader_jiakao"
+
+class RtkSerialReaderM2{
+    public:
+        RtkSerialReaderM2();
+        ~RtkSerialReaderM2();
+        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/newton-m2/launch/include/newton_m2_serial_reader.launch

@@ -0,0 +1,11 @@
+<launch>
+  <arg name="outPutSentenceTopic" default="/m2/nmeaSentence"/>
+  <arg name="gpsComChannel" default="/dev/ttyUSB0"/>
+  <arg name="gpsComBaudRate" default="115200"/>
+
+  <node name="serial_reader_node" pkg="newton-m2" type="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/newton-m2/launch/include/sentence_parse.launch

@@ -0,0 +1,66 @@
+<launch>
+    <arg name="outputNavSatFixTopic_" default="/m2/gpsFix"/>
+    <arg name="outputNavSatFixFrameId" default="gps"/>
+
+    <arg name="outputOdometryTopic_" default="/m2/gpsOdom"/>
+    <arg name="gphdpOdometryTopic_" default="/m2/gphpdOdom"/>
+    <arg name="outputOdometryFrameId" default="gps"/>
+
+    <arg name="outputLidarOdometryTopic_" default="/m2/lidarOdom"/>
+    <arg name="outputLidarOdometryFrameId" default="c32_L"/>
+
+    <arg name="outputTransFromStampedTopic_" default="/m2/gpsTransFromStamped"/>
+    <arg name="outputTransFromStampedFrameId" default="gps"/>
+
+    <arg name="outputImuTopic_" default="/m2/gpsImu"/>
+    <arg name="outputImuFrameId" default="gps"/>
+
+    <arg name="outputGpsPathTopic_" default="/m2/gpsPath"/>
+    <arg name="robotOdomFrameId" default="base_link"/>
+
+    <arg name="inputNmeaSentenceTopic_" default="/m2/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"/>
+
+    <arg name="e_bias" default="0.0"/>
+    <arg name="n_bias" default="0.0"/>
+
+    <node name="m2_sentence_parse_node" pkg="newton-m2" type="m2_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="gphdpOdometryTopic_" value="$(arg gphdpOdometryTopic_)" 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/newton-m2/launch/newton_m2_driver.launch

@@ -0,0 +1,4 @@
+<launch>
+    <include file="$(find newton-m2)/launch/include/newton_m2_serial_reader.launch" />
+    <include file="$(find newton-m2)/launch/include/sentence_parse.launch" />
+</launch>

sensors/newton-m2/package.xml

@@ -0,0 +1,97 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>newton-m2</name>
+  <version>0.0.0</version>
+  <description>The newton-m2 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/newton-m2/scripts/odom_repub.py

@@ -0,0 +1,39 @@
+import rospy
+from pynput import keyboard
+from threading import Thread
+from nav_msgs.msg import Odometry
+
+class Rebpublic:
+    def __init__(self):
+        self.is_pub = True
+        self.lidar_odom1_pub = rospy.Publisher("/m2/gpsOdom1", Odometry, queue_size=1)
+    
+    def on_press(self, key):
+        try:
+            if key.char == "q":
+                self.is_pub = not self.is_pub
+            print(self.is_pub)
+        except:
+            print("error")
+    
+    def odom_callback(self, data):
+        if self.is_pub:
+            self.lidar_odom1_pub.publish(data)
+            print("publish", rospy.get_time())
+        else:
+            print("not publish", rospy.get_time())
+
+    def key_listen(self):
+        with keyboard.Listener(on_press=self.on_press) as listener:
+            listener.join()
+
+    def listen(self):
+        rospy.Subscriber("/m2/gpsOdom", Odometry, self.odom_callback, queue_size=1)
+        rospy.spin()
+
+if __name__ == "__main__":
+    rospy.init_node("republic", anonymous=True)
+    rebpublic = Rebpublic()
+    thread_key_listern = Thread(target=rebpublic.key_listen, args=())
+    thread_key_listern.start()
+    rebpublic.listen()

sensors/newton-m2/scripts/set_m2.py

@@ -0,0 +1,201 @@
+# -*- coding: utf-8 -*-
+
+# 星网宇达组合导航配置
+# ntrip差分帐号配置
+# 输出配置
+# 杆臂配置
+# 航向补偿配置
+# ......
+import serial
+import time
+
+imu_serial = serial.Serial("/dev/ttyS0", 115200)
+
+# 恢复出厂配置
+factory_str = "$cmd,factory,reset*ff"
+imu_serial.write(factory_str.encode("utf-8"))
+time.sleep(0.5)
+
+save_str = "$cmd,save,config*ff"
+imu_serial.write(save_str.encode("utf-8"))
+time.sleep(0.5)
+
+
+# 配置gpfpd协议透传 50HZ
+gpfpd_str = "$cmd,output,com0,gpfpd,0.02*ff"
+imu_serial.write(gpfpd_str.encode("utf-8"))
+time.sleep(0.5)
+
+save_str = "$cmd,save,config*ff"
+imu_serial.write(save_str.encode("utf-8"))
+time.sleep(0.5)
+
+# 配置gphpd协议透传 5Hz
+gphpd_str = "$cmd,output,com0,gphpd,0.2*ff"
+imu_serial.write(gphpd_str.encode("utf-8"))
+time.sleep(0.5)
+
+save_str = "$cmd,save,config*ff"
+imu_serial.write(save_str.encode("utf-8"))
+time.sleep(0.5)
+
+# 配置gtimu输出 50HZ
+gpimu_str = "$cmd,output,com0,gtimu,0.02*ff"
+imu_serial.write(gpimu_str.encode("utf-8"))
+time.sleep(0.5)
+
+save_str = "$cmd,save,config*ff"
+imu_serial.write(save_str.encode("utf-8"))
+time.sleep(0.5)
+
+# 配置千寻查分账号
+# 设置组合导航ip地址　根据路由器网络来配置
+local_ip_str = "$cmd,set,localip,192,168,8,122*ff"
+imu_serial.write(local_ip_str.encode("utf-8"))
+time.sleep(0.5)
+
+save_str = "$cmd,save,config*ff"
+imu_serial.write(save_str.encode("utf-8"))
+time.sleep(0.5)
+
+# 设置子网掩码
+local_mask_str = "$cmd,set,localmask,255,255,255,0*ff"
+imu_serial.write(local_mask_str.encode("utf-8"))
+time.sleep(0.5)
+
+save_str = "$cmd,save,config*ff"
+imu_serial.write(save_str.encode("utf-8"))
+time.sleep(0.5)
+
+# 设置网关地址
+local_gate_str = "$cmd,set,localgate,192,168,8,1*ff"
+imu_serial.write(local_gate_str.encode("utf-8"))
+time.sleep(0.5)
+
+save_str = "$cmd,save,config*ff"
+imu_serial.write(save_str.encode("utf-8"))
+time.sleep(0.5)
+
+# 设置千寻账号网络服务器及端口
+netipport_str = "$cmd,set,netipport,120,253,226,97,8002*ff"
+# netipport_str = "$cmd,set,netipport,rtk,ntrip,qxwz,com,8002*ff"
+imu_serial.write(netipport_str.encode("utf-8"))
+time.sleep(0.5)
+
+save_str = "$cmd,save,config*ff"
+imu_serial.write(save_str.encode("utf-8"))
+time.sleep(0.5)
+
+# 设置千寻账号用户名和密码 user和password换成自己的账号和密码
+cors_account_str = "$cmd,set,netuser,cvmh5553:ap367r6um*ff"
+imu_serial.write(cors_account_str.encode("utf-8"))
+time.sleep(0.5)
+
+save_str = "$cmd,save,config*ff"
+imu_serial.write(save_str.encode("utf-8"))
+time.sleep(0.5)
+
+# 设置挂载点
+# mountpoint_str = "$cmd,set,mountpoint,RTCM32_GGB*ff"
+mountpoint_str = "$cmd,set,mountpoint,RTCM33_GRCEJ*ff"
+imu_serial.write(mountpoint_str.encode("utf-8"))
+time.sleep(0.5)
+
+save_str = "$cmd,save,config*ff"
+imu_serial.write(save_str.encode("utf-8"))
+time.sleep(0.5)
+
+
+# 激活 ntrip
+ntrip_str = "$cmd,set,ntrip,enable,enable*ff"
+imu_serial.write(ntrip_str.encode("utf-8"))
+time.sleep(0.5)
+
+save_str = "$cmd,save,config*ff"
+imu_serial.write(save_str.encode("utf-8"))
+time.sleep(0.5)
+
+# 航向补偿 左边为主天线
+heading_offset_str ="$cmd,set,headoffset,0*ff"
+imu_serial.write(heading_offset_str.encode("utf-8"))
+time.sleep(0.5)
+
+save_str = "$cmd,save,config*ff"
+imu_serial.write(save_str.encode("utf-8"))
+time.sleep(0.5)
+
+# 杆臂配置
+leverarm_str = "$cmd,set,leverarm,gnss,0.0,0.0,0.4*ff"
+imu_serial.write(leverarm_str.encode("utf-8"))
+time.sleep(0.5)
+
+save_str = "$cmd,save,config*ff"
+imu_serial.write(save_str.encode("utf-8"))
+time.sleep(0.5)
+
+
+# 同样，将导航模式一条一条输入
+# 精对准
+imu_serial.write("$cmd,set,navmode,FineAlign,off*ff".encode("utf-8"))
+time.sleep(0.5)
+
+save_str = "$cmd,save,config*ff"
+imu_serial.write(save_str.encode("utf-8"))
+time.sleep(0.5)
+
+# 粗对准
+imu_serial.write("$cmd,set,navmode,coarsealign,off*ff".encode("utf-8"))
+time.sleep(0.5)
+
+save_str = "$cmd,save,config*ff"
+imu_serial.write(save_str.encode("utf-8"))
+time.sleep(0.5)
+
+# 动态对准
+imu_serial.write("$cmd,set,navmode,dynamicalign,on*ff".encode("utf-8"))
+time.sleep(0.5)
+
+save_str = "$cmd,save,config*ff"
+imu_serial.write(save_str.encode("utf-8"))
+time.sleep(0.5)
+
+# gnss双精度
+imu_serial.write("$cmd,set,navmode,gnss,double*ff".encode("utf-8"))
+time.sleep(0.5)
+
+save_str = "$cmd,save,config*ff"
+imu_serial.write(save_str.encode("utf-8"))
+time.sleep(0.5)
+
+
+# 打开车载模式
+imu_serial.write("$cmd,set,navmode,carmode,on*ff".encode("utf-8"))
+time.sleep(0.5)
+
+save_str = "$cmd,save,config*ff"
+imu_serial.write(save_str.encode("utf-8"))
+time.sleep(0.5)
+
+# 打开零速校正
+imu_serial.write("$cmd,set,navmode,zupt,off*ff".encode("utf-8"))
+time.sleep(0.5)
+
+save_str = "$cmd,save,config*ff"
+imu_serial.write(save_str.encode("utf-8"))
+time.sleep(0.5)
+
+# 固件索引
+imu_serial.write("$cmd,set,navmode,firmwareindex,0*ff".encode("utf-8"))
+time.sleep(0.5)
+
+save_str = "$cmd,save,config*ff"
+imu_serial.write(save_str.encode("utf-8"))
+time.sleep(0.5)
+
+
+# 保存设置配置
+save_str = "$cmd,save,config*ff"
+imu_serial.write(save_str.encode("utf-8"))
+time.sleep(0.5)
+
+# 完事后重新上电

sensors/newton-m2/src/m2_sentence_parse_core.cpp

@@ -0,0 +1,318 @@
+#include "m2_sentence_parse_core.h"
+
+M2SentenceParse::M2SentenceParse():nh_(""), private_nh_("~"), listener(buffer){
+    private_nh_.param<std::string>("outputNavSatFixTopic_", outputNavSatFixTopic_, "");
+    private_nh_.param<std::string>("outputNavSatFixFrameId", outputNavSatFixFrameId, "");
+    
+    private_nh_.param<std::string>("outputOdometryTopic_", outputOdometryTopic_, "");
+    private_nh_.param<std::string>("gphdpOdometryTopic_", gphdpOdometryTopic_, "");
+    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);
+    gphpdOdometryPublisher = nh_.advertise<nav_msgs::Odometry>(gphdpOdometryTopic_, 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);
+    nmeaSentenceSubscriber = nh_.subscribe<nmea_msgs::Sentence>(inputNmeaSentenceTopic_, 1, &M2SentenceParse::nmeaSentenceCallback, this);
+    gpsTools.lla_origin_<<orinLat, orinLon, orinAlt;
+}
+
+M2SentenceParse::~M2SentenceParse(){
+}
+
+std::vector<std::string> M2SentenceParse::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 M2SentenceParse::nmeaSentenceCallback(const nmea_msgs::Sentence::ConstPtr& msg){
+    try{
+        std::string str = msg->sentence;
+        std::vector<std::string> result1 = splitStr(str, "*");
+        std::vector<std::string> result2 = splitStr(result1[0], ",");
+        // 解析GPFPD
+        if(result2[0]=="$GPFPD"){
+            // 航向对应Z轴
+            double yaw = -M_PI*std::stod(result2[3])/180.0;
+            // 俯仰对应Y轴
+            double pitch = -1.0*M_PI*std::stod(result2[4])/180.0;
+            // 滚转角对应X轴
+            double roll = -1.0*M_PI*std::stod(result2[5])/180.0;
+            // 纬度
+            double lat = std::stod(result2[6]);
+            // 经度
+            double lon = std::stod(result2[7]);
+            // 高度
+            double alt = std::stod(result2[8]);
+            // 东速
+            double ve = std::stod(result2[9]);
+            // 北速
+            double vn = std::stod(result2[10]);
+            // 天速
+            double vu = std::stod(result2[11]);
+            // 定位状态 4B表示进入固定解
+            std::string status = result2[15];
+            
+            // 发布navsatfix
+            sensor_msgs::NavSatFix navSatFix;
+            navSatFix.header.stamp = msg->header.stamp;
+            navSatFix.header.frame_id = outputNavSatFixFrameId;
+            // 表示由gps提供定位信息
+            navSatFix.status.service = 1;
+            if(status=="45"){
+                navSatFix.status.status = 1;
+            } else if (status == "4B") {
+                navSatFix.status.status = 2;
+            } else if (status == "55") {
+                navSatFix.status.status = 3;
+            } else if (status == "5B") {
+                navSatFix.status.status = 4;
+            } else {
+                navSatFix.status.status = 0;
+            }
+            navSatFix.latitude = lat;
+            navSatFix.longitude = lon;
+            navSatFix.altitude = alt;
+            navSatFix.position_covariance_type = 1;
+            navSatFixPublisher.publish(navSatFix);
+
+            geometry_msgs::Quaternion q;
+            // 可以只考虑yaw 组合导航的pitch实际对应着本体X轴, roll对应着本体Y轴
+            q = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
+
+            // 发布里程计
+            nav_msgs::Odometry gpsOdometry;
+            gpsOdometry.header.frame_id = "map";
+            gpsOdometry.header.stamp = msg->header.stamp;
+            gpsOdometry.child_frame_id = outputOdometryFrameId;
+            Eigen::Vector3d lla = gpsTools.GpsMsg2Eigen(navSatFix);
+            Eigen::Vector3d ecef = gpsTools.LLA2ECEF(lla);
+            Eigen::Vector3d enu = gpsTools.ECEF2ENU(ecef);
+            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 = q.x;
+            gpsOdometry.pose.pose.orientation.y = q.y;
+            gpsOdometry.pose.pose.orientation.z = q.z;
+            gpsOdometry.pose.pose.orientation.w = q.w;
+            // 融合建图用
+            if (status == "4B"){
+                gpsOdometry.pose.covariance[0] = 3.0;
+                gpsOdometry.pose.covariance[7] = 3.0;
+                gpsOdometry.pose.covariance[14] = 207.36;
+            }
+            else{
+                gpsOdometry.pose.covariance[0] = 20.0;
+                gpsOdometry.pose.covariance[7] = 20.0;
+                gpsOdometry.pose.covariance[14] = 207.36;
+            }
+            // just a trick, used to indicate the status of the GPS fix
+            gpsOdometry.pose.covariance[1] = navSatFix.status.status;
+            // 暂时不发布里程计的速度信息
+            odometryPublisher.publish(gpsOdometry);
+
+            /* 发布geometry_msgs::TransFromed,用于标定组合导航与激光雷达的外参 */
+            geometry_msgs::TransformStamped ts;
+            ts.header.stamp = ros::Time().now();
+            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 = q.x;
+            ts.transform.rotation.y = q.y;
+            ts.transform.rotation.z = q.z;
+            ts.transform.rotation.w = q.w;
+            transformStampedPublisher.publish(ts);
+
+            // 发布Path 测试gps性能时用，部署时注释
+            gpsPath.header.frame_id = "map";
+            gpsPath.header.stamp = ros::Time().now();
+            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 = q.x;
+            pose.pose.orientation.y = q.y;
+            pose.pose.orientation.z = q.z;
+            pose.pose.orientation.w = q.w;
+            gpsPath.poses.push_back(pose);
+            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];
+            transform.rotate(Eigen::AngleAxisf(roll, Eigen::Vector3f::UnitX()));
+            transform.rotate(Eigen::AngleAxisf(pitch, Eigen::Vector3f::UnitY()));
+            transform.rotate(Eigen::AngleAxisf(yaw, Eigen::Vector3f::UnitZ()));
+            mat_pose = transform.matrix();
+
+            map_wrt_frame = tf2::eigenToTransform(Eigen::Isometry3d(mat_pose.inverse().cast<double>()));
+            map_wrt_frame.header.stamp = ros::Time().now();
+            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 = ros::Time().now();
+            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;
+            lidarOdometryPublisher.publish(lidar_odom);
+            
+            // 发布动态TF
+            if(publishDynamicTf && status=="4B"){
+                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 = ros::Time().now();
+                odom_trans.header.frame_id = "map";
+                odom_trans.child_frame_id = robotOdomFrameId;
+                transformBroadcaster.sendTransform(odom_trans);
+            }
+        }
+
+        // 解析GPHPD
+        if(result2[0]=="$GPHPD"){
+            // 纬度
+            double lat = std::stod(result2[6]);
+            // 经度
+            double lon = std::stod(result2[7]);
+            // 高度
+            double alt = std::stod(result2[8]);
+            // 东速
+            double ve = std::stod(result2[9]);
+            // 北速
+            double vn = std::stod(result2[10]);
+            // 天速
+            double vu = std::stod(result2[11]);
+            // 定位状态 0B表示进入固定解
+            std::string status = result2[15];
+            int status_int = -1;
+            if (status == "0B") {
+                status_int = 2;
+            } else if (status == "03") {
+                status_int = 3;
+            } else if (status == "04") {
+                status_int = 4;
+            } else if (status == "05") {
+                status_int = 5;
+            }
+
+            nav_msgs::Odometry gphdOdometry;
+            gphdOdometry.header.frame_id = "map";
+            gphdOdometry.header.stamp = msg->header.stamp;
+            gphdOdometry.child_frame_id = outputOdometryFrameId;
+            Eigen::Vector3d lla(lat, lon, alt);
+            Eigen::Vector3d ecef = gpsTools.LLA2ECEF(lla);
+            Eigen::Vector3d enu = gpsTools.ECEF2ENU(ecef);
+            gphdOdometry.pose.pose.position.x = enu[0]+e_bias;
+            gphdOdometry.pose.pose.position.y = enu[1]+n_bias;
+            gphdOdometry.pose.pose.position.z = enu[2];
+            // just a trick, used to indicate the status of the rtk solution
+            gphdOdometry.pose.covariance[1] = status_int;
+            gphpdOdometryPublisher.publish(gphdOdometry);
+        }
+
+        if(result2[0]=="$GTIMU"){
+            // 度/s转rad/s
+            double gyroX = M_PI*std::stod(result2[3])/180.0;
+            double gyroY = M_PI*std::stod(result2[4])/180.0;
+            double gyroZ = M_PI*std::stod(result2[5])/180.0;
+            
+            double accX = std::stod(result2[6])*9.80511;
+            double accY = std::stod(result2[7])*9.80511;
+            double accZ = std::stod(result2[8])*9.80511;
+            
+            sensor_msgs::Imu imu;
+            imu.header.frame_id = outputImuFrameId;
+            imu.header.stamp = ros::Time().now();
+            imu.angular_velocity.x = gyroX;
+            imu.angular_velocity.y = gyroY;
+            imu.angular_velocity.z = gyroZ;
+            imu.linear_acceleration.x = accX;
+            imu.linear_acceleration.y = accY;
+            imu.linear_acceleration.z = accZ;
+            imuPublisher.publish(imu);
+        }
+    }
+    catch(std::exception& ex){
+        ROS_WARN("[%s] parse sentense error, %s", __APP_NAME__, ex.what());
+    }
+
+}

sensors/newton-m2/src/m2_sentence_parse_node.cpp

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

sensors/newton-m2/src/serial_reader_core.cpp

@@ -0,0 +1,38 @@
+#include "serial_reader_core.h"
+
+RtkSerialReaderM2::RtkSerialReaderM2():nh_(""), private_nh_("~"){
+    private_nh_.param<std::string>("outPutSentenceTopic", outPutSentenceTopic, "");
+    private_nh_.param<std::string>("gpsComChannel", gpsComChannel, "");
+    private_nh_.param<int>("gpsComBaudRate", gpsComBaudRate, 115200);
+}
+
+RtkSerialReaderM2::~RtkSerialReaderM2(){
+}
+
+void RtkSerialReaderM2::run(){
+    sentence_publisher = nh_.advertise<nmea_msgs::Sentence>(outPutSentenceTopic, 1);
+    serial::Serial ser;
+    try { 
+        ser.setPort(gpsComChannel); 
+        ser.setBaudrate(gpsComBaudRate); 
+        serial::Timeout timeOut = serial::Timeout::simpleTimeout(1000); 
+        ser.setTimeout(timeOut); 
+        ser.open(); 
+    }catch (serial::IOException& e) { 
+        ROS_ERROR_STREAM("Unable to open port"); 
+        return; 
+    }
+
+    while(ros::ok()){
+        std::string recvMsg = ser.readline();
+        recvMsg.erase(std::remove_if(recvMsg.begin(), recvMsg.end(), ::isspace), recvMsg.end());
+        nmea_msgs::Sentence sentence;
+        sentence.header.frame_id = "";
+        sentence.header.stamp = ros::Time().now();
+        sentence.sentence = recvMsg;
+        sentence_publisher.publish(sentence);
+        ros::spinOnce();
+    }
+    ser.close();
+    return;
+}

sensors/newton-m2/src/serial_reader_node.cpp

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