Perception: resume camera obstacle detection in sensor fusion (#13277)

* Perception: add camera obstacle detection to fusion

* Perception: modify default dag to use CNNSeg model

modules/perception/fusion/app/obstacle_multi_sensor_fusion.cc

@@ -28,7 +28,7 @@ bool ObstacleMultiSensorFusion::Init(
   fusion_ = BaseFusionSystemRegisterer::GetInstanceByName(param.fusion_method);
 
   FusionInitOptions init_options;
-  init_options.main_sensor = param.main_sensor;
+  init_options.main_sensors = param.main_sensors;
   if (fusion_ == nullptr || !fusion_->Init(init_options)) {
     AINFO << "Failed to Get Instance or Initialize " << param.fusion_method;
     return false;

modules/perception/fusion/app/obstacle_multi_sensor_fusion.h

@@ -25,7 +25,7 @@ namespace perception {
 namespace fusion {
 
 struct ObstacleMultiSensorFusionParam {
-  std::string main_sensor;
+  std::vector<std::string> main_sensors;
   std::string fusion_method;
 };
 

modules/perception/fusion/lib/data_association/hm_data_association/hm_tracks_objects_match.cc

@@ -65,6 +65,7 @@ bool HMTrackersObjectsAssociation::Associate(
   double measurement_timestamp = sensor_objects[0]->GetTimestamp();
   track_object_distance_.ResetProjectionCache(measurement_sensor_id,
                                               measurement_timestamp);
+  // TODO(chenjiahao): specify prohibited sensors in config
   bool do_nothing = (sensor_objects[0]->GetSensorId() == "radar_front");
   IdAssign(fusion_tracks, sensor_objects, &association_result->assignments,
            &association_result->unassigned_tracks,

modules/perception/fusion/lib/dummy/dummy_algorithms.cc

@@ -21,7 +21,7 @@ namespace fusion {
 
 // class DummyFusionSystem implementation
 bool DummyFusionSystem::Init(const FusionInitOptions& options) {
-  main_sensor_ = options.main_sensor;
+  main_sensors_ = options.main_sensors;
   return true;
 }
 
@@ -33,7 +33,8 @@ bool DummyFusionSystem::Fuse(const FusionOptions& options,
   }
 
   fused_objects->clear();
-  if (sensor_frame->sensor_info.name != main_sensor_) {
+  if (std::find(main_sensors_.begin(), main_sensors_.end(),
+                sensor_frame->sensor_info.name) == main_sensors_.end()) {
     return true;
   }
 

modules/perception/fusion/lib/dummy/dummy_algorithms_test.cc

@@ -29,7 +29,7 @@ namespace fusion {
 
 TEST(DummyFusionSystemTest, test) {
   FusionInitOptions init_options;
-  init_options.main_sensor = "velodyne64";
+  init_options.main_sensors = std::vector<std::string>{"velodyne64"};
   DummyFusionSystem system;
   EXPECT_TRUE(system.Init(init_options));
 

modules/perception/fusion/lib/fusion_system/probabilistic_fusion/probabilistic_fusion.cc

@@ -43,7 +43,7 @@ ProbabilisticFusion::ProbabilisticFusion() {}
 ProbabilisticFusion::~ProbabilisticFusion() {}
 
 bool ProbabilisticFusion::Init(const FusionInitOptions& init_options) {
-  main_sensor_ = init_options.main_sensor;
+  main_sensors_ = init_options.main_sensors;
 
   BaseInitOptions options;
   if (!GetFusionInitOptions("ProbabilisticFusion", &options)) {
@@ -166,16 +166,13 @@ std::string ProbabilisticFusion::Name() const { return "ProbabilisticFusion"; }
 bool ProbabilisticFusion::IsPublishSensor(
     const base::FrameConstPtr& sensor_frame) const {
   std::string sensor_id = sensor_frame->sensor_info.name;
-  return sensor_id == main_sensor_;
-  // const std::vector<std::string>& pub_sensors =
-  //   params_.publish_sensor_ids;
-  // const auto& itr = std::find(
-  //   pub_sensors.begin(), pub_sensors.end(), sensor_id);
-  // if (itr != pub_sensors.end()) {
-  //   return true;
-  // } else {
-  //   return false;
-  // }
+  const auto& itr = std::find(
+      main_sensors_.begin(), main_sensors_.end(), sensor_id);
+  if (itr != main_sensors_.end()) {
+    return true;
+  } else {
+    return false;
+  }
 }
 
 void ProbabilisticFusion::FuseFrame(const SensorFramePtr& frame) {
@@ -352,7 +349,7 @@ void ProbabilisticFusion::RemoveLostTrack() {
     }
   }
   AINFO << "Remove " << foreground_tracks.size() - foreground_track_count
-        << " foreground tracks";
+        << " foreground tracks. " << foreground_track_count << " tracks left.";
   foreground_tracks.resize(foreground_track_count);
   trackers_.resize(foreground_track_count);
 

modules/perception/fusion/lib/fusion_system/probabilistic_fusion/probabilistic_fusion_test.cc

@@ -37,7 +37,7 @@ TEST(ProbabliticFusionTest, test_init) {
   sensor_manager->Reset();
   sensor_manager->Init();
   FusionInitOptions init_options;
-  init_options.main_sensor = "velodyne64";
+  init_options.main_sensors = std::vector<std::string>{"velodyne64"};
   ProbabilisticFusion pf;
   EXPECT_TRUE(pf.Init(init_options));
   EXPECT_EQ(pf.Name(), "ProbabilisticFusion");
@@ -106,7 +106,7 @@ TEST(ProbabliticFusionTest, test_update) {
   sensor_manager->Reset();
   sensor_manager->Init();
   FusionInitOptions init_options;
-  init_options.main_sensor = "velodyne64";
+  init_options.main_sensors = std::vector<std::string>{"velodyne64"};
   ProbabilisticFusion pf;
   EXPECT_TRUE(pf.Init(init_options));
   EXPECT_EQ(pf.Name(), "ProbabilisticFusion");
@@ -257,7 +257,7 @@ TEST(ProbabilisticFusionTest, test_collect_sensor_measurement) {
   sensor_manager->Reset();
   sensor_manager->Init();
   FusionInitOptions init_options;
-  init_options.main_sensor = "velodyne64";
+  init_options.main_sensors = std::vector<std::string>{"velodyne64"};
   ProbabilisticFusion pf;
   EXPECT_TRUE(pf.Init(init_options));
   EXPECT_EQ(pf.Name(), "ProbabilisticFusion");

modules/perception/fusion/lib/gatekeeper/pbf_gatekeeper/pbf_gatekeeper.cc

@@ -111,6 +111,7 @@ bool PbfGatekeeper::RadarAbleToPublish(const TrackPtr &track, bool is_night) {
   if (params_.publish_if_has_radar && visible_in_radar &&
       radar_object != nullptr) {
     if (radar_object->GetSensorId() == "radar_front") {
+      // TODO(henjiahao): enable radar front
       return false;
       // if (radar_object->GetBaseObject()->radar_supplement.range >
       //         params_.min_radar_confident_distance &&

modules/perception/fusion/lib/interface/base_fusion_system.h

@@ -29,7 +29,7 @@ namespace perception {
 namespace fusion {
 
 struct FusionInitOptions {
-  std::string main_sensor;
+  std::vector<std::string> main_sensors;
 };
 
 struct FusionOptions {};
@@ -54,7 +54,7 @@ class BaseFusionSystem {
   virtual std::string Name() const = 0;
 
  protected:
-  std::string main_sensor_;
+  std::vector<std::string> main_sensors_;
 };
 
 PERCEPTION_REGISTER_REGISTERER(BaseFusionSystem);

modules/perception/onboard/component/fusion_component.cc

@@ -37,7 +37,9 @@ bool FusionComponent::Init() {
 
   // to load component configs
   fusion_method_ = comp_config.fusion_method();
-  fusion_main_sensor_ = comp_config.fusion_main_sensor();
+  for (int i = 0; i < comp_config.fusion_main_sensors_size(); ++i) {
+    fusion_main_sensors_.push_back(comp_config.fusion_main_sensors(i));
+  }
   object_in_roi_check_ = comp_config.object_in_roi_check();
   radius_for_roi_object_check_ = comp_config.radius_for_roi_object_check();
 
@@ -58,21 +60,23 @@ bool FusionComponent::Proc(const std::shared_ptr<SensorFrameMessage>& message) {
                                                        PerceptionObstacles);
   std::shared_ptr<SensorFrameMessage> viz_message(new (std::nothrow)
                                                       SensorFrameMessage);
+
+  // TODO(convert sensor id)
+  const auto& itr = std::find(fusion_main_sensors_.begin(),
+                              fusion_main_sensors_.end(), message->sensor_id_);
+  if (itr == fusion_main_sensors_.end()) {
+    AINFO << "Fusion receives message from " << message->sensor_id_
+          << " which is not in main sensors. Skip sending.";
+    return true;
+  }
+
   bool status = InternalProc(message, out_message, viz_message);
   if (status) {
-    // TODO(conver sensor id)
-    if (message->sensor_id_ != fusion_main_sensor_) {
-      AINFO << "Fusion receive from " << message->sensor_id_ << "not from "
-            << fusion_main_sensor_ << ". Skip send.";
-    } else {
-      // Send("/apollo/perception/obstacles", out_message);
-      writer_->Write(out_message);
-      AINFO << "Send fusion processing output message.";
-      // send msg for visualization
-      if (FLAGS_obs_enable_visualization) {
-        // Send("/apollo/perception/inner/PrefusedObjects", viz_message);
-        inner_writer_->Write(viz_message);
-      }
+    writer_->Write(out_message);
+    AINFO << "Send fusion processing output message.";
+    // send msg for visualization
+    if (FLAGS_obs_enable_visualization) {
+      inner_writer_->Write(viz_message);
     }
   }
   return status;
@@ -81,7 +85,7 @@ bool FusionComponent::Proc(const std::shared_ptr<SensorFrameMessage>& message) {
 bool FusionComponent::InitAlgorithmPlugin() {
   fusion_.reset(new fusion::ObstacleMultiSensorFusion());
   fusion::ObstacleMultiSensorFusionParam param;
-  param.main_sensor = fusion_main_sensor_;
+  param.main_sensors = fusion_main_sensors_;
   param.fusion_method = fusion_method_;
   ACHECK(fusion_->Init(param)) << "Failed to init ObstacleMultiSensorFusion";
 
@@ -130,10 +134,6 @@ bool FusionComponent::InternalProc(
   }
   PERF_BLOCK_END_WITH_INDICATOR("fusion_process", in_message->sensor_id_);
 
-  if (in_message->sensor_id_ != fusion_main_sensor_) {
-    return true;
-  }
-
   Eigen::Matrix4d sensor2world_pose =
       in_message->frame_->sensor2world_pose.matrix();
   if (object_in_roi_check_ && FLAGS_obs_enable_hdmap_input) {

modules/perception/onboard/component/fusion_component.h

@@ -49,7 +49,7 @@ class FusionComponent : public cyber::Component<SensorFrameMessage> {
   static uint32_t s_seq_num_;
 
   std::string fusion_method_;
-  std::string fusion_main_sensor_;
+  std::vector<std::string> fusion_main_sensors_;
   bool object_in_roi_check_ = false;
   double radius_for_roi_object_check_ = 0;
 

modules/perception/onboard/proto/fusion_component_config.proto

@@ -4,12 +4,12 @@ package apollo.perception.onboard;
 
 message FusionComponentConfig {
   optional string fusion_method = 1;
-  optional string fusion_main_sensor = 2;
+  repeated string fusion_main_sensors = 2;
   optional bool object_in_roi_check = 3;
   optional double radius_for_roi_object_check = 4;
 
   optional string output_obstacles_channel_name = 5
-      [default = "/perception/obstacles"];
+      [default = "/perception/vehicle/obstacles"];
   optional string output_viz_fused_content_channel_name = 6
       [default = "/perception/inner/visualization/FusedObjects"];
 }

modules/perception/production/conf/perception/camera/fusion_camera_detection_component.pb.txt

@@ -7,7 +7,7 @@ frame_capacity : 20
 image_channel_num : 3
 enable_undistortion : false
 enable_visualization : false
-output_final_obstacles : true
+output_final_obstacles : false
 output_obstacles_channel_name : "/perception/obstacles"
 camera_perception_viz_message_channel_name : "/perception/inner/camera_viz_msg"
 prefused_channel_name : "/perception/inner/PrefusedObjects"

modules/perception/production/conf/perception/fusion/fusion_component_conf.pb.txt

 fusion_method: "ProbabilisticFusion"
-fusion_main_sensor: "velodyne128"
+fusion_main_sensors: "velodyne128"
+fusion_main_sensors: "front_6mm"
+fusion_main_sensors: "front_12mm"
 object_in_roi_check: true
 radius_for_roi_object_check: 120
 output_obstacles_channel_name: "/perception/vehicle/obstacles"

modules/perception/production/dag/dag_motion_service.dag

@@ -3,9 +3,9 @@ module_config {
   components {
     class_name : "MotionService"
     config {
-    name : "camera_motionService"
-    config_file_path : "/apollo/modules/perception/production/conf/perception/camera/motion_service.config"
-    flag_file_path: "/apollo/modules/perception/production/conf/perception/perception_common.flag"
+      name : "camera_motionService"
+      config_file_path : "/apollo/modules/perception/production/conf/perception/camera/motion_service.config"
+      flag_file_path: "/apollo/modules/perception/production/conf/perception/perception_common.flag"
     }
   }
 }

modules/perception/production/dag/dag_streaming_perception.dag

+module_config {
+  module_library : "/apollo/bazel-bin/modules/perception/onboard/component/libperception_component_camera.so"
+  components {
+    class_name : "FusionCameraDetectionComponent"
+    config {
+      name: "FusionCameraComponent"
+      config_file_path: "/apollo/modules/perception/production/conf/perception/camera/fusion_camera_detection_component.pb.txt"
+      flag_file_path: "/apollo/modules/perception/production/conf/perception/perception_common.flag"
+    }
+  }
+}
+
 module_config {
   module_library : "/apollo/bazel-bin/modules/perception/onboard/component/libperception_component_lidar.so"
 
   components {
-    class_name : "DetectionComponent"
+    class_name : "SegmentationComponent"
     config {
-      name: "Velodyne128Detection"
-      config_file_path: "/apollo/modules/perception/production/conf/perception/lidar/velodyne128_detection_conf.pb.txt"
+      name: "Velodyne128Segmentation"
+      config_file_path: "/apollo/modules/perception/production/conf/perception/lidar/velodyne128_segmentation_conf.pb.txt"
       flag_file_path: "/apollo/modules/perception/production/conf/perception/perception_common.flag"
       readers {
-          channel: "/apollo/sensor/lidar128/compensator/PointCloud2"
-        }
+        channel: "/apollo/sensor/lidar128/compensator/PointCloud2"
+      }
     }
   }
 
@@ -19,8 +31,8 @@ module_config {
       name: "RecognitionComponent"
       config_file_path: "/apollo/modules/perception/production/conf/perception/lidar/recognition_conf.pb.txt"
       readers {
-          channel: "/perception/inner/DetectionObjects"
-        }
+        channel: "/perception/inner/SegmentationObjects"
+      }
     }
   }
 
@@ -29,9 +41,10 @@ module_config {
     config {
       name: "FrontRadarDetection"
       config_file_path: "/apollo/modules/perception/production/conf/perception/radar/front_radar_component_conf.pb.txt"
+      flag_file_path: "/apollo/modules/perception/production/conf/perception/perception_common.flag"
       readers {
-          channel: "/apollo/sensor/radar/front"
-        }
+        channel: "/apollo/sensor/radar/front"
+      }
     }
   }
 
@@ -40,9 +53,10 @@ module_config {
     config {
       name: "RearRadarDetection"
       config_file_path: "/apollo/modules/perception/production/conf/perception/radar/rear_radar_component_conf.pb.txt"
+      flag_file_path: "/apollo/modules/perception/production/conf/perception/perception_common.flag"
       readers {
-          channel: "/apollo/sensor/radar/rear"
-        }
+        channel: "/apollo/sensor/radar/rear"
+      }
     }
   }
 
@@ -52,8 +66,8 @@ module_config {
       name: "SensorFusion"
       config_file_path: "/apollo/modules/perception/production/conf/perception/fusion/fusion_component_conf.pb.txt"
       readers {
-          channel: "/perception/inner/PrefusedObjects"
-        }
+        channel: "/perception/inner/PrefusedObjects"
+      }
     }
   }
 }

modules/perception/production/dag/dag_streaming_perception_lane.dag

@@ -8,9 +8,8 @@ module_config {
       config_file_path: "/apollo/modules/perception/production/conf/perception/camera/lane_detection_component.config"
       flag_file_path: "/apollo/modules/perception/production/conf/perception/perception_common.flag"
       readers {
-          channel: "/apollo/perception/lane_status"
-        }
+        channel: "/apollo/perception/lane_status"
+      }
     }
   }
-
 }

modules/perception/production/dag/dag_streaming_perception_lgsvl.dag

@@ -8,8 +8,8 @@ module_config {
       config_file_path: "/apollo/modules/perception/production/conf/perception/lidar/velodyne128_detection_conf_lgsvl.pb.txt"
       flag_file_path: "/apollo/modules/perception/production/conf/perception/perception_common.flag"
       readers {
-          channel: "/apollo/sensor/lidar128/compensator/PointCloud2"
-        }
+        channel: "/apollo/sensor/lidar128/compensator/PointCloud2"
+      }
     }
   }
 
@@ -19,8 +19,8 @@ module_config {
       name: "RecognitionComponent"
       config_file_path: "/apollo/modules/perception/production/conf/perception/lidar/recognition_conf.pb.txt"
       readers {
-          channel: "/perception/inner/DetectionObjects"
-        }
+        channel: "/perception/inner/DetectionObjects"
+      }
     }
   }
 
@@ -30,8 +30,8 @@ module_config {
       name: "FrontRadarDetection"
       config_file_path: "/apollo/modules/perception/production/conf/perception/radar/front_radar_component_conf.pb.txt"
       readers {
-          channel: "/apollo/sensor/radar/front"
-        }
+        channel: "/apollo/sensor/radar/front"
+      }
     }
   }
 
@@ -41,8 +41,8 @@ module_config {
       name: "RearRadarDetection"
       config_file_path: "/apollo/modules/perception/production/conf/perception/radar/rear_radar_component_conf.pb.txt"
       readers {
-          channel: "/apollo/sensor/radar/rear"
-        }
+        channel: "/apollo/sensor/radar/rear"
+      }
     }
   }
 
@@ -52,8 +52,8 @@ module_config {
       name: "SensorFusion"
       config_file_path: "/apollo/modules/perception/production/conf/perception/fusion/fusion_component_conf.pb.txt"
       readers {
-          channel: "/perception/inner/PrefusedObjects"
-        }
+        channel: "/perception/inner/PrefusedObjects"
+      }
     }
   }
 }

modules/perception/production/dag/dag_streaming_perception_lidar.dag

@@ -8,8 +8,8 @@ module_config {
       config_file_path: "/apollo/modules/perception/production/conf/perception/lidar/velodyne128_detection_conf.pb.txt"
       flag_file_path: "/apollo/modules/perception/production/conf/perception/perception_common.flag"
       readers {
-          channel: "/apollo/sensor/lidar128/compensator/PointCloud2"
-        }
+        channel: "/apollo/sensor/lidar128/compensator/PointCloud2"
+      }
     }
   }
 
@@ -19,23 +19,21 @@ module_config {
       name: "RecognitionComponent"
       config_file_path: "/apollo/modules/perception/production/conf/perception/lidar/recognition_conf.pb.txt"
       readers {
-          channel: "/perception/inner/DetectionObjects"
-        }
+        channel: "/perception/inner/DetectionObjects"
+      }
     }
   }
 
-
   components {
     class_name: "FusionComponent"
     config {
       name: "SensorFusion"
       config_file_path: "/apollo/modules/perception/production/conf/perception/fusion/fusion_component_conf.pb.txt"
       readers {
-          channel: "/perception/inner/PrefusedObjects"
-        }
+        channel: "/perception/inner/PrefusedObjects"
+      }
     }
   }
-
 }
 
 module_config {

modules/perception/production/dag/dag_streaming_perception_lidar_segmentation.dag

@@ -8,8 +8,8 @@ module_config {
       config_file_path: "/apollo/modules/perception/production/conf/perception/lidar/velodyne128_segmentation_conf.pb.txt"
       flag_file_path: "/apollo/modules/perception/production/conf/perception/perception_common.flag"
       readers {
-          channel: "/apollo/sensor/lidar128/compensator/PointCloud2"
-        }
+        channel: "/apollo/sensor/lidar128/compensator/PointCloud2"
+      }
     }
   }
 
@@ -19,30 +19,8 @@ module_config {
       name: "RecognitionComponent"
       config_file_path: "/apollo/modules/perception/production/conf/perception/lidar/recognition_conf.pb.txt"
       readers {
-          channel: "/perception/inner/SegmentationObjects"
-        }
-    }
-  }
-
-  components {
-    class_name: "RadarDetectionComponent"
-    config {
-      name: "FrontRadarDetection"
-      config_file_path: "/apollo/modules/perception/production/conf/perception/radar/front_radar_component_conf.pb.txt"
-      readers {
-          channel: "/apollo/sensor/radar/front"
-        }
-    }
-  }
-
-  components {
-    class_name: "RadarDetectionComponent"
-    config {
-      name: "RearRadarDetection"
-      config_file_path: "/apollo/modules/perception/production/conf/perception/radar/rear_radar_component_conf.pb.txt"
-      readers {
-          channel: "/apollo/sensor/radar/rear"
-        }
+        channel: "/perception/inner/SegmentationObjects"
+      }
     }
   }
 
@@ -52,8 +30,8 @@ module_config {
       name: "SensorFusion"
       config_file_path: "/apollo/modules/perception/production/conf/perception/fusion/fusion_component_conf.pb.txt"
       readers {
-          channel: "/perception/inner/PrefusedObjects"
-        }
+        channel: "/perception/inner/PrefusedObjects"
+      }
     }
   }
 }

modules/perception/production/launch/perception.launch

@@ -15,13 +15,6 @@
         <process_name>lidar_perception</process_name>
         <version>1.0.0</version>
     </module>
-    <module>
-        <name>perception_camera</name>
-        <dag_conf>/apollo/modules/perception/production/dag/dag_streaming_perception_camera.dag</dag_conf>
-        <!-- if not set, use default process -->
-        <process_name>camera_perception</process_name>
-        <version>1.0.0</version>
-    </module>
     <module>
         <name>motion_service</name>
         <dag_conf>/apollo/modules/perception/production/dag/dag_motion_service.dag</dag_conf>

modules/perception/production/launch/perception_all.launch

@@ -14,14 +14,6 @@
         <process_name>perception</process_name>
         <version>1.0.0</version>
     </module>
-    <module>
-        <name>perception_camera</name>
-        <dag_conf>/apollo/modules/perception/production/dag/dag_streaming_perception_camera.dag</dag_conf>
-        <!-- if not set, use default process -->
-        <process_name>perception</process_name>
-        <version>1.0.0</version>
-    </module>
-
     <module>
         <name>perception_traffic_light</name>
         <dag_conf>/apollo/modules/perception/production/dag/dag_streaming_perception_trafficlights.dag</dag_conf>