add pilot_apollo_bridge. not complete.

src/apollo/modules/adc/pilot_apollo_bridge/BUILD

@@ -0,0 +1,25 @@
+load("//tools:apollo_package.bzl", "apollo_cc_binary", "apollo_cc_library", "apollo_cc_test", "apollo_component", "apollo_package")
+load("//tools:cpplint.bzl", "cpplint")
+ 
+package(default_visibility = ["//visibility:public"])
+ 
+apollo_cc_binary(
+    name = "pilot_apollo_bridge",
+    srcs = ["pilot_apollo_bridge.cc"],
+    deps = [
+        "//cyber",
+        "//modules/adc/pilot_apollo_bridge/proto:gpsimu_proto",
+        "//modules/common_msgs/localization_msgs:gps_proto",
+        "//modules/common_msgs/sensor_msgs:ins_cc_proto",
+        "@qt//:qt_core",
+        "@modulecomm",
+    ],
+
+    linkstatic = True,
+)
+
+ 
+apollo_package()
+ 
+cpplint()
+

src/apollo/modules/adc/pilot_apollo_bridge/pilot_apollo_bridge.cc

@@ -0,0 +1,236 @@
+#include "modules/adc/pilot_apollo_bridge/proto/gpsimu.pb.h"
+ #include "cyber/cyber.h"
+ #include "cyber/time/rate.h"
+ #include "cyber/time/time.h"
+ 
+ #include "modules/common_msgs/localization_msgs/gps.pb.h"
+ #include "modules/common_msgs/sensor_msgs/ins.pb.h"
+ 
+ //car数据定义的引用，可以看出其定义来源于一个proto
+// using apollo::communication::proto::Car;
+
+using ::apollo::localization::Gps;
+
+
+
+ #include <iostream>
+ #include <chrono>
+ 
+ #include <qglobal.h>
+ 
+ #include "modulecomm.h"
+ 
+ 
+::apollo::drivers::gnss::Ins ins_;
+::apollo::drivers::gnss::InsStat ins_stat_;
+
+constexpr double DEG_TO_RAD = M_PI / 180.0;
+constexpr float FLOAT_NAN = std::numeric_limits<float>::quiet_NaN();
+
+constexpr double azimuth_deg_to_yaw_rad(double azimuth) {
+  return (90.0 - azimuth) * DEG_TO_RAD;
+}
+
+// File expires on:  December 2017
+
+// There's two concept about 'time'.
+// 1. Unix time : It counts seconds since Jan 1,1970.
+//                Unix time doesn't include the leap seconds.
+//
+// 2. GPS time : It counts seconds since Jan 6,1980.
+//               GPS time does include the leap seconds.
+//
+// Leap seconds was added in every a few years, See details:
+// http://tycho.usno.navy.mil/leapsec.html
+// https://en.wikipedia.org/wiki/Leap_second
+//
+
+/* leap_seconds table since 1980.
+ +======+========+========+======+========+========+
+ | Year | Jun 30 | Dec 31 | Year | Jun 30 | Dec 31 |
+ +======+========+========+======+========+========+
+ | 1980 | (already +19)   | 1994 | +1     | 0      |
+ +------+--------+--------+------+--------+--------+
+ | 1981 | +1     | 0      | 1995 | 0      | +1     |
+ +------+--------+--------+------+--------+--------+
+ | 1982 | +1     | 0      | 1997 | +1     | 0      |
+ +------+--------+--------+------+--------+--------+
+ | 1983 | +1     | 0      | 1998 | 0      | +1     |
+ +------+--------+--------+------+--------+--------+
+ | 1985 | +1     | 0      | 2005 | 0      | +1     |
+ +------+--------+--------+------+--------+--------+
+ | 1987 | 0      | +1     | 2008 | 0      | +1     |
+ +------+--------+--------+------+--------+--------+
+ | 1989 | 0      | +1     | 2012 | +1     | 0      |
+ +------+--------+--------+------+--------+--------+
+ | 1990 | 0      | +1     | 2015 | +1     | 0      |
+ +------+--------+--------+------+--------+--------+
+ | 1992 | +1     | 0      | 2016 | 0      | +1     |
+ +------+--------+--------+------+--------+--------+
+ | 1993 | +1     | 0      | 2017 | 0      | 0      |
+ +------+--------+--------+------+--------+--------+
+
+ Current TAI - UTC = 37. (mean that: 2017 - 1970/01/01 = 37 seconds)
+*/
+
+// We build a lookup table to describe relationship that between UTC and
+// Leap_seconds.
+//
+// Column1: UTC time diff (second).
+//          Shell Example:
+//                 date +%s -d"Jan 1, 2017 00:00:00"
+//          return 1483257600.
+//
+//                 date +%s -d"Jan 1, 1970 00:00:00"
+//          return 28800.
+//
+//          The diff between 1970/01/01 and 2017/01/01 is 1483228800.
+//          (1483257600-28800)
+//
+// Column2: Leap seconds diff with GPS basetime(Jan 6,1980).
+//          We Know that The leap_seconds have been already added 37 seconds
+//          util now(2017).
+//          So we can calculate leap_seconds diff between GPS (from Jan 6,1980)
+//          and UTC time.
+
+// calc with the formula.
+static const int32_t LEAP_SECONDS[][2] = {
+    {1483228800, 18},  // 2017/01/01
+    {1435708800, 17},  // 2015/07/01
+    {1341100800, 16},  // 2012/07/01
+    {1230768000, 15},  // 2009/01/01
+    {1136073600, 14},  // 2006/01/01
+    {915148800, 13},   // 1999/01/01
+    {867711600, 12},   // 1997/07/01
+    {820480320, 11},   // 1996/01/01 ;)
+                       //....
+                       //..
+                       // etc.
+};
+
+// seconds that UNIX time afront of GPS, without leap seconds.
+
+// Shell:
+// time1 = date +%s -d"Jan 6, 1980 00:00:00"
+// time2 = date +%s -d"Jan 1, 1970 00:00:00"
+// dif_tick = time1-time2
+// 315964800 = 315993600 - 28800
+
+const int32_t GPS_AND_SYSTEM_DIFF_SECONDS = 315964800;
+
+template <class T, size_t N>
+constexpr size_t array_size(T (&)[N]) {
+  return N;
+}
+
+template <typename T>
+T unix2gps(const T unix_seconds) {
+  for (size_t i = 0; i < array_size(LEAP_SECONDS); ++i) {
+    if (unix_seconds >= LEAP_SECONDS[i][0]) {
+      return unix_seconds - (GPS_AND_SYSTEM_DIFF_SECONDS - LEAP_SECONDS[i][1]);
+    }
+  }
+  return static_cast<T>(0);
+}
+
+template <typename T>
+T gps2unix(const T gps_seconds) {
+  for (size_t i = 0; i < array_size(LEAP_SECONDS); ++i) {
+    T result = gps_seconds + (GPS_AND_SYSTEM_DIFF_SECONDS - LEAP_SECONDS[i][1]);
+    if (result >= LEAP_SECONDS[i][0]) {
+      return result;
+    }
+  }
+  return static_cast<T>(0);
+}
+ 
+ enum class InsStatus : uint32_t {
+  INACTIVE = 0,
+  ALIGNING,
+  HIGH_VARIANCE,
+  SOLUTION_GOOD,
+  SOLUTION_FREE = 6,
+  ALIGNMENT_COMPLETE,
+  DETERMINING_ORIENTATION,
+  WAITING_INITIAL_POS,
+  NONE = std::numeric_limits<uint32_t>::max(),
+};
+
+void ListenGPSIMU(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+    (void)index;
+    (void)dt;
+    (void)strmemname;
+    iv::gps::gpsimu xgpsimu;
+    if(!xgpsimu.ParseFromArray(strdata,nSize))
+    {
+        std::cout<<"ListenRaw Parse error."<<std::endl;
+        return;
+    }
+    
+    int64_t timenow = std::chrono::system_clock::now().time_since_epoch().count();
+    double seconds = timenow * 1e-9;//gps_week * SECONDS_PER_WEEK + gps_millisecs * 1e-3;
+    
+    ins_.mutable_position()->set_lon(xgpsimu.lon());
+	ins_.mutable_position()->set_lat(xgpsimu.lat());
+	ins_.mutable_position()->set_height(xgpsimu.height());
+	ins_.mutable_euler_angles()->set_x(xgpsimu.roll() * DEG_TO_RAD);
+	ins_.mutable_euler_angles()->set_y(-xgpsimu.pitch() * DEG_TO_RAD);
+	ins_.mutable_euler_angles()->set_z(azimuth_deg_to_yaw_rad(xgpsimu.heading()));
+	ins_.mutable_linear_velocity()->set_x(xgpsimu.ve());
+	ins_.mutable_linear_velocity()->set_y(xgpsimu.vn());
+	ins_.mutable_linear_velocity()->set_z(xgpsimu.vd());
+	ins_.set_type(apollo::drivers::gnss::Ins::GOOD);
+	
+//	ins_.set_measurement_time(seconds);
+	ins_.set_measurement_time(unix2gps(seconds));
+	ins_.mutable_header()->set_timestamp_sec(apollo::cyber::Time::Now().ToSecond());
+	
+	
+	
+	
+	double unix_sec = seconds;//apollo::drivers::util::gps2unix(seconds);
+	ins_stat_.mutable_header()->set_timestamp_sec(unix_sec);
+	ins_stat_.set_ins_status(3);
+	ins_stat_.set_pos_type(xgpsimu.rtk_state());
+
+}
+
+void InitIns(){
+  ins_.mutable_position_covariance()->Resize(9, FLOAT_NAN);
+  ins_.mutable_euler_angles_covariance()->Resize(9, FLOAT_NAN);
+  ins_.mutable_linear_velocity_covariance()->Resize(9, FLOAT_NAN);
+}
+ 
+ int main(int argc, char *argv[]) {
+   // 初始化一个cyber框架
+   apollo::cyber::Init(argv[0]);
+   // 创建talker节点
+   auto talker_node = apollo::cyber::CreateNode("pilot_apollo_bridge");
+   
+   InitIns();
+   
+   void * paraw = iv::modulecomm::RegisterRecv("hcp2_gpsimu",ListenGPSIMU);
+   (void)paraw;
+   
+   std::shared_ptr<apollo::cyber::Writer<apollo::localization::Gps>>
+      gps_writer_ = talker_node->CreateWriter<Gps>("gps");
+   // 从节点创建一个Topic,来实现对车速的查看
+//   auto talker = talker_node->CreateWriter<Car>("car_speed");
+//   AINFO << "I'll start telling you the current speed of the car.";
+ 
+   std::cout<<" enter talker."<<std::endl;
+   //设置初始速度为0，然后速度每秒增加5km/h
+   uint64_t speed = 0;
+   while (apollo::cyber::OK()) {
+//       auto msg = std::make_shared<Car>();
+//       msg->set_speed(speed);
+       //假设车速持续增加
+       speed += 5;
+//       talker->Write(msg);
+       sleep(1);
+   }
+   return 0;
+ }
+
+

src/apollo/modules/adc/pilot_apollo_bridge/proto/BUILD

@@ -0,0 +1,12 @@
+## Auto generated by `proto_build_generator.py`
+load("//tools/proto:proto.bzl", "proto_library")
+load("//tools:apollo_package.bzl", "apollo_package")
+
+package(default_visibility = ["//visibility:public"])
+
+proto_library(
+    name = "gpsimu_proto",
+    srcs = ["gpsimu.proto"],
+)
+
+apollo_package()

src/apollo/modules/adc/pilot_apollo_bridge/proto/gpsimu.proto

@@ -0,0 +1,82 @@
+syntax = "proto2";
+
+package iv.gps;
+
+message pos_accuracy_def
+{
+ optional double latstd = 1;
+ optional double lonstd = 2;
+ optional double hstd = 3;
+};
+
+message vel_accuracy_def
+{
+ optional double vnstd = 1;
+ optional double vestd = 2;
+ optional double vdstd = 3;
+};
+
+message pose_accuracy_def
+{
+ optional double rollstd = 1;
+ optional double pitchstd = 2;
+ optional double yawstd = 3;
+};
+
+message dev_temp_def
+{
+ optional double temp = 1;
+};
+
+message gps_state_def
+{
+ optional int32 pos_state = 1;
+ optional int32 satnum = 2;
+ optional int32 heading_state = 3;
+};
+
+message wheel_state_def
+{
+ optional int32 wheeldata = 1;
+};
+
+
+
+message gpsimu
+{
+ optional double pitch = 1;
+ optional double roll = 2;
+ optional double heading = 3;
+ optional double gyro_x = 4; //惯导x轴角速度
+ optional double gyro_y = 5; //惯导y轴角速度
+ optional double gyro_z = 6; //惯导z轴角速度
+ optional double acce_x = 7;
+ optional double acce_y = 8;
+ optional double acce_z = 9;
+ optional double lat = 10;
+ optional double lon = 11;
+ optional double height = 12;
+ optional double vn = 13;//北向速度
+ optional double ve = 14;//东向速度
+ optional double vd = 15;//地向速度
+ optional int32 state = 16;
+ optional int32 type = 17;
+ optional pos_accuracy_def pos_accuracy = 18;
+ optional vel_accuracy_def vel_accuracy = 19;
+ optional dev_temp_def dev_temp = 20;
+ optional gps_state_def gps_state = 21;
+ optional wheel_state_def wheel_state = 22;
+ optional double time = 23;
+ optional bytes check = 24;
+ optional pose_accuracy_def pose_accuracy = 25;
+ optional int64 msgtime = 26;
+ optional double rtk_state = 27;
+ optional double ins_state = 28;
+ optional double acc_calc = 29;
+ optional int32 satnum1 = 30;
+ optional int32 satnum2 = 31;
+ optional int32 gpsweek = 32; //from 1980-1-6 weeks.
+ optional int32 gpstime = 33; //from sunday 0:00:00 seconds.
+ optional double speed = 34;
+
+};