Initial commit

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