ThirdParty : Add conti_radar callback

modules/third_party_perception/common/third_party_perception_gflags.cc

@@ -76,3 +76,7 @@ DEFINE_int32(
     "a radar object is considered as a movable "
     "if it is moving for consecutive movable_frames_count_threshold frames");
 DEFINE_int32(keep_radar_frames, 5, "number of delphi esr frames to keep");
+
+// TODO(QiL) : remove this temperary gflags
+DEFINE_bool(use_conti_radar, true,
+            "use conti or delphi radar, true is conti, false is delphi");

modules/third_party_perception/common/third_party_perception_gflags.h

@@ -57,4 +57,7 @@ DECLARE_double(movable_heading_threshold);
 DECLARE_int32(movable_frames_count_threshold);
 DECLARE_int32(keep_radar_frames);
 
+// TODO(QiL) : remove this temperary gflags
+DECLARE_bool(use_conti_radar);
+
 #endif

modules/third_party_perception/conf/adapter.conf

 config {
-  type: MOBILEYE 
+  type: MOBILEYE
   mode: RECEIVE_ONLY
 }
 config {
-  type: DELPHIESR 
+  type: DELPHIESR
+  mode: RECEIVE_ONLY
+}
+config: {
+  type: CONTI_RADAR
   mode: RECEIVE_ONLY
 }
 config {

modules/third_party_perception/third_party_perception.cc

@@ -33,6 +33,7 @@ namespace third_party_perception {
 using apollo::common::ErrorCode;
 using apollo::common::Status;
 using apollo::common::adapter::AdapterManager;
+using apollo::drivers::ContiRadar;
 using apollo::drivers::DelphiESR;
 using apollo::drivers::Esr_track01_500;
 using apollo::drivers::Mobileye;
@@ -50,14 +51,23 @@ Status ThirdPartyPerception::Init() {
 
   CHECK(AdapterManager::GetMobileye()) << "Mobileye is not initialized.";
   AdapterManager::AddMobileyeCallback(&ThirdPartyPerception::OnMobileye, this);
-  CHECK(AdapterManager::GetDelphiESR()) << "DelphiESR is not initialized.";
-  AdapterManager::AddDelphiESRCallback(&ThirdPartyPerception::OnDelphiESR,
-                                       this);
   CHECK(AdapterManager::GetLocalization())
       << "Localization is not initialized.";
   AdapterManager::AddLocalizationCallback(&ThirdPartyPerception::OnLocalization,
                                           this);
 
+  // TODO(all) : need to merge the delphi/conti_radar before adaptor manager
+  // level. This is just temperary change.
+  if (!FLAGS_use_conti_radar) {
+    CHECK(AdapterManager::GetDelphiESR()) << "DelphiESR is not initialized.";
+    AdapterManager::AddDelphiESRCallback(&ThirdPartyPerception::OnDelphiESR,
+                                         this);
+  } else {
+    CHECK(AdapterManager::GetContiRadar())
+        << "GetContiRadar is not initialized.";
+    AdapterManager::AddContiRadarCallback(&ThirdPartyPerception::OnContiRadar,
+                                          this);
+  }
   return Status::OK();
 }
 
@@ -91,7 +101,21 @@ void ThirdPartyPerception::OnDelphiESR(const DelphiESR& message) {
   RadarObstacles filtered_radar_obstacles =
       filter::FilterRadarObstacles(current_radar_obstacles_);
   if (FLAGS_enable_radar) {
-    delphi_esr_obstacles_ = conversion::RadarObstaclesToPerceptionObstacles(
+    radar_obstacles_ = conversion::RadarObstaclesToPerceptionObstacles(
+        filtered_radar_obstacles);
+  }
+}
+
+void ThirdPartyPerception::OnContiRadar(const ContiRadar& message) {
+  AINFO << "Received delphi esr data: run delphi esr callback.";
+  std::lock_guard<std::mutex> lock(third_party_perception_mutex_);
+  last_radar_obstacles_.CopyFrom(current_radar_obstacles_);
+  current_radar_obstacles_ = conversion::ContiToRadarObstacles(
+      message, localization_, last_radar_obstacles_);
+  RadarObstacles filtered_radar_obstacles =
+      filter::FilterRadarObstacles(current_radar_obstacles_);
+  if (FLAGS_enable_radar) {
+    radar_obstacles_ = conversion::RadarObstaclesToPerceptionObstacles(
         filtered_radar_obstacles);
   }
 }
@@ -108,13 +132,13 @@ void ThirdPartyPerception::OnTimer(const ros::TimerEvent&) {
   std::lock_guard<std::mutex> lock(third_party_perception_mutex_);
 
   PerceptionObstacles obstacles =
-      fusion::MobileyeRadarFusion(mobileye_obstacles_, delphi_esr_obstacles_);
+      fusion::MobileyeRadarFusion(mobileye_obstacles_, radar_obstacles_);
 
   AdapterManager::FillPerceptionObstaclesHeader(FLAGS_node_name, &obstacles);
   AdapterManager::PublishPerceptionObstacles(obstacles);
 
   mobileye_obstacles_.Clear();
-  delphi_esr_obstacles_.Clear();
+  radar_obstacles_.Clear();
 }
 
 }  // namespace third_party_perception

modules/third_party_perception/third_party_perception.h

@@ -28,6 +28,7 @@
 
 #include "modules/common/apollo_app.h"
 #include "modules/common/macro.h"
+#include "modules/drivers/proto/conti_radar.pb.h"
 #include "modules/drivers/proto/delphi_esr.pb.h"
 #include "modules/drivers/proto/mobileye.pb.h"
 #include "modules/localization/proto/localization.pb.h"
@@ -54,6 +55,7 @@ class ThirdPartyPerception : public apollo::common::ApolloApp {
   void OnMobileye(const apollo::drivers::Mobileye& message);
   // Upon receiving radar data
   void OnDelphiESR(const apollo::drivers::DelphiESR& message);
+  void OnContiRadar(const apollo::drivers::ContiRadar& message);
   // Upon receiving localization data
   void OnLocalization(
       const apollo::localization::LocalizationEstimate& message);
@@ -63,7 +65,7 @@ class ThirdPartyPerception : public apollo::common::ApolloApp {
   ros::Timer timer_;
   std::mutex third_party_perception_mutex_;
   apollo::perception::PerceptionObstacles mobileye_obstacles_;
-  apollo::perception::PerceptionObstacles delphi_esr_obstacles_;
+  apollo::perception::PerceptionObstacles radar_obstacles_;
   apollo::localization::LocalizationEstimate localization_;
   RadarObstacles current_radar_obstacles_;
   RadarObstacles last_radar_obstacles_;