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

#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/adapters/adapter_gflags.h"
 
 #include "modules/common_msgs/localization_msgs/gps.pb.h"
 #include "modules/common_msgs/sensor_msgs/ins.pb.h"

#include "modules/common_msgs/localization_msgs/imu.pb.h"
 
 //car数据定义的引用，可以看出其定义来源于一个proto
// using apollo::communication::proto::Car;

using ::apollo::localization::Gps;



 #include <iostream>
 #include <chrono>
 
 #include <qglobal.h>
 
 #include "Eigen/Geometry"
 
 #include "modulecomm.h"
 
#define ACCEPT_USE_OF_DEPRECATED_PROJ_API_H
 #include <proj_api.h>
 
 
projPJ wgs84pj_source_;
projPJ utm_target_;
 
const char *WGS84_TEXT = "+proj=latlong +ellps=WGS84";

::apollo::drivers::gnss::Ins ins_;
::apollo::drivers::gnss::InsStat ins_stat_;

   std::shared_ptr<apollo::cyber::Writer<apollo::localization::Gps>> gps_writer_ = nullptr;
   std::shared_ptr<apollo::cyber::Writer<apollo::drivers::gnss::InsStat>> insstat_writer_ = nullptr;
   std::shared_ptr<apollo::cyber::Writer<apollo::localization::CorrectedImu>> corrimu_writer_ = nullptr;

//const double DEG_TO_RAD = M_PI / 180.0;
constexpr double DEG_TO_RAD_LOCAL = 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());
	
	
	  ::apollo::drivers::gnss::Ins *ins = &ins_;
  auto imu = std::make_shared<apollo::localization::CorrectedImu>();
//  double unix_sec = seconds;//apollo::drivers::util::gps2unix(ins->measurement_time());
  imu->mutable_header()->set_timestamp_sec(unix_sec);

  auto *imu_msg = imu->mutable_imu();
  imu_msg->mutable_linear_acceleration()->set_x(
      -ins->linear_acceleration().y());
  imu_msg->mutable_linear_acceleration()->set_y(ins->linear_acceleration().x());
  imu_msg->mutable_linear_acceleration()->set_z(ins->linear_acceleration().z());

  imu_msg->mutable_angular_velocity()->set_x(-ins->angular_velocity().y());
  imu_msg->mutable_angular_velocity()->set_y(ins->angular_velocity().x());
  imu_msg->mutable_angular_velocity()->set_z(ins->angular_velocity().z());

  imu_msg->mutable_euler_angles()->set_x(ins->euler_angles().x());
  imu_msg->mutable_euler_angles()->set_y(-ins->euler_angles().y());
  imu_msg->mutable_euler_angles()->set_z(ins->euler_angles().z() -
                                         90 * DEG_TO_RAD_LOCAL);

  corrimu_writer_->Write(imu);
  
    auto gps = std::make_shared<apollo::localization::Gps>();

//  double unix_sec = apollo::drivers::util::gps2unix(ins->measurement_time());
  gps->mutable_header()->set_timestamp_sec(unix_sec);
  auto *gps_msg = gps->mutable_localization();

  // 1. pose xyz
  double x = ins->position().lon();
  double y = ins->position().lat();
  x *= DEG_TO_RAD_LOCAL;
  y *= DEG_TO_RAD_LOCAL;

  pj_transform(wgs84pj_source_, utm_target_, 1, 1, &x, &y, NULL);

  gps_msg->mutable_position()->set_x(x);
  gps_msg->mutable_position()->set_y(y);
  gps_msg->mutable_position()->set_z(ins->position().height());

  // 2. orientation
  Eigen::Quaterniond q =
      Eigen::AngleAxisd(ins->euler_angles().z() - 90 * DEG_TO_RAD_LOCAL,
                        Eigen::Vector3d::UnitZ()) *
      Eigen::AngleAxisd(-ins->euler_angles().y(), Eigen::Vector3d::UnitX()) *
      Eigen::AngleAxisd(ins->euler_angles().x(), Eigen::Vector3d::UnitY());

  gps_msg->mutable_orientation()->set_qx(q.x());
  gps_msg->mutable_orientation()->set_qy(q.y());
  gps_msg->mutable_orientation()->set_qz(q.z());
  gps_msg->mutable_orientation()->set_qw(q.w());

  gps_msg->mutable_linear_velocity()->set_x(ins->linear_velocity().x());
  gps_msg->mutable_linear_velocity()->set_y(ins->linear_velocity().y());
  gps_msg->mutable_linear_velocity()->set_z(ins->linear_velocity().z());

  gps_writer_->Write(gps);
  
  auto ins_stat = std::make_shared<apollo::drivers::gnss::InsStat>();
 // common::util::FillHeader("gnss", ins_stat);
 	ins_stat->mutable_header()->set_timestamp_sec(unix_sec);
	ins_stat->set_ins_status(3);
	ins_stat->set_pos_type(xgpsimu.rtk_state());
  insstat_writer_->Write(ins_stat);

}

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();
   
   wgs84pj_source_ = pj_init_plus(WGS84_TEXT);
   utm_target_ = pj_init_plus("+proj=utm +zone=10 +ellps=WGS84 +towgs84=0,0,0,0,0,0,0 +units=m +no_defs");
   

   
	gps_writer_ = talker_node->CreateWriter<Gps>(FLAGS_gps_topic);
	insstat_writer_ = talker_node->CreateWriter<apollo::drivers::gnss::InsStat>(FLAGS_ins_stat_topic);
	corrimu_writer_ = talker_node->CreateWriter<apollo::localization::CorrectedImu>(FLAGS_imu_topic);
   // 从节点创建一个Topic,来实现对车速的查看
//   auto talker = talker_node->CreateWriter<Car>("car_speed");
//   AINFO << "I'll start telling you the current speed of the car.";

   void * paraw = iv::modulecomm::RegisterRecv("hcp2_gpsimu",ListenGPSIMU);
   (void)paraw;
 
   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;
 }