文章目录
- 组件启动
-
- 实现组件类
-
- 实现组件头文件
- 实现组件源文件
- 设置配置文件
- 启动组件
- 激光感知
-
- 目录结构
- 源码剖析
-
- detection——init
-
- InitAlgorithmPlugin
- detection——Proc
-
- 点云预处理
- 获取高精度地图定位信息
- 障碍物检测
- 构建障碍物边框
- Bounding_box过滤
先感知模块perception_lidar
例如,首先解释组件component
如何启动,后对lidar detection
分析模块代码
apollo注释代码链接:https://github.com/Xiao-Hu-Z/apollo_xiaohu,以后会进一步梳理一些细节
组件启动
Perception
它是核心组件之一,但就像所有 C
每个应用程序都有相同的程序 Main
函数入口,然后引出本章探索 2 个问题:
Perception
入口在哪里Perception
如何启动
看Perception
在模块之前,有必要先了解它cyber
Apollo Cyber
运行时框架(Apollo Cyber RT Framework
) 它是基于组件概念构建的. 每个组件都是Cyber
框架构造块, 它包含一个特定的算法模块, 该算法模块处理一组输入数据,产生一组输出数据。
- 调度不确定性:各节点运行独立,节点运行顺序无法确定,业务逻辑调度顺序无法保证;
- 运行效率:
ROS
分布式系统有通信费用
Apollo
在3.5中引入了Cyber RT
,替换以前的基础ROS的变体, CyberRT
删除了master 机制,用自动发现机制代替,此外,Cyber RT
的核心设计将调度和任务从核心空间转移到用户空间。
创建和启动算法组件, 通过以下四个步骤:
- 初如化组件的文件结构
- 实现组件类
- 设置配置文件
- 启动组件
一个 component 需要创建以下文件:
- Header file: common_component_example.h
- Source file: common_component_example.cc
- Build file: BUILD
- DAG dependency file: common.dag
- Launch file: common.launch
Apollo 它是多数据集成的,它集成了 Camera、Lidar、Radar 目标,而这 3 个都是一个 component。
感知模块的入口在production目录,通过lanuch加载对应的dag,启动感知模块,包括多个子模块onboard定义在目录中。
perception组件路径:
apollo/modules/perception/onboard/component
在此目录下,许多感知相关组件被定义和实现。本文仅关注 Detection。
实现组件类
实现组件头文件
detection_component.h
:
- 继承 Component 类
- 定义自己的 Init 和 Proc 函数. Proc 需要指定输入数据类型。
- 使用CYBER_REGISTER_COMPONENT宏定义将组件类注册为全局可用。
namespace apollo {
namespace perception {
namespace onboard {
class DetectionComponent : public cyber::Component<drivers::PointCloud> {
public: DetectionComponent() = default; virtual ~DetectionComponent() = default; bool Init() override; bool Proc(const std::shared_ptr<drivers::PointCloud> &message) override; private: bool InitAlgorithmPlugin(); bool InternalProc( const std::shared_ptr<const drivers::PointCloud> &in_message, const std::shared_ptr<LidarFrameMessage> &out_message); private: static std::atomic<uint32_t> seq_num_; std::string sensor_name_; std::string detector_name_; bool enable_hdmap_ = true; float lidar_query_tf_offset_ = 20.0f; std::string lidar2novatel_tf2_child_frame_id_; std::string output_channel_name_; base::SensorInfo sensor_info_; // TransformWrapper类:用于查询不同坐标系之间的变换关系 TransformWrapper lidar2world_trans_; std::unique_ptr<lidar::BaseLidarObstacleDetection> detector_; std::shared_ptr<apollo::cyber::Writer<LidarFrameMessage>> writer_; }; // 使用CYBER_REGISTER_COMPONENT宏定义把组件类注册成全局可用 CYBER_REGISTER_COMPONENT(DetectionComponent); } // namespace onboard } // namespace perception } // namespace apollo
实现组件源文件
对于源文件Init
和 Proc
这两个函数需要实现,代码就不贴了,下面会对该部分代码解析
apollo/modules/perception/onboard/component/BUILD
BUILD 文件定义了 perception 中所有的 component 如 camera,radar,lidar 等的信息,本文只关注 Detection。
cc_library(
name = "detection_component",
srcs = ["detection_component.cc"],
hdrs = ["detection_component.h"],
deps = [
":lidar_inner_component_messages",
"//cyber/time:clock",
"//modules/common/util:string_util",
"//modules/perception/common/sensor_manager",
"//modules/perception/lib/registerer",
"//modules/perception/lidar/common",
"//modules/perception/lidar/app:lidar_obstacle_detection",
"//modules/perception/lidar/lib/ground_detector/spatio_temporal_ground_detector",
"//modules/perception/lidar/lib/interface",
"//modules/perception/lidar/lib/object_builder",
"//modules/perception/lidar/lib/object_filter_bank/roi_boundary_filter",
"//modules/perception/lidar/lib/roi_filter/hdmap_roi_filter",
"//modules/perception/lidar/lib/scene_manager/ground_service",
"//modules/perception/lidar/lib/scene_manager/roi_service",
"//modules/perception/lidar/lib/detector/point_pillars_detection:point_pillars_detection",
"//modules/perception/lidar/lib/detector/cnn_segmentation:cnn_segmentation",
"//modules/perception/lidar/lib/detector/ncut_segmentation:ncut_segmentation",
"//modules/perception/onboard/common_flags",
"//modules/perception/onboard/proto:lidar_component_config_cc_proto",
"//modules/perception/onboard/transform_wrapper",
"@eigen",
],
)
设置配置文件
一个 Component 的配置文件有 2 种:
- DAG
- Launch
DAG 定义了模块的依赖关系,Launch 文件定义了模块的启动。
下面看激光雷达感知的Launch 文件:
launch文件:Apollo/modules/perception/production/launch/perception_lidar.launch
<!--this file list the modules which will be loaded dynamicly and
their process name to be running in -->
<cyber>
<desc>cyber modules list config</desc>
<version>1.0.0</version>
<!-- sample module config, and the files should have relative path like
./bin/cyber_launch
./bin/mainboard
./conf/dag_streaming_0.conf -->
<module>
<name>perception_lidar</name>
<dag_conf>/apollo/modules/perception/production/dag/dag_streaming_perception_lidar.dag</dag_conf>
<!-- if not set, use default process -->
<process_name>perception_lidar</process_name>
<version>1.0.0</version>
</module>
</cyber>
对应的dag文件:Apollo/modules/perception/production/dag/dag_streaming_perception_lidar.dag
其中包括DetectionComponent, RecognitionComponent, FusionComponent, V2XFusionComponent
四个组件类,即检测,识别跟踪、融合、车联网融合。单对于lidar模块,主要就是检测和识别跟踪两个组件类的具体实现,融合和车联网融合是lidar模块输出结果的后续处理。
dag_streaming_perception_lidar.dag
内容:
module_config {
module_library : "/apollo/bazel-bin/modules/perception/onboard/component/libperception_component_lidar.so"
components {
class_name : "DetectionComponent"
config {
name: "Velodyne128Detection"
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"
}
}
}
components {
class_name : "RecognitionComponent"
config {
name: "RecognitionComponent"
config_file_path: "/apollo/modules/perception/production/conf/perception/lidar/recognition_conf.pb.txt"
readers {
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"
}
}
}
}
module_config {
module_library : "/apollo/bazel-bin/modules/v2x/fusion/apps/libv2x_fusion_component.so"
components {
class_name : "V2XFusionComponent"
config {
name : "v2x_fusion"
flag_file_path : "/apollo/modules/v2x/conf/v2x_fusion_tracker.conf"
readers: [
{
channel: "/perception/vehicle/obstacles"
}
]
}
}
}
该类配置参数proto定义:modules/perception/onboard/proto/lidar_component_config.proto
, 具体实现文件在:modules/perception/production/conf/perception/lidar/velodyne128_detection_conf.pb.txt
,每个激光雷达一个实现文件。
通过modules/perception/lidar/app/lidar_obstacle_detection.cc
中的LidarObstacleDetection类完成实际激光雷达点云障碍物检测工作;
Apollo7.0的lidar模块的处理过程就是:DetectionComponent
(检测) >> RecognitionComponent
(识别跟踪)。 component位于:
Apollo/modules/perception/onboard/component/detection_component.cc
Apollo/modules/perception/onboard/component/recognition_component.cc
启动组件
定义了一个 component 相关的文档后,就可以启动组件:
-
使用launch文件来启动
cyber_launch start apollo/modules/perception/production/launch/perception_lidar.launch
激光感知
先看perception模块的目录结构
. |-- BUILD // 基础类 |-- base // 基础类 |-- camera //
相机相关 |-- common // 公共目录 |-- data // 相机的内参和外参 |-- fusion // 传感器融合 |-- inference // 深度学习推理模块 |-- lib // 一些基础的库,包括线程、文件配置等 |-- lidar // 激光雷达相关 |-- map // 地图 |-- onboard // 各个子模块的入口 |-- production // 感知模块入口 --- 通过cyber启动子模块 |-- proto // 数据格式,protobuf |-- radar // 毫米波雷达 |-- testdata // 几个模块的测试数据 `-- tool // 离线测试工具
目录结构
文件夹结构:以下文件夹都是在perception/lidar/下
-
app——lidar应用类,主处理类,即最终应用程序应该是实例化该文件夹下的类来完成
-
common——定义lidar感知模块需要用的通用数据结构,例如LidarFrame,通用处理方法等;
-
lib——激光雷达感知中算法实现库
-
interface——各种算法类的基类定义,作为算法通用类的接口
-
lib/roi_filter——包含hdmap_roi_filter和roi_service_filter两个文件夹,前者用来利用高精度地图的信息来对LidarFrame中给出的高精度地图查询信息对点云进行ROI限制。
-
perception/map/hdmap——用在感知模块用来查询与高精度地图相关的信息。
launch文件:Apollo/modules/perception/production/launch/perception_lidar.launch
对应的dag文件:Apollo/modules/perception/production/dag/dag_streaming_perception_lidar.dag
launch文件用来启动,dag文件描述了整个系统的拓扑关系,也定义了每个Component需要订阅的话题
launch文件和dag文件上面有介绍,就不多赘述了,下面直接看modules/perception/onboard/component/detection_component.cc
代码
源码剖析
先看检测部分代码,对应文件:detection_component.cc
detection——init
bool DetectionComponent::Init()
{
// 读取配置文件,配置文件定义:modules/perception/onboard/proto/lidar_component_config.proto
LidarDetectionComponentConfig comp_config;
if (!GetProtoConfig(&comp_config))
{
return false;
}
ADEBUG << "Lidar Component Configs: " << comp_config.DebugString(); // DebugString:打印输出comp_config对象
output_channel_name_ = comp_config.output_channel_name();
sensor_name_ = comp_config.sensor_name();
detector_name_ = comp_config.detector_name();
lidar2novatel_tf2_child_frame_id_ = comp_config.lidar2novatel_tf2_child_frame_id();
lidar_query_tf_offset_ = static_cast<float>(comp_config.lidar_query_tf_offset());
enable_hdmap_ = comp_config.enable_hdmap();
// 发布消息
writer_ = node_->CreateWriter<LidarFrameMessage>(output_channel_name_);
/**配置文件参数 * Apollo/modules/perception/production/conf/perception/lidar/velodyne128_detection_conf.pb.txt: * sensor_name: "velodyne128" * enable_hdmap: true * lidar_query_tf_offset: 0 * lidar2novatel_tf2_child_frame_id: "velodyne128" * output_channel_name: "/perception/inner/DetectionObjects" */
// 初始化成员算法类
if (!InitAlgorithmPlugin())
{
AERROR << "Failed to init detection component algorithm plugin.";
return false;
}
return true;
}
LidarDetectionComponentConfig
路径:{
apollo/modules/perception/onboard/proto/lidar_component_config.proto
InitAlgorithmPlugin
在调用各模块类的处理逻辑process,先对个模块功能类进行参数初始化,先调用LidarObstacleDetection::Init
,后分别分别调用各模块功能类(预处理,根据高精度hdmap获取离定位点一定范围的道路、交叉路口信息,pointpillar目标检测)的init
方法
bool DetectionComponent::InitAlgorithmPlugin()
{
// 读取传感器元数据,元数据的读取是通过SensorManager来完成的,SensorManager 类经宏定义 DECLARE_SINGLETON(SensorManager) 修饰成为单例类,单例对象调用GetSensorInfo函数获取传感器名sensor_name_对应的传感器信息SensorInfo
// 其在初始化时会读取modules/perception/production/data/perception/common/sensor_meta.pt的包含所有传感器元数据的列表
ACHECK(common::SensorManager::Instance()->GetSensorInfo(sensor_name_,&sensor_info_));
// apollo/modules/perception/lib/registerer/registerer.h
// 父类指针detector指向子类LidarObstacleDetection的对象
lidar::BaseLidarObstacleDetection *detector = lidar::BaseLidarObstacleDetectionRegisterer::GetInstanceByName(detector_name_); // 调用宏定义类的静态方法
CHECK_NOTNULL(detector);
detector_.reset(detector);
// lidar型号,hdmap是否使用
lidar::LidarObstacleDetectionInitOptions init_options;
init_options.sensor_name = sensor_name_;
// 调用DEFINE_bool宏获取hdmap选择FLAGS_obs_enable_hdmap_input
// DEFINE_bool宏位于:modules/perception/onboard/common_flags/common_flags.cpp
init_options.enable_hdmap_input = FLAGS_obs_enable_hdmap_input && enable_hdmap_;
// 多态性:子类LidarObstacleDetection重写父类BaseLidarObstacleDetection的虚函数init
// 调用子类LidarObstacleDetection的init函数
ACHECK(detector_->Init(init_options)) << "lidar obstacle detection init error";
lidar2world_trans_.Init(lidar2novatel_tf2_child_frame_id_);
return true;
}
1.获取传感器信息sensor_info_,对应代码:
lidar::BaseLidarObstacleDetection *detector = lidar::BaseLidarObstacleDetectionRegisterer::GetInstanceByName(detector_name_); // 调用宏定义类的静态方法
CHECK_NOTNULL(detector);
detector_.reset(detector);
SensorManager
类路径:apollo/modules/perception/common/sensor_manager/sensor_manager.cc
common::SensorManager::Instance()会返回SensorManager
的唯一实例,同时调用构造函数,而构造函数又调用Init()方法,对传感器元数据初始化,会读取modules/perception/production/data/perception/common/sensor_meta.pt
的包含所有传感器元数据,并将传感器名字和传感器信息SensorInfo存储在sensor_info_map_字典
// glog 提供了CHECK()宏帮助我们检查程序的错误,当CHECK()的条件不满足时,它会记录FATAL日志并终止程序
SensorManager::SensorManager() {
CHECK_EQ(this->Init(), true); }
bool SensorManager::Init()
{
std::lock_guard<std::mutex> lock(mutex_);
if (inited_)
{
return true;
}
sensor_info_map_.clear();
distort_model_map_.clear();
undistort_model_map_.clear();
// 传感器元数据(名字,传感器型号,摆放位置)文件路径: apollo/modules/perception/production/data/perception/common/sensor_meta.pt
// 调用gflags库DEFINE_type宏获取传感器元数据文件路径FLAGS_obs_sensor_meta_path
const std::string file_path = cyber::common::GetAbsolutePath(lib::ConfigManager::Instance()->work_root(), FLAGS_obs_sensor_meta_path);
MultiSensorMeta sensor_list_proto;
// 从文件中读取信息存储到sensor_list_proto中
if (!GetProtoFromASCIIFile(file_path, &sensor_list_proto))
{
AERROR << "Invalid MultiSensorMeta file: " << FLAGS_obs_sensor_meta_path;
return false;
}
auto AddSensorInfo = [this](const SensorMeta &sensor_meta_proto)
{
SensorInfo sensor_info;
sensor_info.name = sensor_meta_proto.name();
sensor_info.type = static_cast<SensorType>(sensor_meta_proto.type());
sensor_info.orientation =
static_cast<SensorOrientation>(sensor_meta_proto.orientation());
sensor_info.frame_id = sensor_meta_proto.name();
// sensor_info_map_字典存储传感器名字和传感器信息SensorInfo
// SensorInfo 结构体类型 位于apollo/modules/perception/base/sensor_meta.h
auto pair = sensor_info_map_.insert(
make_pair(sensor_meta_proto.name(), sensor_info));
if (!pair.second)
{
AERROR << "Duplicate sensor name error.";
return false;
}
for (const SensorMeta &sensor_meta_proto : sensor_list_proto.sensor_meta())
{
if (!AddSensorInfo(sensor_meta_proto))
{
AERROR << "Failed to add sensor_info: " << sensor_meta_proto.name();
return false;
}
}
inited_ = true;
AINFO << "Init sensor_manager success.";
return true;
}
// 根据传感器名获取传感器信息SensorInfo
bool SensorManager::GetSensorInfo(const std::string &name,
SensorInfo *sensor_info) const
{
if (sensor_info == nullptr)
{
AERROR << "Nullptr error.";
return false;
}
const auto &itr = sensor_info_map_.find(name);
if (itr == sensor_info_map_.end())
{
return false;
}
*sensor_info = itr->second;
return true;
}
apollo/modules/perception/proto/sensor_meta_schema.proto
传感器信息proto字段
message SensorMeta {
enum SensorType {
UNKNOWN_SENSOR_TYPE = -1;
VELODYNE_64 = 0;
VELODYNE_32 = 1;
VELODYNE_16 = 2;
LDLIDAR_4 = 3;
LDLIDAR_1 = 4;
SHORT_RANGE_RADAR = 5;
LONG_RANGE_RADAR = 6;
MONOCULAR_CAMERA = 7;
STEREO_CAMERA = 8;
ULTRASONIC = 9;
VELODYNE_128 = 10;
}
enum SensorOrientation {
FRONT = 0;
LEFT_FORWARD = 1;
LEFT = 2;
LEFT_BACKWARD = 3;
REAR = 4;
RIGHT_BACKWARD = 5;
RIGHT = 6;
RIGHT_FORWARD = 7;
PANORAMIC = 8;
}
optional string name = 1;
optional SensorType type = 2;
optional SensorOrientation orientation = 3;
}
message MultiSensorMeta {
repeated SensorMeta sensor_meta = 1;
}
SensorInfo类型,位于位于apollo/modules/perception/base/sensor_meta.h
struct SensorInfo {
std::string name = "UNKNONW_SENSOR";
SensorType type = SensorType::UNKNOWN_SENSOR_TYPE;
SensorOrientation orientation = SensorOrientation::FRONT;
std::string frame_id = "UNKNOWN_FRAME_ID";
void Reset() {
name = "UNKNONW_SENSOR";
type = SensorType::UNKNOWN_SENSOR_TYPE;
orientation = SensorOrientation::FRONT;
frame_id = "UNKNOWN_FRAME_ID";
}
};
2.lidar障碍物检测基类对象指针指向子类的对象,对应代码:
lidar::BaseLidarObstacleDetection *detector = lidar::BaseLidarObstacleDetectionRegisterer::GetInstanceByName(detector_name_); // 调用宏定义类的静态方法
BaseLidarObstacleDetectionRegistere
作为雷达障碍物检测的基类,通过多态形式调用子类的init
函数,路径:
apollo/modules/perception/lidar/lib/interface/base_lidar_obstacle_detection.h
lidar::BaseLidarObstacleDetectionRegisterer
调用一个宏定义类,类BaseLidarObstacleDetectionRegisterer
路径:apollo/modules/perception/lib/registerer/registerer.h
#define PERCEPTION_REGISTER_REGISTERER(base_class) \
class base_class##Registerer { \
typedef ::apollo::perception::lib::Any Any; \
typedef ::apollo::perception::lib::FactoryMap FactoryMap; \
\
public: \
static base_class *GetInstanceByName(const ::std::string &name) { \
FactoryMap &map = \
::apollo::perception::lib::GlobalFactoryMap()[#base_class]; \
FactoryMap::iterator iter = map.find(name); \
if (iter == map.end()) { \
for (auto c : map) { \
AERROR << "Instance:" << c.first; \
} \
AERROR << "Get instance " << name << " failed."; \
return nullptr; \
} \
Any object = iter->second->NewInstance(); \
return *(object.AnyCast<base_class *>()); \
} \
static std::vector<base_class *> GetAllInstances() { \
std::vector<base_class *> instances; \
FactoryMap &map = \
::apollo::perception::lib::GlobalFactoryMap()[#base_class]; \
instances.reserve(map.size()); \
for (auto item : map) { \
Any object = item.second->NewInstance(); \
instances.push_back(*(object.AnyCast<base_class *>())); \
} \
return instances; \
} \
static const ::std::string GetUniqInstanceName() { \
FactoryMap &map = \
::apollo::perception::lib::GlobalFactoryMap()[#base_class]; \
CHECK_EQ(map.size(), 1U) << map.size(); \
return map.begin()->first; \
} \
static base_class *GetUniqInstance() { \
FactoryMap &map = \
::apollo::perception::lib::GlobalFactoryMap()[#base_class]; \
CHECK_EQ(map.size(), 1U) << map.size(); \
Any object = map.begin()->second->NewInstance(); \
return *(object.AnyCast<base_class *>()); \
} \
static bool IsValid(const ::std::string &name) { \
FactoryMap &map = \
::apollo::perception::lib::GlobalFactoryMap()[#base_class]; \
return map.find(name) != map.end(); \
} \
};
3.初始化雷达型号,是否使用hdmap,作为类LidarObstacleDetection的init函数传入参数
lidar::LidarObstacleDetectionInitOptions init_options;
init_options.sensor_name = sensor_name_;
// 调用DEFINE_bool宏返回FLAGS_obs_enable_hdmap_input,bool类型,表达是否有hdmap输入
// DEFINE_bool宏位于:modules/perception/onboard/common_flags/common_flags.cpp
init_options.enable_hdmap_input = FLAGS_obs_enable_hdmap_input && enable_hdmap_;
类LidarObstacleDetectionInitOptions位于
apollo/modules/perception/lidar/lib/interface/base_lidar_obstacle_detection.h
struct LidarObstacleDetectionInitOptions {
std::string sensor_name = "velodyne64";
bool enable_hdmap_input = true;
};
4.调用子类LidarObstacleDetection的init函数初始化参数
// 多态性:子类LidarObstacleDetection重写父类BaseLidarObstacleDetection的虚函数init
// 调用子类LidarObstacleDetection的init函数
ACHECK(detector_->Init(init_options)) << "lidar obstacle detection init error";
Init函数位于:apollo/modules/perception/lidar/app/lidar_obstacle_detection.cc
下面看雷达障碍物检测类的初始化函数init
,初始化各模块的参数
bool LidarObstacleDetection::Init(const LidarObstacleDetectionInitOptions &options) { auto &sensor_name = options.sensor_name; // 传感器名,如"velodyne128" // ConfigManager类经宏定义 DECLARE_SINGLETON(ConfigManager) 修饰成为单例类 auto config_manager = lib::ConfigManager::Instance(); const lib::ModelConfig *model_config = nullptr; // 获取 LidarObstacleDetection 配置参数model_config,第一次调用GetModelConfig将各模块功能类和其配置参数存储在字典,从字典查找LidarObstacleDetection对应的参数信息 // LidarObstacleDetection在文件中是存储在 apollo/modules/perception/production/conf/perception/lidar/modules/lidar_obstacle_pipeline.config ACHECK(config_manager->GetModelConfig(Name(), &model_config)); const std::string work_root = config_manager->work_root(); std::string config_file; std::string root_path; // root_path:./data/perception/lidar/models/lidar_obstacle_pipeline ACHECK(model_config->get_value("root_path", &root_path));