Apollo自动驾驶平台：订阅点云数据实现避障

Posted on | In | | Visitors: | times

Cyber RT

Apollo平台的代码在3.5版本及以后完全摒弃了ROS(Robot Operating System)，而是将整个平台基于Baidu自行研发的Cyber RT系统上。Cyber RT相较于ROS系统有以下优点：

  • 实时性高，时延小：自动驾驶对于时延的要求十分高，而ROS系统主要针对机器人，其对时延的要求较低，这也是baidu研发Cyber RT的初衷。
  • 调度确定性：ROS的任务调度具有不确定性，任务的执行顺序取决于系统，而Cyber RT将任务调度从内核空间移至用户空间，调度可以与算法逻辑密切结合。

Cyber RT vs ROS

Cyber RT ROS 含义
Component 无 组件之间通过 Cyber channel 通信
Channel Topic channel 用于管理数据通信，用户可以通过 publish/subscribe 相同的 channel 来通信
Node Node 每一个模块包含 Node 并通过 Node 来通信。一个模块通过定义 read/write 和/或 service/client 使用不同的通信模式
Reader/Writer Publish/Subscribe 订阅者模式。往 channel 读写消息的类。 通常作为 Node 主要的消息传输接口
Service/Client Service/Client 请求/响应模式，支持节点间双向通信
Message Message Cyber RT 中用于模块间通信的数据单元。其实现基于 protobuf
Parameter Parameter Parameter 服务提供全局参数访问接口。该服务基于 service/client 模式
Record file Bag file 用于记录从 channel 发送或接收的消息。 回放 record file 可以重现之前的操作行为
Launch file Launch file 提供一种启动模块的便利途径。通过在 launch file 中定义一个或多个 dag 文件，可以同时启动多个 modules
Task 无 异步计算任务
CRoutine 无 协程，优化线程使用与系统资源分配
Scheduler 无 任务调度器，用户空间
Dag file 无 定义模块拓扑结构的配置文件

Apollo 代码结构

整体框架

Apollo 5.0 Github 开源代码

  • /cyber:Baidu研发的Cyber RT系统
  • /data
    • /bag:可以用于放置录制的数据包，用于仿真模拟。可以通过cyber_recorder play -f /data/bag/test_filename来播放数据包
    • /log:保存着运行的日志文件。
  • /docs:大量的代码文档、操作文档、说明书等。
  • /docker:有关docker操作的一些脚本文件
  • /modules:核心模块，包含bridge、calibaration、canbus、common、contrib、control、data、dreamview、drivers、guardian、localization、map、monitor、perception、planning、prediction、routing、v2x等模块
  • /scripts:包含调试、运行的一些脚本。

模块的结构

一个模块通常有/conf、/dag、/proto、/launch四个子目录。

/launch

以下为modules/control/launch/control.launch的内容，该文件主要指示了一个dag文件。

1
2
3
4
5
6
7
<cyber>
    <module>
        <name>control</name>
        <dag_conf>/apollo/modules/control/dag/control.dag</dag_conf>
        <process_name>control</process_name>
    </module>
</cyber>

若是想要单独启动某一模块，如control模块，可以通过以下脚本启动。当然也可以在浏览器里Dreamview界面里打开control模块的开关来启动control模块，但在命令行以脚本启动control模块可以看到输出的日志信息，便于错误排查

1
cyber_luanch module/control/launch/control.launch

/dag

dag文件里主要指示了模块的配置文件(flag_file_path属性)，还是以control模块为例，以下为modules/control/dag/control.dag的内容

1
2
3
4
5
6
7
8
9
10
11
module_config {
    module_library : "/apollo/bazel-bin/modules/control/libcontrol_component.so"
    timer_components {
        class_name : "ControlComponent"
        config {
            name: "control"
            flag_file_path: "/apollo/modules/control/conf/control.conf"
            interval: 10
      }
   }
}

/proto

proto是协议protobuf的简写，里面定义了发布的数据格式，protobuf是google团队开发的用于高效存储和读取结构化数据的工具。

/conf

里面主要包含模块的配置文件，包含了模块的一些参数，类似于在命令行执行脚本给的参数。由control.dag文件指向的主配置文件control.conf部分内容如下：

1
2
3
--flagfile=/apollo/modules/common/data/global_flagfile.txt
--control_conf_file=/apollo/modules/control/conf/scout_conf.pb.txt
--enable_speed_station_preview=false

这里的参数名可以在模块对应的common下的flags文件里定义，也可以在modules下的common模块里定义。比如control/common/目录下有control_gflags.h和 control_gflags.cc两个文件，分别用来声明参数名，和定义参数并给定参数默认值。

比如若要自定一个模块配置参数x_min，首先需要在control_gflags.h里声明：

1
DECLARE_double(x_min);

然后在control_gflags.cc里定义，中间参数为默认值，第三个参数为对参数的描述，类似于帮助文档：

1
DEFINE_double(x_min,0.0001,"x min");

然后若要使用参数x_min，则可以在其他文件里include control_flags.h 这个头文件，然后通过FLAGS_x_min来使用其值，如果需要更改x_min的值，则可以在control.conf文件里修改，如将其改为-5.0，可以增加以下一行。由于这些都是命令行参数，因此修改这些值，是不需要重新编译代码的，方便调参。

1
--x_min=-5.0

代码编写

主要更改代码的控制模块modules/control/，在控制模块通过订阅激光雷达的点云数据，获取小车正前方的的激光点云，判断若前方有障碍物则将让小车停下来。

修改control/Build文件

由于要使用激光点云数据，需要额外增加依赖项，在/modules/control/Build文件里找到cc_library，在其deps数组里增加

1
"//modules/drivers/proto:sensor_proto",

sensor_proto这个库的名字可以在modules/drivers/proto/Build文件下的cc_proto_library里找到。

初始化PointCloud Reader

Cyber RT采用Publish/Subscribe的通信机制，不同的模块在各自的Channel上发布Publish数据，其他模块可以通过订阅Subscribe来获取Channel上的数据。
而在订阅前，当然需要先指定一个Reader，由Reader去订阅数据并读取数据。control模块已经定义了chassis_reader_用于读取车底盘的数据、localization_reader_用于读取定位数据，可以获取车辆的ENU坐标、trajectory_reader_用于获取轨迹数据等reader，接下来就可以仿照这些reader定义自己的point_cloud_reader_了。
首先在modules/control/control_component.h文件里声明reader,需注意命名空间的问题。

1
std::shared_ptr<Reader<apollo::drivers::PointCloud>>point_cloud_reader_;

然后在modules/control/control_component.cc文件里的Init函数里对reader进行初始化：

1
2
3
4
5
6
7
8
// Point cload initialization
cyber::ReaderConfig pointcloud_reader_config;
// 设置reader的channel name，reader通过这个topic来寻找点云数据所在的channel
pointcloud_reader_config.channel_name = FLAGS_lidar_16_front_center_topic;
pointcloud_reader_config.pending_queue_size = FLAGS_pointcloud_pending_queue_size;
// 实例化Reader
point_cloud_reader_ = node_->CreateReader<apollo::drivers::PointCloud>(pointcloud_reader_config, nullptr);
CHECK(point_cloud_reader_ != nullptr);

  • FLAGS_开头的为gflags库定义的参数，gflags是使用Google的用于处理命令行参数的库，Apollo的代码里广泛运用了该库，大部分模块下都有一个/common文件夹下面有对应的gflags文件，里面定义了该模块使用的参数，使用gflags来配置模块参数的好处是更改参数后，无需编译，只需要重新启动对应模块，即可应用该参数。
  • FLAGS_lidar_16_front_center_topic：16线激光雷达正前方的点云数据所使用的topic名。
  • FLAGS_pointcloud_pending_queue_size为自定义的参数，默认值为10，设置方式参考上面设置x_min的方法。

如何找到对应的topic名呢？

  • velodyne.launch

    首先，drivers下有一个激光的子模块velodyne，在modules/drivers/velodyne，在其launch/目录下有不同激光雷达的启动文件，由于小车使用的是16线激光雷达，所以需要16线激光雷达的启动文件，velodyne.launch为16线激光雷达的启动文件，该文件指向了一个dag文件velodyne.dag。

  • velodyne.dag

    为了简化操作，只使用激光的正前方的数据，找到#16_front_center，其config里有一个config_file_path属性，指向了对应的配置文件velodyne16_front_center_conf.pb.txt.

  • velodyne16_front_center_conf.pb.txt

    改文件给出了对应的topic名

    1
    convert_channel_name: "/apollo/sensor/lidar16/front/center/PointCloud2"

    而该topic已在/modules/common/adapters/adapters_gflags.cc里定义了参数名：

    1
    2
    3
    DEFINE_string(lidar_16_front_center_topic,
                "/apollo/sensor/lidar16/front/center/PointCloud2",
                "front center 16 beam lidar topic name");

    由于代码的版本不同，可能没有该topic的FLAGS，不过可以直接用其值代替。

获取点云数据

点云数据格式

目前控制模块已经订阅了激光模块的点云数据，激光会通过订阅的channel发消息给控制模块，控制模块收到后需要解析该消息。

消息的格式定义在对应的proto文件里，点云消息的格式在/modules/drivers/proto/pointcloud.proto里，其内容如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
syntax = "proto2";
package apollo.drivers;

import "modules/common/proto/header.proto";

message PointXYZIT {
  optional float x = 1 [default = nan];
  optional float y = 2 [default = nan];
  optional float z = 3 [default = nan];
  optional uint32 intensity = 4 [default = 0];
  optional uint64 timestamp = 5 [default = 0];
}

message PointCloud {
  optional apollo.common.Header header = 1;
  optional string frame_id = 2;
  optional bool is_dense = 3;
  repeated PointXYZIT point = 4;
  optional double measurement_time = 5;
  optional uint32 width = 6;
  optional uint32 height = 7;
}

  • 可以看到点云里面有头部header、帧id frame_d、还包括点 point
  • 点的格式为PointXYZIT其包含x、y、z、intensity、timestamp。
  • 点由repeated修饰，说明point可以重复，有多个，可以当作是点的数组

获取消息对应属性的数据

假设收到了激光雷达发来的点云消息msg。

  • 所有repeated修饰的属性都有大小，可以通过msg->point_size()来获取点云里点的数量。
  • 索引点云：msg->point(i);
  • 获取点的坐标：msg->point(i).x();

获取数据代码

思路：非常简单的障碍物识别，获取点云数据后，若在小车前进方向某一范围内有点，说明前方有障碍物，点的数量超过一定值后就让小车停下来。

为了方便调参，将范围的参数（x_min,x_max,…,z_max）全部提取出来放到control_gflags.cc文件里定义，这样就可以很方便的在control.conf文件里面修改了。

注意：小车的正前方为为x的正方向、左方为y的正方向、上方为Z的正方向。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
// 开始监听channel上的数据
point_cloud_reader_->Observe();
// 获取最近收到的数据
const auto &point_cloud_msg = point_cloud_reader_->GetLatestObserverd();

int point_num = 0;
for( int i = 0; i < point_cloud_msg->point_size(); ++i)
{
  float x = point_cloud_msg->point(i).x();
  float y = point_cloud_msg->point(i).y();
  float z = point_cloud_msg->point(i).z();

  if( x > FLAGS_x_min && x < FLAGS_x_max && y > FLAGS_y_min && 
      y < FLAGS_y_max && z > FLAGS_z_min && z < FLAGS_z_max){
    point_num++;
  }

  if(point_num>20){
    std::cout<< "Obstacle detected. set vehicle speed to zero."<<std::endl;
    // 设置车速为0
    control_command.set_speed(0.0);
    // 设置车的角速度为0
    control_command.set_angular_velocity(0.0);
  }
}

到此，代码就完成了。
将代码编译

1
bash apollo.sh build

编译通过后就可以上车测试了。

仿真测试

若没有小车进行实测，可以先用录制的bag数据进行仿真测试。

  • 播放数据

    1
    cyber_recorder play -f /data/bag/test.00000

  • 启动控制模块

    1
    cyber_launch modules/control/launch/control.launch

    或者

    1
    bash control.sh start_fe

注意：AINFO/ADEBUG打印输出的内容在控制台上看不到，需要用std::cout打印输出。由于没有和小车相连，对应的车地盘数据获取不到，会出现chassis msg not ready的错误信息，这不影响。

上车测试

  • 首先录制一条小车运行的轨迹
  • 让小车自己跑起来，测试小车会不会在遇到障碍物后停下来

注意：这里可以用仿真测试里的命令行的形式启动control模块，而不是在dreamview里打开control模块的开关，这样可以方便查看控制台日志信息。

实操效果