diff --git a/modules/third_party_perception/common/third_party_perception_gflags.cc b/modules/third_party_perception/common/third_party_perception_gflags.cc index 6cfa719d4c67481a82e9172db990d634fe567a9f..5ae289ef59a3f865156991e9eaf7f480e02af322 100644 --- a/modules/third_party_perception/common/third_party_perception_gflags.cc +++ b/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"); diff --git a/modules/third_party_perception/common/third_party_perception_gflags.h b/modules/third_party_perception/common/third_party_perception_gflags.h index 095c5151dbd6375db16d6c024542625a425b85ca..46931458af5db1bdea7a5e3177dfff632039cc02 100644 --- a/modules/third_party_perception/common/third_party_perception_gflags.h +++ b/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 diff --git a/modules/third_party_perception/conf/adapter.conf b/modules/third_party_perception/conf/adapter.conf index f875936e0f3ae8d8ff951315017214986bb99359..4de3c25b53f3e013ffd8db2013c7ce96ba0be5bc 100644 --- a/modules/third_party_perception/conf/adapter.conf +++ b/modules/third_party_perception/conf/adapter.conf @@ -1,9 +1,13 @@ config { - type: MOBILEYE + type: MOBILEYE mode: RECEIVE_ONLY } config { - type: DELPHIESR + type: DELPHIESR + mode: RECEIVE_ONLY +} +config: { + type: CONTI_RADAR mode: RECEIVE_ONLY } config { diff --git a/modules/third_party_perception/third_party_perception.cc b/modules/third_party_perception/third_party_perception.cc index 8f2c5d0c67800ea3d50294ec51ce83839fe26411..c1d55755e6076936a2f10ead7d72adcb9289fff9 100644 --- a/modules/third_party_perception/third_party_perception.cc +++ b/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 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 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 diff --git a/modules/third_party_perception/third_party_perception.h b/modules/third_party_perception/third_party_perception.h index 471aae21f384291f49357bd36b004fbb0ecea743..20bdac7a618c4b908dbe943a6072f8ff7c2f5bb5 100644 --- a/modules/third_party_perception/third_party_perception.h +++ b/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_;