Merge branch 'master' of http://47.96.250.93:3000/adc_pilot/modularization

adciv.pro

@@ -10,3 +10,8 @@ SUBDIRS += \
     $$PWD/src/common/xmlparam \
     $$PWD/src/driver/driver_lidar_rs16 \
     $$PWD/src/tool/view_gps \
+    $$PWD/src/driver/driver_cloud_client \
+    $$PWD/src/driver/driver_cloud_grpc_client \
+    $$PWD/src/driver/driver_lidar_leishen16   \
+
+

src/decition/decition_brain/decition/adc_math/coordinate_conversion.h

@@ -1,122 +0,0 @@
-/****************************************
- * Functions:  Convert coordinates between GPS (longitude, latitude) system and UTM system.
- * Purpose:    Vehicle localization
- * Author:     Zhao Liang
- * Last Updated:  2021/01/20
- * Update note:  call third-party lib 'proj4' to do the conversionseee.
-*****************************************
-*
-* Convention: East is the X-axis, North is the Y-axis, Up (sky) is the Z-axis,
-* the pair (longitude, latitude) are in degrees: longitude ~ (-180, 180) and
-* latitude ~(-90, 90).
-*
-* Online tool: https://www.latlong.net/lat-long-utm.html
-*
-* Basically the convertion procedure can be splitted into three steps:
-* 1. Declare two `projPJ` objects for the source system and the target system.
-* 2. Put your projection params in two strings.
-* 3. Call `pj_transform` to do the conversion.
-*
-* String used by Apollo:
-*
-* "+proj=longlat +ellps=GRS80 +towgs84=0,0,0,0,0,0,0 +no_defs"
-*
-* 1. +proj=longlat means we are projecting from (longitude, latitude).
-* 2. +ellps=GRS80 means the ellipsoid model is GRS80.
-* 3. +towgs84=0,0,0,0,0,0,0 gives the 7 params for datum transformation.
-* 4. +no_defs means we don't want proj to read the default config file,
-* this option is obsolete since proj 6.x
-*/
-
-#ifndef COORDINATE_CONVERSION_H
-#define COORDINATE_CONVERSION_H
-
-#include <string>
-#include <proj_api.h>
-
-namespace iv {
-namespace math {
-
-/**
- * @brief: Convert (lon, lat) coordinates to UTM coordinates.
- * @param: longitude in degrees, range ~ (-180, 180).
- * @param: latitude in degrees, range ~ (-90, 90).
- * @param: x coordinate (East direction).
- * @param: y coordinate (North direction).
- * @return: Return true if the conversion between the two systems is valid.
-*/
-bool LatLonToUtmXY(double longitude, double latitude, double &x, double &y)
-{
-    projPJ pj_latlon;
-    projPJ pj_utm;
-    int zone = static_cast<int>((longitude + 180) / 6) + 1;
-    std::string latlon_src =
-        "+proj=longlat +ellps=GRS80 +towgs84=0,0,0,0,0,0,0 +no_defs";
-    std::string utm_dst =
-        "+proj=utm +zone=" + std::to_string(zone) + " +ellps=GRS80 +units=m +no_defs";
-
-    if (!(pj_latlon = pj_init_plus(latlon_src.c_str())))
-    {
-        return false;
-    }
-
-    if (!(pj_utm = pj_init_plus(utm_dst.c_str())))
-    {
-        return false;
-    }
-
-    // the pj_transform requires the (lon, lat) are in radians
-    double lon = longitude * DEG_TO_RAD;
-    double lat = latitude * DEG_TO_RAD;
-
-    // do the actual conversion
-    pj_transform(pj_latlon, pj_utm, 1, 1, &lon, &lat, nullptr);
-
-    x = lon;
-    y = lat;
-    pj_free(pj_latlon);
-    pj_free(pj_utm);
-    return true;
-}
-
-/**
- * @brief: Convert UTM coordinates to (lon, lat) coordinates.
- * @param: x coordinate (East direction).
- * @param: y coordinate (North direction).
- * @param: zone number.
- * @param: longitude in degrees, range ~ (-180, 180).
- * @param: latitude in degrees, range ~ (-90, 90).
- * @return: Return true if the conversion between the two systems is valid.
-*/
-bool UtmXYToLatLon(double x, double y, int zone, double &longitude, double &latitude)
-{
-    projPJ pj_latlon;
-    projPJ pj_utm;
-    std::string latlon_src =
-        "+proj=longlat +ellps=GRS80 +towgs84=0,0,0,0,0,0,0 +no_defs";
-    std::string utm_dst =
-        "+proj=utm +zone=" + std::to_string(zone) + " +ellps=GRS80 +units=m +no_defs";
-
-    if (!(pj_latlon = pj_init_plus(latlon_src.c_str())))
-    {
-        return false;
-    }
-
-    if (!(pj_utm = pj_init_plus(utm_dst.c_str())))
-    {
-        return false;
-    }
-
-    // do the actual conversion
-    pj_transform(pj_utm, pj_latlon, 1, 1, &x, &y, nullptr);
-
-    // the result given by pj_transform are in radians
-    longitude = x * RAD_TO_DEG;
-    latitude = y * RAD_TO_DEG;
-    pj_free(pj_latlon);
-    pj_free(pj_utm);
-    return true;
-}
-}
-}
-#endif // COORDINATE_CONVERSION_H

src/decition/decition_brain/decition/adc_math/core/include/splines/PolynomialXd.h

@@ -0,0 +1,130 @@
+#ifndef POLYNOMIALXD_H
+#define POLYNOMIALXD_H
+
+#pragma once
+
+#include <vector>
+
+#include "absl/strings/str_cat.h"
+#include "absl/strings/str_join.h"
+
+namespace adtoolbox {
+namespace core {
+
+class PolynomialXd
+{
+public:
+    PolynomialXd() = default;
+    explicit PolynomialXd(const size_t order);
+    explicit PolynomialXd(const std::vector<double> &params);
+    double operator()(const double value) const;
+    double operator[](const size_t index) const;
+    void setParams(const std::vector<double> &params);
+
+    static PolynomialXd derivedFrom(const PolynomialXd &other);
+    static PolynomialXd integratedFrom(const PolynomialXd &other, const double c);
+
+    size_t order() const;
+    const std::vector<double> &params() const;
+    std::string toString() const;
+
+private:
+    std::vector<double> params_;
+};
+
+// ----------------------------------------
+
+PolynomialXd::PolynomialXd(const size_t order)
+    : params_(order + 1, 0.0) {}
+
+// ----------------------------------------
+
+PolynomialXd::PolynomialXd(const std::vector<double> &params)
+    : params_(params) {}
+
+// ----------------------------------------
+
+size_t PolynomialXd::order() const
+{
+    return params_.size() - 1;
+}
+
+// ----------------------------------------
+
+void PolynomialXd::setParams(const std::vector<double> &params)
+{
+    params_ = params;
+}
+
+// ----------------------------------------
+
+const std::vector<double> &PolynomialXd::params() const
+{
+    return params_;
+}
+
+// ----------------------------------------
+
+double PolynomialXd::operator()(const double value) const
+{
+    double result = 0.0;
+    for (auto rit = params_.rbegin(); rit != params_.rend(); ++rit)
+    {
+        result = result * value + (*rit);
+    }
+    return result;
+}
+
+// ----------------------------------------
+
+double PolynomialXd::operator[](const size_t index) const
+{
+    return index >= params_.size() ? 0.0 : params_[index];
+}
+
+// ----------------------------------------
+
+PolynomialXd PolynomialXd::derivedFrom(const PolynomialXd &other)
+{
+    std::vector<double> params;
+    if (other.order() <= 0)
+    {
+        params.clear();
+    }
+    else
+    {
+        params.resize(other.params().size() - 1);
+        for (size_t i = 1; i < other.order() + 1; i++)
+        {
+            params[i - 1] = other[i] * i;
+        }
+    }
+    return PolynomialXd(params);
+}
+
+// ----------------------------------------
+
+PolynomialXd PolynomialXd::integratedFrom(const PolynomialXd &other, const double c)
+{
+    std::vector<double> params;
+    params.resize(other.params().size() + 1);
+    params[0] = c;
+    for (size_t i = 0; i < other.params().size(); i++)
+    {
+        params[i + 1] = other[i] / (i + 1);
+    }
+    return PolynomialXd(params);
+}
+
+// ----------------------------------------
+
+std::string PolynomialXd::toString() const
+{
+    return absl::StrCat("PolynomailXd(", absl::StrJoin(params_, ", "), ")");
+}
+// ----------------------------------------
+// Ends class PolynomialXd
+}
+}
+
+#endif // POLYNOMIALXD_H

src/decition/decition_brain/decition/adc_math/quartic_polynomail.cpp → src/decition/decition_brain/decition/adc_math/core/include/splines/QuarticPolynomial.h

@@ -1,9 +1,47 @@
-#include "quartic_polynomial.h"
+#ifndef QUARTICPOLYNOMIAL_H
+#define QUARTICPOLYNOMIAL_H
+
+#pragma once
+
+#include <array>
 #include "absl/strings/str_cat.h"
 #include "absl/strings/str_join.h"
 
-namespace iv {
-namespace math {
+namespace adtoolbox {
+namespace core {
+
+class QuarticPolynomial
+{
+public:
+    QuarticPolynomial() = default;
+    virtual ~QuarticPolynomial() = default;
+
+    QuarticPolynomial(const double x0, const double dx0, const double ddx0,
+                      const double dx1, const double ddx1,
+                      const double t);
+
+    QuarticPolynomial(const std::array<double, 3> &start,
+                      const std::array<double, 2> &end,
+                      const double t);
+
+    double evaluate(const size_t order, const double t) const;
+    size_t order() const;
+    double paramLength() const;
+    double coef(const size_t order) const;
+    std::string toString() const;
+
+private:
+    void computeCoefficients(const double x0, const double dx0, const double ddx0,
+                             const double dx1, const double ddx1,
+                             const double t);
+
+    double t_;
+    std::array<double, 3> start_condition_;
+    std::array<double, 2> end_condition_;
+    std::array<double, 5> coef_;
+};
+
+// ----------------------------------------
 
 QuarticPolynomial::QuarticPolynomial(const double x0, const double dx0, const double ddx0,
                                      const double dx1, const double ddx1,
@@ -18,30 +56,15 @@ QuarticPolynomial::QuarticPolynomial(const double x0, const double dx0, const do
     computeCoefficients(x0, dx0, ddx0, dx1, ddx1, t);
 }
 
+// ----------------------------------------
+
 QuarticPolynomial::QuarticPolynomial(const std::array<double, 3> &start,
                                      const std::array<double, 2> &end,
                                      const double t)
     : QuarticPolynomial::QuarticPolynomial(start[0], start[1], start[2],
                                            end[0], end[1], t) {}
 
-void QuarticPolynomial::computeCoefficients(const double x0, const double dx0, const double ddx0,
-                                            const double dx1, const double ddx1,
-                                            const double t)
-{
-    coef_[0] = x0;
-    coef_[1] = dx0;
-    coef_[2] = ddx0 * 0.5;
-
-    double b0 = dx1 - ddx0 * t - dx0;
-    double b1 = ddx1 - ddx0;
-    double t2 = t * t;
-    double t3 = t2 * t;
-
-    coef_[3] = (3 * b0 - b1 * t) / (3 * t2);
-    coef_[4] = (-2 * b0 + b1 * t) / (4 * t3);
-}
-
-double QuarticPolynomial::evaluate(const std::uint32_t order, const double p) const
+double QuarticPolynomial::evaluate(const size_t order, const double p) const
 {
     switch (order)
     {
@@ -60,19 +83,45 @@ double QuarticPolynomial::evaluate(const std::uint32_t order, const double p) co
     }
 }
 
-double QuarticPolynomial::coef(const size_t order) const
-{
-    return coef_[order];
-}
+// ----------------------------------------
+
+size_t QuarticPolynomial::order() const { return 4; }
+
+// ----------------------------------------
+
+double QuarticPolynomial::coef(const size_t order) const { return coef_[order];}
+
+// ----------------------------------------
 
-const std::array<double, 5> &QuarticPolynomial::coef() const
+double QuarticPolynomial::paramLength() const { return t_; }
+
+// ----------------------------------------
+
+std::string QuarticPolynomial::toString() const
 {
-    return coef_;
+    return absl::StrCat("QuarticPoly(", absl::StrJoin(coef_, ", "), ")");
 }
 
-std::string QuarticPolynomial::toString() const
+// ----------------------------------------
+
+void QuarticPolynomial::computeCoefficients(const double x0, const double dx0, const double ddx0,
+                                            const double dx1, const double ddx1,
+                                            const double t)
 {
-    return absl::StrCat("QuarticPolynomial(", absl::StrJoin(coef_, ", "), ")");
+    coef_[0] = x0;
+    coef_[1] = dx0;
+    coef_[2] = ddx0 * 0.5;
+
+    double b0 = dx1 - ddx0 * t - dx0;
+    double b1 = ddx1 - ddx0;
+    double t2 = t * t;
+    double t3 = t2 * t;
+
+    coef_[3] = (3 * b0 - b1 * t) / (3 * t2);
+    coef_[4] = (-2 * b0 + b1 * t) / (4 * t3);
 }
+// ----------------------------------------
+// Ends class QuarticPolynomial
 }
 }
+#endif // QUARTICPOLYNOMIAL_H

src/decition/decition_brain/decition/adc_math/quintic_polynomial.cpp → src/decition/decition_brain/decition/adc_math/core/include/splines/QuinticPolynomial.h

@@ -1,9 +1,51 @@
-#include "quintic_polynomial.h"
+#ifndef QUINTICPOLYNOMIAL_H
+#define QUINTICPOLYNOMIAL_H
+
+#pragma once
+
+#include <array>
 #include "absl/strings/str_cat.h"
 #include "absl/strings/str_join.h"
 
-namespace iv {
-namespace math {
+namespace adtoolbox {
+namespace core {
+
+class QuinticPolynomial
+{
+public:
+    QuinticPolynomial() = default;
+    virtual ~QuinticPolynomial() = default;
+
+    QuinticPolynomial(const double x0, const double dx0, const double ddx0,
+                      const double x1, const double dx1, const double ddx1,
+                      const double t);
+
+    QuinticPolynomial(const std::array<double, 3> &start,
+                      const std::array<double, 3> &end,
+                      const double t);
+
+    void fitBoundaryConditions(const double x0, const double dx0, const double ddx0,
+                               const double x1, const double dx1, const double ddx1,
+                               const double t);
+
+    double evaluate(const size_t order, const double t) const;
+    size_t order() const;
+    double paramLength() const;
+    double coef(const size_t order) const;
+    std::string toString() const;
+
+private:
+    void computeCoefficients(const double x0, const double dx0, const double ddx0,
+                             const double x1, const double dx1, const double ddx1,
+                             const double t);
+
+    double t_;
+    std::array<double, 3> start_condition_;
+    std::array<double, 3> end_condition_;
+    std::array<double, 6> coef_;
+};
+
+// ----------------------------------------
 
 QuinticPolynomial::QuinticPolynomial(const double x0, const double dx0, const double ddx0,
                                      const double x1, const double dx1, const double ddx1,
@@ -19,13 +61,31 @@ QuinticPolynomial::QuinticPolynomial(const double x0, const double dx0, const do
     computeCoefficients(x0, dx0, ddx0, x1, dx1, ddx1, t);
 }
 
+// ----------------------------------------
+
 QuinticPolynomial::QuinticPolynomial(const std::array<double, 3> &start,
                                      const std::array<double, 3> &end,
                                      const double t)
     : QuinticPolynomial::QuinticPolynomial(start[0], start[1], start[2],
                                            end[0], end[1], end[2], t) {}
 
-double QuinticPolynomial::evaluate(std::uint32_t order, const double p) const
+void QuinticPolynomial::fitBoundaryConditions(const double x0, const double dx0, const double ddx0,
+                                              const double x1, const double dx1, const double ddx1,
+                                              const double t)
+{
+    t_ = t;
+    start_condition_[0] = x0;
+    start_condition_[1] = dx0;
+    start_condition_[2] = ddx0;
+    end_condition_[0] = x1;
+    end_condition_[1] = dx1;
+    end_condition_[2] = ddx1;
+    computeCoefficients(x0, dx0, ddx0, x1, dx1, ddx1, t);
+}
+
+// ----------------------------------------
+
+double QuinticPolynomial::evaluate(const size_t order, const double p) const
 {
     switch (order)
     {
@@ -46,30 +106,36 @@ double QuinticPolynomial::evaluate(std::uint32_t order, const double p) const
     }
 }
 
-void QuinticPolynomial::fitByBoundaryConditions(const double x0, const double dx0, const double ddx0,
-                                                const double x1, const double dx1, const double ddx1,
-                                                const double t)
+// ----------------------------------------
+
+size_t QuinticPolynomial::order() const
 {
-    t_ = t;
-    start_condition_[0] = x0;
-    start_condition_[1] = dx0;
-    start_condition_[2] = ddx0;
-    end_condition_[0] = x1;
-    end_condition_[1] = dx1;
-    end_condition_[2] = ddx1;
-    computeCoefficients(x0, dx0, ddx0, x1, dx1, ddx1, t);
+    return 5;
 }
 
+// ----------------------------------------
+
 double QuinticPolynomial::coef(const size_t order) const
 {
     return coef_[order];
 }
 
-const std::array<double, 6> &QuinticPolynomial::coef() const
+// ----------------------------------------
+
+double QuinticPolynomial::paramLength() const
+{
+    return t_;
+}
+
+// ----------------------------------------
+
+std::string QuinticPolynomial::toString() const
 {
-    return coef_;
+    return absl::StrCat("QuinticPoly(", absl::StrJoin(coef_, ", "), ")");
 }
 
+// ----------------------------------------
+
 void QuinticPolynomial::computeCoefficients(const double x0, const double dx0, const double ddx0,
                                             const double x1, const double dx1, const double ddx1,
                                             const double t)
@@ -89,10 +155,8 @@ void QuinticPolynomial::computeCoefficients(const double x0, const double dx0, c
     coef_[4] = (-15.0 * c0 + 7.0 * c1 - c2) / t;
     coef_[5] = (6.0 * c0 - 3.0 * c1 + 0.5 * c2) / t2;
 }
-
-std::string QuinticPolynomial::toString() const
-{
-    return absl::StrCat("QuinticPolynomial(", absl::StrJoin(coef_, ", "), ")");
-}
+// ----------------------------------------
+// Ends class QuinticPolynomial
 }
 }
+#endif // QUINTICPOLYNOMIAL_H

src/decition/decition_brain/decition/adc_math/core/include/splines/Spline1dSegment.h

@@ -0,0 +1,120 @@
+#ifndef SPLINE1DSEGMENT_H
+#define SPLINE1DSEGMENT_H
+
+#pragma once
+
+#include <vector>
+#include <eigen3/Eigen/Core>
+
+#include "core/include/splines/PolynomialXd.h"
+
+namespace adtoolbox {
+namespace core {
+
+class Spline1dSegment
+{
+public:
+    explicit Spline1dSegment(const size_t order);
+    explicit Spline1dSegment(const std::vector<double> &params);
+    ~Spline1dSegment() = default;
+
+    void setParams(const std::vector<double> &params);
+    double operator()(const double x) const;
+    double derivative(const double x) const;
+    double secondOrderDerivative(const double x) const;
+    double thirdOrderDerivative(const double x) const;
+
+    const PolynomialXd &splineFunc() const;
+    const PolynomialXd &derivativeFunc() const;
+    const PolynomialXd &secondOrderDerivativeFunc() const;
+    const PolynomialXd &thirdOrderDerivativeFunc() const;
+
+private:
+    void setSplineFunc(const PolynomialXd &spline_func);
+
+    PolynomialXd f_;
+    PolynomialXd df_;
+    PolynomialXd ddf_;
+    PolynomialXd dddf_;
+};
+
+// ----------------------------------------
+
+Spline1dSegment::Spline1dSegment(const size_t order)
+{
+    setSplineFunc(PolynomialXd(order));
+}
+
+// ----------------------------------------
+
+Spline1dSegment::Spline1dSegment(const std::vector<double> &params)
+{
+    setSplineFunc(PolynomialXd(params));
+}
+
+// ----------------------------------------
+
+void Spline1dSegment::setParams(const std::vector<double> &params)
+{
+    setSplineFunc(PolynomialXd(params));
+}
+
+// ----------------------------------------
+
+void Spline1dSegment::setSplineFunc(const PolynomialXd &spline_func)
+{
+    f_ = spline_func;
+    df_ = PolynomialXd::derivedFrom(f_);
+    ddf_ = PolynomialXd::derivedFrom(df_);
+    dddf_ = PolynomialXd::derivedFrom(ddf_);
+}
+
+// ----------------------------------------
+
+double Spline1dSegment::operator()(const double x) const
+{
+    return f_(x);
+}
+
+double Spline1dSegment::derivative(const double x) const
+{
+    return df_(x);
+}
+
+double Spline1dSegment::secondOrderDerivative(const double x) const
+{
+    return ddf_(x);
+}
+
+double Spline1dSegment::thirdOrderDerivative(const double x) const
+{
+    return dddf_(x);
+}
+
+// ----------------------------------------
+
+const PolynomialXd& Spline1dSegment::splineFunc() const
+{
+    return f_;
+}
+
+const PolynomialXd& Spline1dSegment::derivativeFunc() const
+{
+    return df_;
+}
+
+const PolynomialXd& Spline1dSegment::secondOrderDerivativeFunc() const
+{
+    return ddf_;
+}
+
+const PolynomialXd& Spline1dSegment::thirdOrderDerivativeFunc() const
+{
+    return dddf_;
+}
+// ----------------------------------------
+// Ends class Spline1dSegment
+}
+}
+
+#endif // SPLINE1DSEGMENT_H

src/decition/decition_brain/decition/adc_math/core/include/splines/Spline2dSegment.h

@@ -0,0 +1,133 @@
+#ifndef SPLINE2DSEGMENT_H
+#define SPLINE2DSEGMENT_H
+
+#pragma once
+
+#include <vector>
+#include <eigen3/Eigen/Core>
+
+#include "core/include/splines/PolynomialXd.h"
+
+namespace adtoolbox {
+namespace core {
+
+class Spline2dSegment
+{
+public:
+    explicit Spline2dSegment(const size_t order);
+    Spline2dSegment(const std::vector<double> &x_param,
+                    const std::vector<double> &y_param);
+    ~Spline2dSegment() = default;
+
+    bool setParams(const std::vector<double> &x_param,
+                   const std::vector<double> &y_param);
+
+    std::pair<double, double> operator()(const double t) const;
+    double x(const double t) const;
+    double y(const double t) const;
+    double derivativeX(const double t) const;
+    double derivativeY(const double t) const;
+    double secondOrderDerivativeX(const double t) const;
+    double secondOrderDerivativeY(const double t) const;
+    double thirdOrderDerivativeX(const double t) const;
+    double thirdOrderDerivativeY(const double t) const;
+
+    const PolynomialXd& splineFuncX() const;
+    const PolynomialXd& splineFuncY() const;
+    const PolynomialXd& derivativeFuncX() const;
+    const PolynomialXd& derivativeFuncY() const;
+    const PolynomialXd& secondOrderDerivativeFuncX() const;
+    const PolynomialXd& secondOrderDerivativeFuncY() const;
+    const PolynomialXd& thirdOrderDerivativeFuncX() const;
+    const PolynomialXd& thirdOrderDerivativeFuncY() const;
+
+private:
+    PolynomialXd fx_;
+    PolynomialXd fy_;
+    PolynomialXd dfx_;
+    PolynomialXd dfy_;
+    PolynomialXd ddfx_;
+    PolynomialXd ddfy_;
+    PolynomialXd dddfx_;
+    PolynomialXd dddfy_;
+};
+
+// ----------------------------------------
+
+Spline2dSegment::Spline2dSegment(const size_t order)
+    : fx_(order), fy_(order)
+{
+    dfx_ = PolynomialXd::derivedFrom(fx_);
+    dfy_ = PolynomialXd::derivedFrom(fy_);
+    ddfx_ = PolynomialXd::derivedFrom(dfx_);
+    ddfy_ = PolynomialXd::derivedFrom(dfy_);
+    dddfx_ = PolynomialXd::derivedFrom(ddfx_);
+    dddfy_ = PolynomialXd::derivedFrom(ddfy_);
+}
+
+// ----------------------------------------
+
+Spline2dSegment::Spline2dSegment(const std::vector<double>& x_param,
+                                 const std::vector<double>& y_param)
+    : fx_(x_param), fy_(y_param)
+{
+    dfx_ = PolynomialXd::derivedFrom(fx_);
+    dfy_ = PolynomialXd::derivedFrom(fy_);
+    ddfx_ = PolynomialXd::derivedFrom(dfx_);
+    ddfy_ = PolynomialXd::derivedFrom(dfy_);
+    dddfx_ = PolynomialXd::derivedFrom(ddfx_);
+    dddfy_ = PolynomialXd::derivedFrom(ddfy_);
+}
+
+// ----------------------------------------
+
+bool Spline2dSegment::setParams(const std::vector<double> &x_param,
+                                const std::vector<double> &y_param)
+{
+    if (x_param.size() != y_param.size())
+        return false;
+
+    fx_ = PolynomialXd(x_param);
+    fy_ = PolynomialXd(y_param);
+    dfx_ = PolynomialXd::derivedFrom(fx_);
+    dfy_ = PolynomialXd::derivedFrom(fy_);
+    ddfx_ = PolynomialXd::derivedFrom(dfx_);
+    ddfy_ = PolynomialXd::derivedFrom(dfy_);
+    dddfx_ = PolynomialXd::derivedFrom(ddfx_);
+    dddfy_ = PolynomialXd::derivedFrom(ddfy_);
+    return true;
+}
+
+// ----------------------------------------
+
+std::pair<double, double> Spline2dSegment::operator()(const double t) const
+{
+    return std::make_pair(fx_(t), fy_(t));
+}
+
+// ----------------------------------------
+
+double Spline2dSegment::x(const double t) const { return fx_(t); }
+double Spline2dSegment::y(const double t) const { return fy_(t); }
+double Spline2dSegment::derivativeX(const double t) const { return dfx_(t); }
+double Spline2dSegment::derivativeY(const double t) const { return dfy_(t); }
+double Spline2dSegment::secondOrderDerivativeX(const double t) const { return ddfx_(t); }
+double Spline2dSegment::secondOrderDerivativeY(const double t) const { return ddfy_(t); }
+double Spline2dSegment::thirdOrderDerivativeX(const double t) const { return dddfx_(t); }
+double Spline2dSegment::thirdOrderDerivativeY(const double t) const { return dddfy_(t); }
+
+// ----------------------------------------
+
+const PolynomialXd& Spline2dSegment::splineFuncX() const { return fx_; }
+const PolynomialXd& Spline2dSegment::splineFuncY() const { return fy_; }
+const PolynomialXd& Spline2dSegment::derivativeFuncX() const { return dfx_; }
+const PolynomialXd& Spline2dSegment::derivativeFuncY() const { return dfy_; }
+const PolynomialXd& Spline2dSegment::secondOrderDerivativeFuncX() const { return ddfx_; }
+const PolynomialXd& Spline2dSegment::secondOrderDerivativeFuncY() const { return ddfy_; }
+const PolynomialXd& Spline2dSegment::thirdOrderDerivativeFuncX() const { return dddfx_; }
+const PolynomialXd& Spline2dSegment::thirdOrderDerivativeFuncY() const { return dddfy_; }
+// ----------------------------------------
+// Ends class Spline2dSegment
+}
+}
+#endif // SPLINE2DSEGMENT_H

src/decition/decition_brain/decition/adc_math/quartic_polynomial.h

@@ -1,100 +0,0 @@
-/****************************************
- * Class:    Quartic Polynomial class
- * Purpose:  Fitting a quartic polynomial on interval [0, t] by given
- *     boundary conditions.
- * Author:   Zhao Liang
- * Last Updated:  2021/02/23
-*****************************************
-*
-* Note: The polynomial is defined on interval [0, t] and has form
-*
-*     f(t) = a0 + a1*t + a2*t^2 + a3*t^3 + a4*t^4
-*
-* Boundary conditions are given by:
-* 1. x0=f(0), dx0=f'(0), ddx0=f''(0)
-* 2. dx1=f'(t), ddx1=f''(t)
-*/
-
-#ifndef QUARTIC_POLYNOMIAL_H
-#define QUARTIC_POLYNOMIAL_H
-
-#pragma once
-
-#include <array>
-
-namespace iv {
-namespace math {
-
-class QuarticPolynomial
-{
-public:
-    QuarticPolynomial() = default;
-    ~QuarticPolynomial() = default;
-
-    /**
-    * @brief Initialize a quartic polynomial by given boundary conditions.
-    * @param x0: start point at x=0
-    * @param dx0: first order derivative at the start point
-    * @param ddx0: second order derivative at the start point
-    * @param dx1: first order derivative at the end point x=t.
-    * @param ddx1: second order derivative at the end point x=t.
-    * @param t: parameter length.
-    */
-    QuarticPolynomial(const double x0, const double dx0, const double ddx0,
-                      const double dx1, const double ddx1,
-                      const double t);
-
-    /**
-    * @brief You can also pack the boundary conditions into two arrays.
-    */
-    QuarticPolynomial(const std::array<double, 3> &start,
-                      const std::array<double, 2> &end,
-                      const double t);
-
-    /**
-    * @brief Evaluate the n-th derivative of this polynomial at a given point p.
-    * @param order: the order of the derivative to be evaluated.
-    * @param p: the point to be evaluated.
-    */
-    double evaluate(const std::uint32_t order, const double p) const;
-
-    /**
-    * @brief Return the coefficient of the k-th term.
-    */
-    double coef(const size_t order) const;
-
-    /**
-    * @brief Return all coefficients as a std::array.
-    */
-    const std::array<double, 5> &coef() const;
-
-    /**
-    * @brief Return a pretty string representation of this polynomial
-    */
-    std::string toString() const;
-
-    size_t order() const { return 4; }
-
-    /**
-    * @brief Return the length of the fitting interval [0, t]
-    */
-    double paramLength() const { return t_; }
-
-private:
-
-    /**
-    * @brief Compute the coefficients array.
-    */
-    void computeCoefficients(const double x0, const double dx0, const double ddx0,
-                             const double dx1, const double ddx1,
-                             const double t);
-
-    double t_;
-    std::array<double, 3> start_condition_;
-    std::array<double, 2> end_condition_;
-    std::array<double, 5> coef_;
-};
-}
-}
-
-#endif // QUARTIC_POLYNOMIAL_H

src/decition/decition_brain/decition/adc_math/quintic_polynomial.h

@@ -1,93 +0,0 @@
-/****************************************
- * Class:    Quintic Polynomial class
- * Purpose:  Fitting a quinticc polynomial on interval [0, t] by given
- *     boundary conditions.
- * Author:   Zhao Liang
- * Last Updated:  2021/02/24
-*****************************************
-*
-* Note: The polynomial is defined on interval [0, t] and has form
-*
-*     f(t) = a0 + a1*t + a2*t^2 + a3*t^3 + a4*t^4 + a5*t^5
-*
-* Boundary conditions are given by:
-* 1. x0=f(0), dx0=f'(0), ddx0=f''(0)
-* 2. x1=f(t), dx1=f'(t), ddx1=f''(t)
-*/
-#ifndef QUINTIC_POLYNOMIAL_H
-#define QUINTIC_POLYNOMIAL_H
-
-#pragma once
-
-#include <array>
-
-namespace iv {
-namespace math {
-
-class QuinticPolynomial
-{
-public:
-    QuinticPolynomial() = default;
-    ~QuinticPolynomial() = default;
-
-    /**
-    * @brief Initialize a quintic polynomial by given boundary conditions.
-    * @param x0: start point at x=0
-    * @param dx0: first order derivative at the start point
-    * @param ddx0: second order derivative at the start point
-    * @param x1: end point at x=t
-    * @param dx1: first order derivative at the end point x=t.
-    * @param ddx1: second order derivative at the end point x=t.
-    * @param t: parameter length.
-    */
-    QuinticPolynomial(const double x0, const double dx0, const double ddx0,
-                      const double x1, const double dx1, const double ddx1,
-                      const double t);
-
-    QuinticPolynomial(const std::array<double, 3> &start,
-                      const std::array<double, 3> &end,
-                      const double t);
-
-    void fitByBoundaryConditions(const double x0, const double dx0, const double ddx0,
-                                 const double x1, const double dx1, const double ddx1,
-                                 const double t);
-
-    /**
-    * @brief Evaluate the n-th derivative of this polynomial at a given point p.
-    * @param order: the order of the derivative to be evaluated.
-    * @param p: the point to be evaluated.
-    */
-    double evaluate(const std::uint32_t order, const double p) const;
-
-    /**
-    * @brief Return the coefficient of the k-th term.
-    */
-    double coef(const size_t order) const;
-
-    /**
-    * @brief Return all coefficients as a std::array.
-    */
-    const std::array<double, 6> &coef() const;
-
-    /**
-    * @brief Return a pretty string representation of this polynomial
-    */
-    std::string toString() const;
-
-    size_t order() const { return 5; }
-    double paramLength() const { return t_; }
-
-private:
-    void computeCoefficients(const double x0, const double dx0, const double ddx0,
-                             const double x1, const double dx1, const double ddx1,
-                             const double t);
-
-    double t_;
-    std::array<double, 3> start_condition_;
-    std::array<double, 3> end_condition_;
-    std::array<double, 6> coef_;
-};
-}
-}
-
-#endif // QUINTIC_POLYNOMIAL_H

src/decition/decition_brain/decition/adc_math/vec2.h

@@ -1,289 +0,0 @@
-/****************************************
-* Class:    2D vector class
-* Purpose:  A single header file handling 2D vector computations.
-* Author:   Zhao Liang
-* Last Updated:  2021/01/20
-*****************************************
-*
-* Note: Need to add error handling procedures in divisions.
-*
-*/
-
-#ifndef VEC2_H
-#define VEC2_H
-
-#pragma once
-
-#include <cmath>
-#include <iostream>
-
-namespace iv {
-namespace math {
-
-constexpr double _EPSILON = 1e-12;
-
-class Vec2
-{
-public:
-
-    // ----------------------------------------
-
-    /*
-     * Constructors.
-    */
-
-    Vec2() : x_(0), y_(0) {}
-
-    explicit Vec2(double a) : x_(a), y_(a) {}
-
-    Vec2(double a, double b) : x_(a), y_(b) {}
-
-    // ----------------------------------------
-
-    // Compare to another vector
-    bool operator == (const Vec2 &v) const
-    {
-        return (std::abs(x_ - v.x()) < _EPSILON &&
-                std::abs(y_ - v.y()) < _EPSILON);
-    }
-
-    // ----------------------------------------
-
-    /*
-     * Return the unit vector Vec2(cos(angle), sin(angle)),
-     * The param 'angle' should be in radians.
-    */
-    static Vec2 unitVec2(const double angle)
-    {
-        return Vec2(cos(angle), sin(angle));
-    }
-
-    // ----------------------------------------
-
-    /*
-     * Access x and y components
-    */
-    double x() const { return x_; }
-    double y() const { return y_; }
-
-    // ----------------------------------------
-
-    /*
-     * Set x and y components
-    */
-    void set_x(const double x) { x_ = x; }
-    void set_y(const double y) { y_ = y; }
-
-    // ----------------------------------------
-
-    /*
-     * Operations with other scalars,
-     * vec2 is the left operand, scalar is the right operand.
-    */
-
-    // ----------------------------------------
-
-    // add a scalar
-    Vec2 operator + (double c) const { return Vec2(x() + c, y() + c); }
-    // subtract a scalar
-    Vec2 operator - (double c) const { return Vec2(x() - c, y() - c); }
-    // multiply a scalar
-    Vec2 operator * (double c) const { return Vec2(x() * c, y() * c); }
-    // divide by a scalar
-    Vec2 operator / (double c) const { return Vec2(x() / c, y() / c); }
-    // in-place add a scalar
-    Vec2 operator += (double c) { x_ += c; y_ += c; return (*this); }
-    // in-place subtract a scalar
-    Vec2 operator -= (double c) { x_ -= c; y_ -= c; return (*this); }
-    // in-place multiply a scalar
-    Vec2 operator *= (double c) { x_ *= c; y_ *= c; return (*this); }
-    // in-place divide by a scalar
-    Vec2 operator /= (double c) { x_ /= c; y_ /= c; return (*this); }
-
-    // ----------------------------------------
-
-    /*
-     * Operations with other vec2
-    */
-
-    // ----------------------------------------
-
-    // add two vectors
-    Vec2 operator + (const Vec2 &v) const { return Vec2(x() + v.x(), y() + v.y()); }
-    // subtract another vector
-    Vec2 operator - (const Vec2 &v) const { return Vec2(x() - v.x(), y() - v.y()); }
-    // multiply two vectors as complex numbers
-    Vec2 operator * (const Vec2 &v) const
-    {
-        double x1 = x() * v.x() - y() * v.y();
-        double y1 = x() * v.y() + y() * v.x();
-        return Vec2(x1, y1);
-    }
-    // division as complex numbers
-    Vec2 operator / (const Vec2 &v) const
-    {
-        double snorm = v.squared_norm();
-        return Vec2(this->dot(v) / snorm, -this->cross(v) / snorm);
-    }
-    // in-place vector addition
-    Vec2 operator += (const Vec2 &v) { x_ += v.x(); y_ += v.y(); return (*this); }
-    // in-place vector subtraction
-    Vec2 operator -= (const Vec2 &v) { x_ -= v.x(); y_ -= v.y(); return (*this); }
-    // in-place vector multiplication as complex numbers
-    Vec2 operator *= (const Vec2 &v)
-    {
-        double x1 = x() * v.x() - y() * v.y();
-        double y1 = x() * v.y() + y() * v.x();
-        x_ = x1;
-        y_ = y1;
-        return (*this);
-    }
-    // in-place vector division as complex numbers
-    Vec2 operator /= (const Vec2 &v)
-    {
-        double snorm = v.squared_norm();
-        double x1 = this->dot(v) / snorm;
-        double y1 = -this->cross(v) / snorm;
-        x_ = x1;
-        y_ = y1;
-        return (*this);
-    }
-
-    // ----------------------------------------
-
-    /*
-     * Vector util functions
-    */
-
-    // ----------------------------------------
-
-    // Return squared norm
-    double squared_norm() const { return x() * x() + y() * y(); }
-
-    // Return the usual Euclidean norm
-    double norm() const { return sqrt(x() * x() + y() * y()); }
-
-    // Inner product of two vectors
-    double dot(const Vec2 &v) const { return x() * v.x() + y() * v.y(); }
-
-    // Angle with the positive x-axis in radians
-    double angle() const { return atan2(y(), x()); }
-
-    // Angle between two vectors
-    double angle(const Vec2 &v) const
-    {
-        return acos(this->dot(v) / (norm() * v.norm()));
-    }
-
-    // Distance between two vectors
-    double dist(const Vec2 &v) const { return (*this - v).norm(); }
-
-    // Squared distance between two vectors
-    double squared_dist(const Vec2 &v) const { return (*this - v).squared_norm(); }
-
-    // Cross product of two vectors
-    double cross(const Vec2 &v) const { return x() * v.y() - y() * v.x(); }
-
-    // Return a normalized version of this vector
-    Vec2 normalize() const
-    {
-        double s = norm();
-        return (*this) / s;
-    }
-
-    // Translate a vector
-    Vec2 translate(double tx, double ty) const { return Vec2(x() + tx, y() + ty); }
-
-    // Rotate a vector by angle
-    Vec2 rotate(const double theta) const
-    {
-        double ct = cos(theta), st = sin(theta);
-        double x1 = x() * ct - y() * st;
-        double y1 = x() * ct + y() * st;
-        return Vec2(x1, y1);
-    }
-
-    // return a perpendicular vector to this one
-    Vec2 perp() const { return Vec2(-y(), x()); }
-
-private:
-    double x_;
-    double y_;
-};
-
-// ----------------------------------------
-
-/*
- * Vec2 operations as right operand
-*/
-
-// ----------------------------------------
-
-// print formatting
-std::ostream & operator << (std::ostream &out, const Vec2 &v)
-{
-    out << "Vec2(" << v.x() << ", " << v.y() << ")";
-    return out;
-}
-
-// add another scalar
-Vec2 operator + (double c, const Vec2 &v) { return Vec2(v.x() + c, v.y() + c); }
-// subtract another scalar
-Vec2 operator - (double c, const Vec2 &v) { return Vec2(c - v.x(), c - v.y()); }
-// multiply another scalar
-Vec2 operator * (double c, const Vec2 &v) { return Vec2(v.x() * c, v.y() * c); }
-// divide another scalar
-Vec2 operator / (double c, const Vec2 &v)
-{
-    double snorm = v.squared_norm();
-    return Vec2(v.x() * c / snorm, -v.y() * c / snorm);
-}
-
-// ----------------------------------------
-
-/*
- * Vector util functions
-*/
-
-// ----------------------------------------
-
-// Inner product
-double vdot(const Vec2 &v1, const Vec2 &v2) { return v1.dot(v2); }
-
-// Cross
-double vcross(const Vec2 &v1, const Vec2 &v2) { return v1.cross(v2); }
-
-// Vector length
-double vlength(const Vec2 &v) { return v.norm(); }
-
-// Angle with positive x-axis
-double vangle(const Vec2 &v) { return v.angle(); }
-
-// Angle between two vectors
-double vangle(const Vec2 &v1, const Vec2 &v2) { return v1.angle(v2); }
-
-// distance between two vectors
-double vdist(const Vec2 &v1, const Vec2 &v2) { return v1.dist(v2); }
-
-// squared distance between two vectors
-double vsquared_dist(const Vec2 &v1, const Vec2 &v2) { return v1.squared_dist(v2); }
-
-// normalize a vector
-Vec2 vnormalize(const Vec2 &v) { return v.normalize(); }
-
-// return a perpendicular one
-Vec2 vperp(const Vec2 &v) { return v.perp(); }
-
-// rotate a vector
-Vec2 vrotate(const Vec2 &v, double theta) { return v.rotate(theta); }
-
-// return a unit vector of given direction
-Vec2 vdir(const double angle) { return Vec2::unitVec2(angle); }
-
-// return a linear combination of two vectors
-Vec2 vinterp(const Vec2 &v1, const Vec2 &v2, double t) { return v1 * (1 - t) + v2 * t; }
-
-}
-}
-
-#endif // VEC2_H

src/driver/driver_lidar_leishen16/driver_lidar_leishen16.pro

@@ -0,0 +1,54 @@
+#-------------------------------------------------
+#
+# Project created by QtCreator 2018-12-14T18:35:51
+#
+#-------------------------------------------------
+
+QT       += network
+
+QT       -= gui
+
+CONFIG += c++11 console
+CONFIG -= app_bundle
+
+QMAKE_LFLAGS += -no-pie
+
+# The following define makes your compiler emit warnings if you use
+# any feature of Qt which has been marked as deprecated (the exact warnings
+# depend on your compiler). Please consult the documentation of the
+# deprecated API in order to know how to port your code away from it.
+DEFINES += QT_DEPRECATED_WARNINGS
+
+# You can also make your code fail to compile if you use deprecated APIs.
+# In order to do so, uncomment the following line.
+# You can also select to disable deprecated APIs only up to a certain version of Qt.
+#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
+
+SOURCES += \
+        lidar_driver_leishen16.cpp \
+    main.cpp
+
+HEADERS += \
+        lidar_driver_leishen16.h \
+    lidar_leishen16_rawdata.h
+
+
+!include(../../../include/common.pri ) {
+    error( "Couldn't find the common.pri file!" )
+}
+
+!include(../../../include/ivprotobuf.pri ) {
+    error( "Couldn't find the ivprotobuf.pri file!" )
+}
+
+!include(../../../include/ivboost.pri ) {
+    error( "Couldn't find the ivboost.pri file!" )
+}
+
+!include(../../../include/ivyaml-cpp.pri ) {
+    error( "Couldn't find the ivyaml-cpp.pri file!" )
+}
+
+!include(../../../include/ivpcl.pri ) {
+    error( "Couldn't find the ivpcl.pri file!" )
+}

src/driver/driver_lidar_leishen16/lidar_driver_leishen16.cpp

@@ -0,0 +1,553 @@
+
+#include <thread>
+#include <iostream>
+#include <QUdpSocket>
+//#include <QNetworkDatagram>
+#include <iostream>
+#include <QMutex>
+#include <QDateTime>
+
+
+//#include <pcl/conversions.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+#include "modulecomm.h"
+
+#include "lidar_driver_leishen16.h"
+#include "lidar_leishen16_rawdata.h"
+
+#include "ivexit.h"
+
+#include "ivfault.h"
+#include "ivlog.h"
+
+
+#ifdef VV7_1
+
+int vv7;
+#endif
+
+
+
+#define Lidar_Pi 3.1415926535897932384626433832795
+#define Lidar32 (unsigned long)3405883584//192.168.1.203
+#define Lidar_roll_ang 90*Lidar_Pi/180.0
+
+
+static std::thread * g_pleishen16Thread;
+static std::thread * g_pleishen16ProcThread;
+
+//float gV_theta[16] =  {-15,1,-13,3,-11,5,-9,7,-7,9,-5,11,-3,13,-1,15};   //16 Angles
+static float gV_theta[16] =  {-15,1,-13,3,-11,5,-9,7,-7,9,-5,11,-3,13,-1,15};   //16 Angles
+static float gV_theta_cos[16],gV_theta_sin[16];
+
+//static void * g_leishen16_raw;
+
+static void * g_lidar_pc;
+
+
+static bool g_bleishen16_run = false;
+static bool g_bleishen16_running = false;
+static bool g_bleishen16_Proc_running = false;
+
+static int g_seq = 0;
+
+static unsigned short glidar_port=2368;
+
+extern char gstr_memname[256];
+extern char gstr_rollang[256];
+extern char gstr_inclinationang_yaxis[256];  //from y axis
+extern char gstr_inclinationang_xaxis[256];  //from x axis
+//char gstr_hostip[256];
+extern char gstr_port[256];
+extern char gstr_yaml[256];
+
+
+extern iv::Ivfault * gIvfault;
+extern iv::Ivlog * gIvlog;
+
+/**
+ * @brief The leishen16_Buf class
+ *  Use for Lidar UDP DATA Save
+ */
+class leishen16_Buf
+{
+private:
+    char * mstrdata;
+    int mnSize;  //Data SizeUse
+    QMutex mMutex;
+    int mIndex;
+public:
+    leishen16_Buf()
+    {
+        mstrdata = new char[BK32_DATA_BUFSIZE];
+        mIndex = 0;
+        mnSize = 0;
+    }
+    ~leishen16_Buf()
+    {
+        delete mstrdata;
+    }
+    void WriteData(const char * strdata,const int nSize)
+    {
+        mMutex.lock();
+        memcpy(mstrdata,strdata,nSize);
+        mnSize = nSize;
+        mIndex++;
+        mMutex.unlock();
+    }
+    int ReadData(char * strdata,const int nRead,int * pnIndex)
+    {
+        int nRtn = 0;
+        if(mnSize <= 0)return 0;
+        mMutex.lock();
+        nRtn = mnSize;
+        if(nRtn >nRead)
+        {
+            nRtn = nRead;
+            std::cout<<"lidar_leishen16 leishen16_Buf ReadData data nRead = "<<nRead<<" is small"<<std::endl;
+        }
+        memcpy(strdata,mstrdata,nRtn);
+        mnSize = 0;
+        if(pnIndex != 0)*pnIndex = mIndex;
+        mMutex.unlock();
+        return nRtn;
+    }
+};
+
+
+
+static leishen16_Buf  * g_leishen16_Buf;
+static char * g_RawData_Buf;  //Buffer UDP Data
+static int gnRawPos = 0;
+static float gAngle_Old = 0;
+static float gAngle_Total = 0;
+
+static unsigned short gold = 0;
+
+static int gnPac = 0;
+
+#include <QTime>
+
+static QTime gTime;
+
+/**
+ * @brief processleishen16_Data
+ * @param ba UDP Buffer
+ * 1.UDP ByteArray Length is 1206.
+ * 2.if Angle is More than 360. Tell Another thread process.
+ */
+static void processleishen16_Data(QByteArray ba)
+{
+
+    gnPac++;
+    unsigned short * pAng;
+    float fAng;
+
+    char * pdata;
+    pdata = ba.data();
+//    std::cout<<"len is "<<ba.length()<<std::endl;
+    if(ba.length() == 1206)
+    {
+        pAng = (unsigned short *)(pdata+2);
+        fAng = *pAng;fAng = fAng*0.01;
+//        std::cout<<"ang is "<<*pAng<<std::endl;
+        if(fabs(fAng-gAngle_Old)>300)
+        {
+            gAngle_Total = gAngle_Total + fabs(fabs(fAng-gAngle_Old)-360);
+        }
+        else
+        {
+            gAngle_Total = gAngle_Total + fabs(fabs(fAng-gAngle_Old));
+        }
+            gAngle_Old = fAng;
+            if(gAngle_Total > 360)
+            {
+                g_leishen16_Buf->WriteData(g_RawData_Buf,gnRawPos);
+                lidar_leishen16_raw * p = new lidar_leishen16_raw();
+                p->mnLen = gnRawPos;
+                memcpy(p->mstrdata,g_RawData_Buf,gnRawPos);
+//                iv::modulecomm::ModuleSendMsg(g_leishen16_raw,(char *)p,sizeof(lidar_leishen16_raw));
+                delete p;
+                memcpy(g_RawData_Buf,pdata,1206);
+                gnRawPos = 1206;
+  //                  std::cout<<"index = "<<gnPac<<" time ="<<gTime.elapsed()<<" a cycle"<<std::endl;
+                    gAngle_Total = 0;
+            }
+            else
+            {
+                if((gnRawPos+1206)<= BK32_DATA_BUFSIZE)
+                {
+                    memcpy(g_RawData_Buf+gnRawPos,pdata,1206);
+                    gnRawPos= gnRawPos+1206;
+                }
+                else
+                {
+                    std::cout<<"lidar_leishen16 processleishen16_Data data is very big gnRawPos = "<<gnRawPos<<std::endl;
+                }
+            }
+
+ //       std::cout<<*pAng<<std::endl;
+
+   //     gold = *pAng;
+        if(gnRawPos == 0)
+        {
+            gAngle_Total = 0;
+            gAngle_Old = *pAng;
+            gAngle_Old = gAngle_Old*0.01;
+            memcpy(g_RawData_Buf,pdata,1206);
+            gnRawPos = gnRawPos+1206;
+        }
+    }
+    else
+    {
+        std::cout<<"lidar_leishen16 processleishen16_Data receive data packet len is not 1206 "<<std::endl;
+    }
+}
+
+
+
+
+/**
+ * @brief leishen16_Func thread for receive udp socket
+ * @param n
+ */
+static void leishen16_Func(int n)
+{
+
+    gTime.start();
+    std::cout<<"Enter leishen16_Func."<<std::endl;
+
+    QUdpSocket * udpSocket = new QUdpSocket( );
+    udpSocket->bind(QHostAddress::Any, glidar_port);
+
+    unsigned int ndatamisstime = 0;
+
+    int nstate = 0;
+    int nlaststate = 0;
+    while(g_bleishen16_run)
+    {
+        if(udpSocket->hasPendingDatagrams())
+        {
+            ndatamisstime = 0;
+  //          std::cout<<"have data."<<std::endl;
+            QByteArray datagram;
+            datagram.resize(udpSocket->pendingDatagramSize());
+            QHostAddress sender;
+            quint16 senderPort;
+
+            udpSocket->readDatagram(datagram.data(), datagram.size(),
+                                            &sender, &senderPort);
+
+//                    processTheDatagram(datagram);
+  //          std::cout<<"have data."<<std::endl;
+//            QNetworkDatagram datagram = udpSocket->receiveDatagram();
+            processleishen16_Data(datagram);
+            datagram.clear();
+
+        }
+        else
+        {
+//            std::cout<<"running."<<std::endl;
+            std::this_thread::sleep_for(std::chrono::milliseconds(1));
+            if(ndatamisstime <  1000000) ndatamisstime++;
+        }
+
+        if(ndatamisstime > 1000)
+        {
+            nstate = 1;
+        }
+        if(ndatamisstime > 60000)
+        {
+            nstate = 2;
+        }
+        if(ndatamisstime < 100)
+        {
+            nstate = 0;
+        }
+        if(nlaststate != nstate)
+        {
+            nlaststate = nstate;
+            switch (nstate) {
+            case 0:
+                gIvfault->SetFaultState(0,0,"OK");
+                gIvlog->info("received udp data.device is ok.");
+                break;
+            case 1:
+                gIvfault->SetFaultState(1,1,"No data");
+                gIvlog->warn("more than 1 second no data. warning.");
+                break;
+            case 2:
+                gIvfault->SetFaultState(2,2,"No data,Please Check Device or setting.");
+                gIvlog->error("more than 60 seconds no data. error. Please check device or setting.");
+                break;
+            default:
+                break;
+            }
+        }
+
+    }
+    udpSocket->close();
+    delete udpSocket;
+    g_bleishen16_running = false;
+    std::cout<<"leishen16_Func Exit."<<std::endl;
+}
+
+
+/**
+ * @brief process_leishen16obs  Make PointCloud And Share
+ * @param strdata  pointer to data
+ * @param nLen  data length
+ */
+static void process_leishen16obs(char * strdata,int nLen)
+{
+
+    double frollang = atof(gstr_rollang) *M_PI/180.0;    //Roll Angle
+    double finclinationang_xaxis = atof(gstr_inclinationang_xaxis)*M_PI/180.0;  //Inclination from x axis
+    double finclinationang_yaxis = atof(gstr_inclinationang_yaxis)*M_PI/180.0;   //Inclination from y axis
+    bool binclix = false;
+    bool bincliy = false;
+    if(finclinationang_xaxis != 0.0)binclix = true;
+    if(finclinationang_yaxis != 0.0)bincliy = true;
+
+    double cos_finclinationang_xaxis = cos(finclinationang_xaxis);
+    double sin_finclinationang_xaxis = sin(finclinationang_xaxis);
+    double cos_finclinationang_yaxis = cos(finclinationang_yaxis);
+    double sin_finclinationang_yaxis = sin(finclinationang_yaxis);
+
+
+    QDateTime dt = QDateTime::currentDateTime();
+    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
+                new pcl::PointCloud<pcl::PointXYZI>());
+
+
+    point_cloud->header.frame_id = "velodyne";
+    point_cloud->height = 1;
+    point_cloud->header.stamp = dt.currentMSecsSinceEpoch();
+    point_cloud->width = 0;
+    point_cloud->header.seq =g_seq;
+    g_seq++;
+
+    unsigned char * pstr = (unsigned char *)strdata;
+
+//    std::cout<<"enter obs."<<std::endl;
+//    float w = 0.0036;
+    float Ang = 0;
+    float Range = 0;
+    int bag = 0;
+    int Group = 0;
+    int pointi = 0;
+    float wt = 0;
+    int wt1 = 0;
+
+
+
+    int buf1len = nLen/1206;
+
+    unsigned short * pAng;
+    double ang1,ang2;
+    pAng = (unsigned short *)(strdata+2+0*100);
+    ang1 = *pAng;ang1 = ang1/100.0;
+    pAng = (unsigned short *)(strdata+2+1*100);
+    ang2 = *pAng;ang2 = ang2/100.0;
+
+    double angdiff = ang2 - ang1;
+    if(angdiff<0)
+        angdiff = angdiff + 360.0;
+    angdiff = angdiff/2.0;
+
+    for (bag = 0; bag < buf1len; bag++)
+    {
+        for (Group = 0; Group <= 11; Group++)
+        {
+            wt1 = ((pstr[bag*1206 +3 + Group * 100] *256) + pstr[bag*1206 + 2 + Group * 100]) ;
+            wt = wt1/ 100.0;
+ //           std::cout<<"wt1 is "<<wt1<<std::endl;
+
+            for (pointi = 0; pointi <16; pointi++)
+            {
+                Ang = (0 - wt) / 180.0 * Lidar_Pi;
+                Range = ((pstr[bag*1206 + Group * 100 + 5 + 3 * pointi] << 8) + pstr[bag*1206+Group * 100 + 4 + 3 * pointi]);
+                unsigned char intensity = pstr[bag*1206 + Group * 100 + 6 + 3 * pointi];
+                Range=Range* 0.0025;
+
+
+                pcl::PointXYZI point;
+                point.x  = Range* gV_theta_cos[pointi]*cos(Ang + frollang);
+                point.y  = Range* gV_theta_cos[pointi] *sin(Ang + frollang);
+                point.z  = Range* gV_theta_sin[pointi];
+                point.intensity = intensity;
+
+                if(binclix)
+                {
+                    double y,z;
+                    y = point.y;z = point.z;
+                    point.y = y*cos_finclinationang_xaxis +z*sin_finclinationang_xaxis;
+                    point.z = z*cos_finclinationang_xaxis - y*sin_finclinationang_xaxis;
+                }
+                if(bincliy)
+                {
+                    double z,x;
+                    z = point.z;x = point.x;
+                    point.z = z*cos_finclinationang_yaxis + x*sin_finclinationang_yaxis;
+                    point.x = x*cos_finclinationang_yaxis - z*sin_finclinationang_yaxis;
+                }
+                point_cloud->points.push_back(point);
+
+
+                ++point_cloud->width;
+
+            }
+
+
+            wt = wt + 0.2;   //扫描帧频10Hz时的水平角分辨率是0.2°，
+
+
+
+            for (pointi = 0; pointi < 16; pointi++)
+            {
+                Ang = (0 - wt) / 180.0 * Lidar_Pi;
+                Range = ((pstr[bag*1206 + Group * 100 + 53 + 3 * pointi] << 8) + pstr[bag*1206+Group * 100 + 52 + 3 * pointi]);
+                unsigned char intensity = pstr[bag*1206 + Group * 100 + 54 + 3 * pointi];
+                Range=Range* 0.0025;
+
+
+                pcl::PointXYZI point;
+                point.x  = Range* gV_theta_cos[pointi] *cos(Ang + frollang);
+                point.y  = Range* gV_theta_cos[pointi] *sin(Ang + frollang);
+                point.z  = Range* gV_theta_sin[pointi] ;
+                point.intensity = intensity;
+                if(binclix)
+                {
+                    double y,z;
+                    y = point.y;z = point.z;
+                    point.y = y*cos_finclinationang_xaxis +z*sin_finclinationang_xaxis;
+                    point.z = z*cos_finclinationang_xaxis - y*sin_finclinationang_xaxis;
+                }
+                if(bincliy)
+                {
+                    double z,x;
+                    z = point.z;x = point.x;
+                    point.z = z*cos_finclinationang_yaxis + x*sin_finclinationang_yaxis;
+                    point.x = x*cos_finclinationang_yaxis - z*sin_finclinationang_yaxis;
+                }
+                point_cloud->points.push_back(point);
+
+
+                ++point_cloud->width;
+
+            }
+        }
+    }
+
+    char * strOut = new char[4+sizeof(point_cloud->header.frame_id.size()) + 4+8+point_cloud->width * sizeof(pcl::PointXYZI)];
+
+    int * pHeadSize = (int *)strOut;
+    *pHeadSize = 4 + point_cloud->header.frame_id.size()+4+8;
+    memcpy(strOut+4,point_cloud->header.frame_id.c_str(),point_cloud->header.frame_id.size());
+    pcl::uint32_t * pu32 = (pcl::uint32_t *)(strOut+4+sizeof(point_cloud->header.frame_id.size()));
+    *pu32 = point_cloud->header.seq;
+    memcpy(strOut+4+sizeof(point_cloud->header.frame_id.size()) + 4,&point_cloud->header.stamp,8);
+    pcl::PointXYZI * p;
+    p = (pcl::PointXYZI *)(strOut +4+sizeof(point_cloud->header.frame_id.size()) + 4+8 );
+    memcpy(p,point_cloud->points.data(),point_cloud->width * sizeof(pcl::PointXYZI));
+
+    iv::modulecomm::ModuleSendMsg(g_lidar_pc,strOut,4+sizeof(point_cloud->header.frame_id.size()) + 4+8+point_cloud->width * sizeof(pcl::PointXYZI));
+    delete strOut;
+
+//            std::cout<<"point cloud width = "<<point_cloud->width<<"  size = "<<point_cloud->size()<<std::endl;
+}
+
+
+/**
+ * @brief leishen16_Proc_Func
+ *    thread for process data to PointCloud
+ * @param n
+ */
+static void leishen16_Proc_Func(int n)
+{
+    std::cout<<"Enter leishen16_Proc_Func"<<std::endl;
+    char * strdata = new char[BK32_DATA_BUFSIZE];
+    int nIndex;
+    int nRead;
+    while(g_bleishen16_run)
+    {
+        if((nRead = g_leishen16_Buf->ReadData(strdata,BK32_DATA_BUFSIZE,&nIndex))>0)
+        {
+            //process data.
+            process_leishen16obs(strdata,nRead);
+        }
+        else
+        {
+ //           std::cout<<"running."<<std::endl;
+            std::this_thread::sleep_for(std::chrono::milliseconds(1));
+        }
+    }
+
+    g_bleishen16_Proc_running = false;
+    std::cout<<"Exit leishen16_Proc_Func"<<std::endl;
+}
+
+
+
+void exitfunc()
+{
+    StopLidar_leishen16();
+
+}
+
+/**
+ * @brief StartLidar_leishen16x Start
+ * @return
+ */
+int LIDAR_DRIVER_LEISHEN16SHARED_EXPORT StartLidar_leishen16()
+{
+
+    iv::ivexit::RegIVExitCall(exitfunc);
+
+    std::cout<<"Now Start leishen16 Listen."<<std::endl;
+    g_RawData_Buf = new char[BK32_DATA_BUFSIZE];
+
+    int i;
+    for(i=0;i<16;i++)
+    {
+        gV_theta[i] = gV_theta[i]*M_PI/180.0;
+        gV_theta_cos[i] = cos(gV_theta[i]);
+        gV_theta_sin[i] = sin(gV_theta[i]);
+    }
+
+    g_leishen16_Buf = new leishen16_Buf();
+    g_bleishen16_run = true;
+    g_bleishen16_running = true;
+    g_bleishen16_Proc_running = true;
+
+    glidar_port = atoi(gstr_port);
+
+//    g_leishen16_raw = iv::modulecomm::RegisterSend(strmemnameraw,10*sizeof(lidar_leishen16_raw),10);
+    g_lidar_pc = iv::modulecomm::RegisterSend(gstr_memname,20000000,1);
+    g_pleishen16Thread = new std::thread(leishen16_Func,0);
+    g_pleishen16ProcThread = new std::thread(leishen16_Proc_Func,0);
+
+    return 0;
+}
+
+/**
+ * @brief StopLidar_leishen16 Stop
+ */
+void LIDAR_DRIVER_LEISHEN16SHARED_EXPORT StopLidar_leishen16()
+{
+    std::cout<<"Now Close leishen16. "<<std::endl;
+    g_bleishen16_run = false;
+    g_pleishen16Thread->join();
+    g_pleishen16ProcThread->join();
+
+    iv::modulecomm::Unregister(g_lidar_pc);
+
+    delete g_pleishen16ProcThread;
+    delete g_pleishen16Thread;
+
+    delete g_leishen16_Buf;
+    delete g_RawData_Buf;
+
+}

src/driver/driver_lidar_leishen16/lidar_driver_leishen16.h

@@ -0,0 +1,17 @@
+#ifndef LIDAR_DRIVER_LEISHEN16_H
+#define LIDAR_DRIVER_LEISHEN16_H
+
+
+#include <QtCore/qglobal.h>
+
+#if defined(LIDAR_DRIVER_LEISHEN16_LIBRARY)
+#  define LIDAR_DRIVER_LEISHEN16SHARED_EXPORT Q_DECL_EXPORT
+#else
+#  define LIDAR_DRIVER_LEISHEN16SHARED_EXPORT
+#endif
+
+
+int LIDAR_DRIVER_LEISHEN16SHARED_EXPORT StartLidar_leishen16();
+void LIDAR_DRIVER_LEISHEN16SHARED_EXPORT StopLidar_leishen16();
+
+#endif // LIDAR_DRIVER_VLP16_H

src/driver/driver_lidar_leishen16/lidar_leishen16_rawdata.h

@@ -0,0 +1,13 @@
+#ifndef LIDAR_LEISHEN16_RAWDATA_H
+#define LIDAR_LEISHEN16_RAWDATA_H
+
+#define BK32_DATA_BUFSIZE 2000000
+
+class lidar_leishen16_raw
+{
+public:
+    unsigned int mnLen;
+    char mstrdata[BK32_DATA_BUFSIZE];
+};
+
+#endif // LIDAR_LEISHEN16_RAWDATA_H

src/driver/driver_lidar_leishen16/main.cpp

@@ -0,0 +1,217 @@
+#include <QCoreApplication>
+
+#include "lidar_driver_leishen16.h"
+
+#include "ivversion.h"
+
+#include <getopt.h>
+#include <iostream>
+
+#include <iostream>
+#include <fstream>
+#include <yaml-cpp/yaml.h>
+
+#include "ivfault.h"
+#include "ivlog.h"
+
+iv::Ivfault * gIvfault;
+iv::Ivlog * gIvlog;
+
+char gstr_memname[256];
+char gstr_rollang[256];
+char gstr_inclinationang_yaxis[256];  //from y axis
+char gstr_inclinationang_xaxis[256];  //from x axis
+//char gstr_hostip[256];
+char gstr_port[256];
+char gstr_yaml[256];
+char gstr_modulename[256];
+
+/**
+ * @brief print_useage
+ */
+void print_useage()
+{
+    std::cout<<" -n --modulename $modulename : set module name. eq.  -n lidarleft"<<std::endl;
+    std::cout<<" -m --memname $memname : share memory name. eq.  -m lidar_pc"<<std::endl;
+    std::cout<<" -r --rollang $rollang : roll angle. eq.  -r 10.0"<<std::endl;
+    std::cout<<" -x --inclinationang_xaxis $inclinationang_xaxis : inclination angle from x axis. eq.  -x 0.0"<<std::endl;
+    std::cout<<" -y --inclinationang_yaxis $inclinationang_yaxis : inclination angle from y axis. eq.  -y 0.0"<<std::endl;
+//    std::cout<<" -o --hostip $hostip : host ip. eq.  -o 192.168.1.111"<<std::endl;
+    std::cout<<" -p --port $port : port . eq.  -p 2368"<<std::endl;
+    std::cout<<" -s --setyaml $yaml : port . eq.  -s rs1.yaml"<<std::endl;
+    std::cout<<" -h --help print help"<<std::endl;
+}
+
+int  GetOptLong(int argc, char *argv[]) {
+    int nRtn = 0;
+    int opt; // getopt_long() 的返回值
+    int digit_optind = 0; // 设置短参数类型及是否需要参数
+
+    // 如果option_index非空，它指向的变量将记录当前找到参数符合long_opts里的
+    // 第几个元素的描述，即是long_opts的下标值
+    int option_index = 0;
+    // 设置短参数类型及是否需要参数
+    const char *optstring = "n:m:r:x:y:p:s:h";
+
+    // 设置长参数类型及其简写，比如 --reqarg <==>-r
+    /*
+    struct option {
+             const char * name;  // 参数的名称
+             int has_arg; // 是否带参数值，有三种：no_argument， required_argument，optional_argument
+             int * flag; // 为空时，函数直接将 val 的数值从getopt_long的返回值返回出去，
+                     // 当非空时，val的值会被赋到 flag 指向的整型数中，而函数返回值为0
+             int val; // 用于指定函数找到该选项时的返回值，或者当flag非空时指定flag指向的数据的值
+        };
+    其中：
+        no_argument(即0)，表明这个长参数不带参数（即不带数值，如：--name）
+            required_argument(即1)，表明这个长参数必须带参数（即必须带数值，如：--name Bob）
+            optional_argument(即2)，表明这个长参数后面带的参数是可选的，（即--name和--name Bob均可）
+     */
+    static struct option long_options[] = {
+        {"modulename", required_argument, NULL, 'n'},
+        {"memname", required_argument, NULL, 'm'},
+        {"rollang", required_argument, NULL, 'r'},
+        {"inclinationang_xaxis", required_argument, NULL, 'x'},
+        {"inclinationang_yaxis", required_argument, NULL, 'y'},
+        {"port", required_argument, NULL, 'p'},
+        {"setyaml", required_argument, NULL, 's'},
+        {"help",  no_argument,       NULL, 'h'},
+ //       {"optarg", optional_argument, NULL, 'o'},
+        {0, 0, 0, 0}  // 添加 {0, 0, 0, 0} 是为了防止输入空值
+    };
+
+    while ( (opt = getopt_long(argc,
+                               argv,
+                               optstring,
+                               long_options,
+                               &option_index)) != -1) {
+//        printf("opt = %c\n", opt); // 命令参数，亦即 -a -b -n -r
+//        printf("optarg = %s\n", optarg); // 参数内容
+//        printf("optind = %d\n", optind); // 下一个被处理的下标值
+//        printf("argv[optind - 1] = %s\n",  argv[optind - 1]); // 参数内容
+//        printf("option_index = %d\n", option_index);  // 当前打印参数的下标值
+//        printf("\n");
+        switch(opt)
+        {
+        case 'n':
+            strncpy(gstr_modulename,optarg,255);
+            break;
+        case 'm':
+            strncpy(gstr_memname,optarg,255);
+            break;
+        case 'r':
+            strncpy(gstr_rollang,optarg,255);
+            break;
+        case 'x':
+            strncpy(gstr_inclinationang_xaxis,optarg,255);
+            break;
+        case 'y':
+            strncpy(gstr_inclinationang_yaxis,optarg,255);
+            break;
+//        case 'o':
+//            strncpy(gstr_hostip,optarg,255);
+//            break;
+        case 'p':
+            strncpy(gstr_port,optarg,255);
+            break;
+        case 's':
+            strncpy(gstr_yaml,optarg,255);
+            break;
+        case 'h':
+            print_useage();
+            nRtn = 1; //because use -h
+            break;
+        default:
+            break;
+        }
+
+    }
+
+    return nRtn;
+}
+
+/**
+ * @brief decodeyaml
+ * @param stryaml yaml path
+ */
+void decodeyaml(const char * stryaml)
+{
+    YAML::Node config;
+    try
+    {
+        config = YAML::LoadFile(stryaml);
+    }
+    catch(YAML::BadFile e)
+    {
+        qDebug("load yaml error.");
+        return;
+    }
+
+
+    if(config["memname"])
+    {
+        strncpy(gstr_memname,config["memname"].as<std::string>().data(),255);
+    }
+    if(config["rollang"])
+    {
+        strncpy(gstr_rollang,config["rollang"].as<std::string>().data(),255);
+    }
+    if(config["inclinationang_xaxis"])
+    {
+        strncpy(gstr_inclinationang_xaxis,config["inclinationang_xaxis"].as<std::string>().data(),255);
+    }
+    if(config["inclinationang_yaxis"])
+    {
+        strncpy(gstr_inclinationang_yaxis,config["inclinationang_yaxis"].as<std::string>().data(),255);
+    }
+//    if(config["hostip"])
+//    {
+//        strncpy(gstr_hostip,config["hostip"].as<std::string>().data(),255);
+//    }
+    if(config["port"])
+    {
+        strncpy(gstr_port,config["port"].as<std::string>().data(),255);
+    }
+
+//    std::cout<<gstr_memname<<std::endl;
+//    std::cout<<gstr_rollang<<std::endl;
+//    std::cout<<gstr_inclinationang_xaxis<<std::endl;
+//    std::cout<<gstr_inclinationang_yaxis<<std::endl;
+//    std::cout<<gstr_hostip<<std::endl;
+//    std::cout<<gstr_port<<std::endl;
+}
+
+
+int main(int argc, char *argv[])
+{
+    showversion("driver_lidar_leishen16");
+    QCoreApplication a(argc, argv);
+
+    snprintf(gstr_memname,255,"lidar_pc");
+    snprintf(gstr_rollang,255,"270.0");//这个角度要根据安装情况进行校正！！！！！！！！
+    snprintf(gstr_inclinationang_xaxis,255,"0.0");
+    snprintf(gstr_inclinationang_yaxis,255,"0");
+//    snprintf(gstr_hostip,255,"192.168.1.102");
+    snprintf(gstr_port,255,"2368");//默认端口号
+    snprintf(gstr_yaml,255,"");
+    snprintf(gstr_modulename,255,"driver_lidar_leishen16");
+
+    int nRtn = GetOptLong(argc,argv);
+    if(nRtn == 1)  //show help,so exit.
+    {
+        return 0;
+    }
+
+    if(strnlen(gstr_yaml,255)>0)
+    {
+        decodeyaml(gstr_yaml);
+    }
+
+    gIvfault = new  iv::Ivfault(gstr_modulename);
+    gIvlog = new iv::Ivlog(gstr_modulename);
+    gIvfault->SetFaultState(0,0,"Initial State");
+    gIvlog->info("driver_lidar_leishen16 starting.");
+
+    StartLidar_leishen16();
+    return a.exec();
+}

src/ui/ADCIntelligentShow_grpc/gps_nbtype.h

@@ -0,0 +1,59 @@
+#ifndef GPS_NBTYPE_H
+#define GPS_NBTYPE_H
+namespace iv {
+    struct MAP_GPS_INS
+    {
+        int valid = 0xff;
+        int index = 0;	//gps点序号
+
+        double gps_lat = 0;//纬度
+        double gps_lng = 0;//经度
+
+        double gps_x = 0;
+        double gps_y = 0;
+        double gps_z = 0;
+
+        double ins_roll_angle = 0;	//横滚角 一般定义载体的右、前、上三个方向构成右手系，绕向前的轴旋转就是横滚角，绕向右的轴旋转就是俯仰角，绕向上的轴旋转就是航向角
+        double ins_pitch_angle = 0;	//俯仰角
+        double ins_heading_angle = 0;	//航向角
+
+        int ins_status = 0;	//惯导状态 4
+        int rtk_status = 0;	//rtk状态 6 -5 -3
+        int gps_satelites_num = 0;
+
+        //-----加速度--------------
+        double accel_x = 0;
+        double accel_y = 0;
+        double accel_z = 0;
+
+        //-------角速度------------
+        double ang_rate_x = 0;
+        double ang_rate_y = 0;
+        double ang_rate_z = 0;
+
+        //-----------方向速度--------------
+        double vel_N = 0;
+        double vel_E = 0;
+        double vel_D = 0;
+
+        int speed_mode = 0;
+        int mode2 = 0;
+        double speed = 3.5;			//速度  若导航点则为导航预设速度  若为当前点则为当前车速
+
+        int roadMode;
+        int runMode;
+        int roadSum;
+        int roadOri;
+
+    double mfLaneWidth = 3.5; // Current Lane Width
+
+    double mfDisToLaneLeft = 1.8; //Distance to Lane Left
+    int mnLaneChangeMark = 0; //1 to Left 0 not change   -1 to right
+    double mfDisToRoadLeft = 1.8; //Distance to Road Left
+    double mfRoadWidth = 3.5; // Road Width
+
+
+
+    };
+}
+#endif // GPS_NBTYPE_H