change pilot_apollo_bridge. prepare switch message.

src/apollo/modules/adc/pilot_apollo_bridge/BUILD

@@ -8,14 +8,19 @@ apollo_cc_binary(
     srcs = ["pilot_apollo_bridge.cc"],
     deps = [
         "//cyber",
+        "//modules/common/math",
+        "//modules/common/util:common_util",
+        "//modules/common/util:util_tool",
         "//modules/common/adapters:adapter_gflags",
         "//modules/adc/pilot_apollo_bridge/proto:gpsimu_proto",
         "//modules/common_msgs/localization_msgs:gps_proto",
+        "//modules/common_msgs/localization_msgs:localization_proto",
         "//modules/common_msgs/sensor_msgs:ins_cc_proto",
         "//modules/common_msgs/localization_msgs:imu_cc_proto",
         "@qt//:qt_core",
         "@eigen",
         "@proj",
+        "@geographic",
         "@modulecomm",
     ],
 

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

@@ -2,6 +2,8 @@
  #include "cyber/cyber.h"
  #include "cyber/time/rate.h"
  #include "cyber/time/time.h"
+ #include "modules/common/math/math_utils.h"
+#include "modules/common/math/quaternion.h"
  
  #include "modules/common/adapters/adapter_gflags.h"
  
@@ -9,12 +11,35 @@
  #include "modules/common_msgs/sensor_msgs/ins.pb.h"
 
 #include "modules/common_msgs/localization_msgs/imu.pb.h"
+
+#include "modules/common_msgs/localization_msgs/localization.pb.h"
+
+#include <GeographicLib/MGRS.hpp>
+#include <GeographicLib/UTMUPS.hpp>
+
+#include "modules/common/math/math_utils.h"
+#include "modules/common/math/quaternion.h"
+
+#include "modules/common/util/message_util.h"
+#include "modules/common/util/util.h"
  
  //car数据定义的引用，可以看出其定义来源于一个proto
 // using apollo::communication::proto::Car;
 
 using ::apollo::localization::Gps;
 
+void testutm()
+{
+    double x,y;
+    int zone{};
+    bool is_north{};
+
+    double flon = 117.0874969;
+    double flat = 39.1364644;
+        GeographicLib::UTMUPS::Forward(flat, flon, zone, is_north, x,y);
+
+    std::cout<<" x: "<<x<<" y:"<<y<<std::endl;
+}
 
 
  #include <iostream>
@@ -41,6 +66,9 @@ const char *WGS84_TEXT = "+proj=latlong +ellps=WGS84";
    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;
+   
+  std::shared_ptr<apollo::cyber::Writer<apollo::localization::LocalizationEstimate>>
+      localization_writer_;
 
 //const double DEG_TO_RAD = M_PI / 180.0;
 constexpr double DEG_TO_RAD_LOCAL = M_PI / 180.0;
@@ -137,6 +165,72 @@ static const int32_t LEAP_SECONDS[][2] = {
 
 const int32_t GPS_AND_SYSTEM_DIFF_SECONDS = 315964800;
 
+std::string stradcpose = "/apollo/localization/adcpose";
+
+using apollo::common::Point3D;
+using apollo::common::math::HeadingToQuaternion;
+using apollo::common::Quaternion;
+
+using apollo::common::math::InverseQuaternionRotate;
+using apollo::common::util::FillHeader;
+
+void TransformToVRF(const Point3D &point_mrf, const Quaternion &orientation,
+                    Point3D *point_vrf) {
+  Eigen::Vector3d v_mrf(point_mrf.x(), point_mrf.y(), point_mrf.z());
+  auto v_vrf = InverseQuaternionRotate(orientation, v_mrf);
+  point_vrf->set_x(v_vrf.x());
+  point_vrf->set_y(v_vrf.y());
+  point_vrf->set_z(v_vrf.z());
+}
+
+void PublishLocalization(const double x,const double y,const double z,const double yaw,const double v,const double kappa,const double a) {
+  auto localization = std::make_shared<apollo::localization::LocalizationEstimate>();
+  FillHeader("SimPerfectControl", localization.get());
+  auto *pose = localization->mutable_pose();
+
+  pose->mutable_position()->set_x(x);
+  pose->mutable_position()->set_y(y);
+  pose->mutable_position()->set_z(z);
+  // Set orientation and heading
+  double cur_theta = yaw;
+
+
+
+  Eigen::Quaternion<double> cur_orientation =
+      HeadingToQuaternion<double>(cur_theta);
+  pose->mutable_orientation()->set_qw(cur_orientation.w());
+  pose->mutable_orientation()->set_qx(cur_orientation.x());
+  pose->mutable_orientation()->set_qy(cur_orientation.y());
+  pose->mutable_orientation()->set_qz(cur_orientation.z());
+  pose->set_heading(cur_theta);
+
+  // Set linear_velocity
+  pose->mutable_linear_velocity()->set_x(std::cos(cur_theta) * v);
+  pose->mutable_linear_velocity()->set_y(std::sin(cur_theta) * v);
+  pose->mutable_linear_velocity()->set_z(0);
+
+  // Set angular_velocity in both map reference frame and vehicle reference
+  // frame
+  pose->mutable_angular_velocity()->set_x(0);
+  pose->mutable_angular_velocity()->set_y(0);
+  pose->mutable_angular_velocity()->set_z(v*kappa);
+
+  TransformToVRF(pose->angular_velocity(), pose->orientation(),
+                 pose->mutable_angular_velocity_vrf());
+
+  // Set linear_acceleration in both map reference frame and vehicle reference
+  // frame
+  auto *linear_acceleration = pose->mutable_linear_acceleration();
+  linear_acceleration->set_x(std::cos(cur_theta) * a);
+  linear_acceleration->set_y(std::sin(cur_theta) * a);
+  linear_acceleration->set_z(0);
+
+  TransformToVRF(pose->linear_acceleration(), pose->orientation(),
+                 pose->mutable_linear_acceleration_vrf());
+  localization_writer_->Write(localization);
+
+}
+
 template <class T, size_t N>
 constexpr size_t array_size(T (&)[N]) {
   return N;
@@ -187,6 +281,8 @@ void ListenGPSIMU(const char * strdata,const unsigned int nSize,const unsigned i
         return;
     }
     
+    std::cout<<"recv gpsimu."<<std::endl;
+    
     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;
     
@@ -243,12 +339,14 @@ void ListenGPSIMU(const char * strdata,const unsigned int nSize,const unsigned i
   auto *gps_msg = gps->mutable_localization();
 
   // 1. pose xyz
-  double x = ins->position().lon();
-  double y = ins->position().lat();
+  double x = ins->position().lat();
+  double y = ins->position().lon();
   x *= DEG_TO_RAD_LOCAL;
   y *= DEG_TO_RAD_LOCAL;
 
   pj_transform(wgs84pj_source_, utm_target_, 1, 1, &x, &y, NULL);
+  
+  std::cout<<" x: "<<x<<" y: "<<y<<std::endl;
 
   gps_msg->mutable_position()->set_x(x);
   gps_msg->mutable_position()->set_y(y);
@@ -288,6 +386,8 @@ void InitIns(){
 }
  
  int main(int argc, char *argv[]) {
+	 testutm();
+	 testutm();
    // 初始化一个cyber框架
    apollo::cyber::Init(argv[0]);
    // 创建talker节点
@@ -298,17 +398,19 @@ void 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);
+	localization_writer_ = talker_node->CreateWriter<apollo::localization::LocalizationEstimate>(stradcpose);
    // 从节点创建一个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;
+   
+   void * pasend = iv::modulecomm::RegisterSend("b1",1000,1);
  
    std::cout<<" enter talker."<<std::endl;
    //设置初始速度为0，然后速度每秒增加5km/h
@@ -319,6 +421,10 @@ void InitIns(){
        //假设车速持续增加
        speed += 5;
 //       talker->Write(msg);
+		        char str[1000];
+        int64_t xTime = std::chrono::system_clock::now().time_since_epoch().count();
+        memcpy(str,&xTime,8);
+        iv::modulecomm::ModuleSendMsg(pasend,str,30);
        sleep(1);
    }
    return 0;