change pilot_apollo_bridge, complete gps message, but not test.

src/apollo/modules/adc/pilot_apollo_bridge/BUILD

@@ -14,6 +14,8 @@ apollo_cc_binary(
         "//modules/common_msgs/sensor_msgs:ins_cc_proto",
         "//modules/common_msgs/localization_msgs:imu_cc_proto",
         "@qt//:qt_core",
+        "@eigen",
+        "@proj",
         "@modulecomm",
     ],
 

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

@@ -22,13 +22,28 @@ using ::apollo::localization::Gps;
  
  #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_;
 
-constexpr double DEG_TO_RAD = M_PI / 180.0;
+   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) {
@@ -197,6 +212,72 @@ void ListenGPSIMU(const char * strdata,const unsigned int nSize,const unsigned i
 	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);
 
 }
 
@@ -214,17 +295,20 @@ void InitIns(){
    
    InitIns();
    
-   void * paraw = iv::modulecomm::RegisterRecv("hcp2_gpsimu",ListenGPSIMU);
-   (void)paraw;
+   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");
    
-   std::shared_ptr<apollo::cyber::Writer<apollo::localization::Gps>>
-   gps_writer_ = talker_node->CreateWriter<Gps>("gps");
+
    
-   std::shared_ptr<apollo::cyber::Writer<InsStatus>> insstatus_writer_ = talker_node->CreateWriter<InsStatus>(FLAGS_ins_status_topic);
-   std::shared_ptr<apollo::cyber::Writer<apollo::localization::CorrectedImu>> corrimu_writer_ = talker_node->CreateWriter<apollo::localization::CorrectedImu>(FLAGS_imu_topic);
+	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