change pilot_apollo_bridge. add tracked object.

src/apollo/modules/adc/pilot_apollo_bridge/BUILD

@@ -14,11 +14,13 @@ apollo_cc_binary(
         "//modules/common/adapters:adapter_gflags",
         "//modules/adc/pilot_apollo_bridge/proto:gpsimu_proto",
         "//modules/adc/pilot_apollo_bridge/proto:apolloctrlcmd_proto",
+        "//modules/adc/pilot_apollo_bridge/proto:objectarray_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",
         "//modules/common_msgs/control_msgs:control_cmd_proto",
+        "//modules/common_msgs/perception_msgs:perception_obstacle_proto",
         "@qt//:qt_core",
         "@eigen",
         "@proj",

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

@@ -1,5 +1,6 @@
 #include "modules/adc/pilot_apollo_bridge/proto/gpsimu.pb.h"
 #include "modules/adc/pilot_apollo_bridge/proto/apolloctrlcmd.pb.h"
+#include "modules/adc/pilot_apollo_bridge/proto/objectarray.pb.h"
 #include "cyber/cyber.h"
 #include "cyber/time/rate.h"
 #include "cyber/time/time.h"
@@ -10,19 +11,16 @@
  
 #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"
-
 #include "modules/common_msgs/localization_msgs/localization.pb.h"
-
 #include "modules/common_msgs/control_msgs/control_cmd.pb.h"
+#include "modules/common_msgs/perception_msgs/perception_obstacle.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"
  
@@ -31,22 +29,11 @@
 
 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>
  #include <chrono>
+ #include <thread>
  
  #include <qglobal.h>
  
@@ -69,6 +56,8 @@ 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::perception::PerceptionObstacles>>  perception_writer_ = nullptr; 
    
   std::shared_ptr<apollo::cyber::Writer<apollo::localization::LocalizationEstimate>>
       localization_writer_;
@@ -77,6 +66,11 @@ const char *WGS84_TEXT = "+proj=latlong +ellps=WGS84";
       control_command_reader_;
       
 void * gpadecision;
+void * gpalidartrack;
+
+double gfutm_x = 0;
+double gfutm_y = 0;
+double gfutm_hdg = 0;
 
 //const double DEG_TO_RAD = M_PI / 180.0;
 constexpr double DEG_TO_RAD_LOCAL = M_PI / 180.0;
@@ -244,11 +238,11 @@ void onControl(
     const std::shared_ptr<apollo::control::ControlCommand> &xCmd) {
 //  std::lock_guard<std::mutex> lock(mutex_);
 
-  std::cout<<"steer: "<<xCmd->steering_target()<<" acc: "<<xCmd->acceleration()<<" brake:"<<xCmd->brake()<<std::endl;
+//  std::cout<<"steer: "<<xCmd->steering_target()<<" acc: "<<xCmd->acceleration()<<" brake:"<<xCmd->brake()<<std::endl;
   
     iv::apollo::apolloctrlcmd xctrlcmd;
     xctrlcmd.set_acceleration(xCmd->acceleration());
-    xctrlcmd.set_brake(xCmd->brake());
+    xctrlcmd.set_brake(xCmd->brake()); 
     xctrlcmd.set_steering_target(xCmd->steering_target());
     
     int nbytesize = (int)xctrlcmd.ByteSize();
@@ -311,7 +305,7 @@ void ListenGPSIMU(const char * strdata,const unsigned int nSize,const unsigned i
         return;
     }
     
-    std::cout<<"recv gpsimu."<<std::endl;
+ //   std::cout<<"recv gpsimu."<<std::endl;
     
     double flon = xgpsimu.lon();
     double flat = xgpsimu.lat();
@@ -322,6 +316,8 @@ void ListenGPSIMU(const char * strdata,const unsigned int nSize,const unsigned i
     GeographicLib::UTMUPS::Forward(flat, flon, zone, is_north, utm_x,utm_y);
     
     PublishLocalization(utm_x,utm_y,xgpsimu.height(),(90-xgpsimu.heading())*M_PI/180.0,xgpsimu.speed(),0,0);
+
+    gfutm_x = utm_x; gfutm_y = utm_y; gfutm_hdg = (90-xgpsimu.heading())*M_PI/180.0;
     
     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;
@@ -386,7 +382,7 @@ void ListenGPSIMU(const char * strdata,const unsigned int nSize,const unsigned i
 
   pj_transform(wgs84pj_source_, utm_target_, 1, 1, &x, &y, NULL);
   
-  std::cout<<" x: "<<x<<" y: "<<y<<std::endl;
+ // std::cout<<" x: "<<x<<" y: "<<y<<std::endl;
 
   gps_msg->mutable_position()->set_x(x);
   gps_msg->mutable_position()->set_y(y);
@@ -419,15 +415,146 @@ void ListenGPSIMU(const char * strdata,const unsigned int nSize,const unsigned i
 
 }
 
+void ListenLiarTrack(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname){
+  iv::lidar::objectarray xobjarray;
+
+  if(!xobjarray.ParseFromArray(strdata,nSize))
+  {
+    std::cout<<" Parse Object Array Fail."<<std::endl;
+    return;
+  }
+
+  int nsize = static_cast<int>(xobjarray.obj_size());
+  int i;
+  auto perceptionobjs = std::make_shared<apollo::perception::PerceptionObstacles>();
+  FillHeader("SimPerfectControl", perceptionobjs.get());
+  for(i=0;i<nsize;i++){
+
+    iv::lidar::lidarobject * plidarobj = xobjarray.mutable_obj(i);
+    auto pobj = perceptionobjs->add_perception_obstacle();
+
+    double xbase = gfutm_x;
+    double ybase = gfutm_y;
+    pobj->set_id(i);
+    apollo::common::Point3D  * ppos = new apollo::common::Point3D();
+    double xrot = 0;
+    double yrot = 0;
+  //  std::cout<<" obj x: "<<plidarobj->position().x()<<" y:"<<plidarobj->position().y()<<std::endl;
+    xrot = plidarobj->position().y() * cos(gfutm_hdg) - plidarobj->position().x() * (-1.0) * sin(gfutm_hdg);
+    yrot = plidarobj->position().y() * sin(gfutm_hdg) + plidarobj->position().x() * (-1.0) * cos(gfutm_hdg);
+    ppos->set_x(xrot + xbase);
+    ppos->set_y(yrot + ybase);
+    ppos->set_z(plidarobj->position().z());
+    pobj->set_allocated_position(ppos);
+
+    double fobjhdg = plidarobj->tyaw() + gfutm_hdg;
+
+    double xp[4],yp[4];
+    double fpedlen = plidarobj->dimensions().x();
+    double fpedwidth = plidarobj->dimensions().y();
+    double fhdg = fobjhdg - M_PI/2.0;
+    double x = xrot;
+    double y = yrot;
+    xp[0] = x + (0 - fpedlen/2.0) * cos(fhdg) - (0 - fpedwidth/2.0) * sin(fhdg);
+    yp[0] = y + (0 - fpedlen/2.0) * sin(fhdg) + (0 - fpedwidth/2.0) * cos(fhdg);
+    xp[1] = x + (0 + fpedlen/2.0) * cos(fhdg) - (0 - fpedwidth/2.0) * sin(fhdg);
+    yp[1] = y + (0 + fpedlen/2.0) * sin(fhdg) + (0 - fpedwidth/2.0) * cos(fhdg);
+    xp[2] = x + (0 + fpedlen/2.0) * cos(fhdg) - (0 + fpedwidth/2.0) * sin(fhdg);
+    yp[2] = y + (0 + fpedlen/2.0) * sin(fhdg) + (0 + fpedwidth/2.0) * cos(fhdg);
+    xp[3] = x + (0 - fpedlen/2.0) * cos(fhdg) - (0 + fpedwidth/2.0) * sin(fhdg);
+    yp[3] = y + (0 - fpedlen/2.0) * sin(fhdg) + (0 + fpedwidth/2.0) * cos(fhdg);
+    apollo::common::Point3D * p1 = pobj->add_polygon_point();
+    p1->set_x(xp[1]+ xbase);p1->set_y(yp[1]+ ybase);p1->set_z(0);
+    apollo::common::Point3D * p2 = pobj->add_polygon_point();
+    p2->set_x(xp[2]+ xbase);p2->set_y(yp[2]+ ybase);p2->set_z(0);
+    apollo::common::Point3D * p3 = pobj->add_polygon_point();
+    p3->set_x(xp[3]+ xbase);p3->set_y(yp[3]+ ybase);p3->set_z(0);
+    apollo::common::Point3D * p4 = pobj->add_polygon_point();
+    p4->set_x(xp[0]+ xbase);p4->set_y(yp[0]+ ybase);p4->set_z(0);
+
+    pobj->set_length(fpedlen);
+    pobj->set_width(fpedwidth);
+    pobj->set_height(plidarobj->dimensions().z());
+
+    pobj->set_tracking_time(1.0);
+
+    apollo::common::Point3D  * pvel = new apollo::common::Point3D();
+    pvel->set_x(0); pvel->set_y(0); pvel->set_z(0);
+    pobj->set_allocated_velocity(pvel);
+
+    apollo::common::Point3D * paccel = new apollo::common::Point3D();
+    paccel->set_x(0); paccel->set_y(0); paccel->set_z(0);
+    pobj->set_allocated_acceleration(paccel);
+
+    pobj->set_type(apollo::perception::PerceptionObstacle_Type_VEHICLE);
+
+    
+  }
+
+  perception_writer_->Write(perceptionobjs);
+
+}
+
 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);
 }
+
+void threadtestobj()
+{
+  while(1)
+  {
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    std::cout<<" send obj"<<std::endl;
+
+    auto perceptionobjs = std::make_shared<apollo::perception::PerceptionObstacles>();
+    FillHeader("SimPerfectControl", perceptionobjs.get());
+
+    auto pobj = perceptionobjs->add_perception_obstacle();
+
+    double xbase = 507503.95;
+    double ybase = 4331922.95;
+    pobj->set_id(0);
+    apollo::common::Point3D  * ppos = new apollo::common::Point3D();
+    ppos->set_x(10 + xbase);
+    ppos->set_y(0 + ybase);
+    ppos->set_z(0);
+    pobj->set_allocated_position(ppos);
+
+    apollo::common::Point3D * p1 = pobj->add_polygon_point();
+    p1->set_x(12.5+ xbase);p1->set_y(-0.8+ ybase);p1->set_z(0);
+    apollo::common::Point3D * p2 = pobj->add_polygon_point();
+    p2->set_x(12.5+ xbase);p2->set_y(0.8+ ybase);p2->set_z(0);
+    apollo::common::Point3D * p3 = pobj->add_polygon_point();
+    p3->set_x(7.5+ xbase);p3->set_y(0.8+ ybase);p3->set_z(0);
+    apollo::common::Point3D * p4 = pobj->add_polygon_point();
+    p4->set_x(7.5+ xbase);p4->set_y(-0.8+ ybase);p4->set_z(0);
+
+    pobj->set_length(5);
+    pobj->set_width(2.5);
+    pobj->set_height(1.6);
+
+    pobj->set_tracking_time(1.0);
+
+    apollo::common::Point3D  * pvel = new apollo::common::Point3D();
+    pvel->set_x(0); pvel->set_y(0); pvel->set_z(0);
+    pobj->set_allocated_velocity(pvel);
+
+    apollo::common::Point3D * paccel = new apollo::common::Point3D();
+    paccel->set_x(0); paccel->set_y(0); paccel->set_z(0);
+    pobj->set_allocated_acceleration(paccel);
+
+    pobj->set_type(apollo::perception::PerceptionObstacle_Type_VEHICLE);
+
+    perception_writer_->Write(perceptionobjs);
+
+    
+
+  }
+}
  
  int main(int argc, char *argv[]) {
-	 testutm();
-	 testutm();
    // 初始化一个cyber框架
    apollo::cyber::Init(argv[0]);
    // 创建talker节点
@@ -443,6 +570,8 @@ void InitIns(){
 	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);
+
+  perception_writer_ = talker_node->CreateWriter<apollo::perception::PerceptionObstacles>("/apollo/perception/obstacles");
 	
     control_command_reader_ = talker_node->CreateReader<apollo::control::ControlCommand>(
       "/apollo/control",onControl);
@@ -452,6 +581,8 @@ void InitIns(){
 
    void * paraw = iv::modulecomm::RegisterRecv("hcp2_gpsimu",ListenGPSIMU);
    (void)paraw;
+
+   gpalidartrack = iv::modulecomm::RegisterRecv("lidar_track",ListenLiarTrack);
    
    void * pasend = iv::modulecomm::RegisterSend("b1",1000,1);
    
@@ -459,10 +590,12 @@ void InitIns(){
    
    gpadecision = pactrl;
  
-   std::cout<<" enter talker."<<std::endl;
+   std::cout<<" enter talker."<<std::endl;  
    //设置初始速度为0，然后速度每秒增加5km/h
    uint64_t speed = 0;
 
+//   std::thread * pthread = new std::thread(threadtestobj);
+
    while (apollo::cyber::OK()) {
 //       auto msg = std::make_shared<Car>();
 //       msg->set_speed(speed);

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

@@ -14,4 +14,13 @@ proto_library(
     srcs = ["apolloctrlcmd.proto"],
 )
 
+
+
+proto_library(
+    name = "objectarray_proto",
+    srcs = ["objectarray.proto"],
+)
+
+
+
 apollo_package()

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

@@ -0,0 +1,59 @@
+syntax = "proto2";
+
+package iv.lidar;
+
+
+message PointXYZI {
+  optional float x = 1 [default = nan];
+  optional float y = 2 [default = nan];
+  optional float z = 3 [default = nan];
+  optional float i = 4 [default = 0];
+};
+
+message PointXYZ {
+  optional float x = 1 [default = nan];
+  optional float y = 2 [default = nan];
+  optional float z = 3 [default = nan];
+};
+
+message Dimension {
+  optional float x = 1 [default = 0];
+  optional float y = 2 [default = 0];
+  optional float z = 3 [default = 0];
+};
+
+message VelXY {
+  optional float x =1 [default = 0];
+  optional float y =2 [default = 0];
+};
+
+message lidarobject
+{
+  optional PointXYZ min_point = 1;
+  optional PointXYZ max_point = 2;
+  optional PointXYZ centroid = 3;
+  optional Dimension dimensions = 4;
+  optional PointXYZ position = 5;
+  optional double angle =6;
+  optional double velocity_linear_x = 7;
+  optional double acceleration_linear_y = 8;
+  optional double timestamp = 9;
+  optional double tyaw = 10;
+  required int32 mnType = 11;
+  optional int32 id = 12;
+  optional int32 behavior_state = 13;
+  optional float score = 14;
+  optional bool velocity_reliable = 15;
+  optional bool pose_reliable = 16;
+  repeated float type_probs = 17;
+  repeated PointXYZI cloud = 18;
+  optional string type_name = 19;
+  optional VelXY vel_relative = 20; //相对速度
+};
+
+
+message objectarray
+{
+  repeated lidarobject obj = 1;
+  optional double timestamp = 2;
+};