月度归档： 2022年2月

Apollo二次规划算法(piecewise jerk speed optimizer)解析

原创文章，转载请注明： 转载自慢慢的回味

本文链接地址: Apollo二次规划算法(piecewise jerk speed optimizer)解析

Apollo里面最重要的模块就是感知和规划模块，规划模块最重要的是路径规划和速度规划任务，对应ROS机器人里面的局部规划。Apollo的规划模块首先根据当前的情况进行多场景（scenario）调度，每个场景又包含一个或者多个阶段（stage），每个阶段又由多个具有独立功能的小模块任务（task）完成。前面介绍了路径规划piecewise jerk speed optimizer这个任务，这篇文章就解读一下速度规划piecewise jerk speed optimizer这个任务。SL规划保证车辆的横向偏移足够平滑，ST规划保证车辆的前进方向速度变化足够平滑。

Content:
  1. Apollo 中二次规划问题的构造
  2. Apollo代码的实现

继续阅读“Apollo二次规划算法(piecewise jerk speed optimizer)解析”本作品采用知识共享署名 4.0 国际许可协议进行许可。

Cartesian与Frenet坐标系转换公式推导

原创文章，转载请注明： 转载自慢慢的回味

本文链接地址: Cartesian与Frenet坐标系转换公式推导

车在道路上行驶，以车的视角来看，车就如同在一条光滑的曲线上移动，且不时带有左右偏移。为了算法简单，我们选择了Frenet坐标系，它可以把直角坐标系下的复杂轨迹转换为只有S，L两个维度的简单曲线。

目录:
  1. Frenet坐标系简介
  2. Cartesian与Frenet坐标系转换公式推导
    1. 已知\(\vec{x}=[x_{x}, y_{x}]\)推导\([s, l]\)
      1. 求\(s\)
      2. 求\(l\)
    2. 已知\(\vec{x}=[x_{x}, y_{x}],\theta_{x},v_x\)推导\([s, l,\dot{s},l']\)
      1. 求\(l'\)
      2. 求\(\dot{s} \)
    3. 已知\(\vec{x}=[x_{x}, y_{x}],\theta_{x},v_x,a_x,\kappa_{x} \)推导\([s, l,\dot{s},l',\ddot{s},l'']\)
      1. 求\(\l''\)
      2. 求\(\ddot{s}\)

Frenet坐标系简介

如下图所示，3维空间中一条连续可微的曲线K，P为曲线K上的一个点，方格背景平面为曲线K在点P处的运动平面。\vec{T}为曲线K在点P处的切向量，即质点再P处的运动方向；\vec{N}为K在P处的法向量，垂直于质点运动方向\vec{T}，\vec{N}和\vec{T}在同一运动平面；\vec{B}为曲线K在P处的副法向量，且同时垂直于\vec{T}和\vec{N}，即垂直于运动平面。


Frenet的定义公式如下：

    \[ \begin{Bmatrix} \frac{d\vec{T}}{ds}= & \kappa\vec{N} \\ \frac{d\vec{N}}{ds}= & -\kappa\vec{T}+\tau\vec{B} \\ \frac{d\vec{B}}{ds}= & -\tau\vec{N} \end{Bmatrix} \]

其中，\frac{d}{ds}表示某一方向向量对弧长s的导数，\kappa为曲率为曲线相对于直线的弯曲程度，当为0时为直线，表述为曲线运动方向的变化关于弧长的导数（\kappa = \left\| \frac {d\mathbf {T} }{ds}}\right\|），\tau为挠率是曲线不能形成在同一平面内运动曲线的度量值，挠率越趋于0，则曲线越趋近于在同一平面内运动。Apollo的运动在大地上，局部路面可看作一个平面，挠率可设定为0，而Frenet公式可简化为：

(1)   \[ \begin{Bmatrix} \frac{d\vec{T}}{ds}= & \kappa\vec{N} \\ \frac{d\vec{N}}{ds}= & -\kappa\vec{T} \end{Bmatrix} \right \]

继续阅读“Cartesian与Frenet坐标系转换公式推导”本作品采用知识共享署名 4.0 国际许可协议进行许可。

Apollo无人车的消息流转

原创文章，转载请注明： 转载自慢慢的回味

本文链接地址: Apollo无人车的消息流转

Apollo由无数的组件构成，每个组件独立运行，通过消息来进行相互依赖。每个组件构建在Cyber RT框架上，处理一组输入并产生其输出数椐。Launch 文件提供组件入口，DAG 文件配置组件依赖的消息输入等。

Content:
  1. 使用 Cyber RT 创建新的组件
  2. 实现组件类
    1. 头文件
    2. 源文件
    3. 创建 BUILD 文件
    4. 配置 DAG 文件
    5. 配置 Launch 启动文件
  3. Apollo的消息流转
使用 Cyber RT 创建新的组件

要创建并启动一个算法组件，需要通过以下 4 个步骤：

– 初如化组件的目录结构
– 实现组件类
– 设置配置文件
– 启动组件

以cyber/examples/common_component_example目录下的样例程序为例：

– C++头文件: common_component_example.h
– C++源文件: common_component_example.cc
– Bazel 构建文件: BUILD
– DAG 文件: common.dag
– Launch 文件: common.launch

实现组件类
头文件

如何实现`common_component_example.h`:

– 继承 Component 类
– 定义自己的 `Init` 和 `Proc` 函数。Proc 需要指定输入数椐类型。
– 使用`CYBER_REGISTER_COMPONENT`宏定义把组件类注册成全局可用。

#include <memory>
 
#include "cyber/component/component.h"
#include "cyber/examples/proto/examples.pb.h"
 
using apollo::cyber::Component;
using apollo::cyber::ComponentBase;
using apollo::cyber::examples::proto::Driver;
 
class CommonComponentSample : public Component<Driver, Driver> {
 public:
  bool Init() override;
  bool Proc(const std::shared_ptr<Driver>& msg0,
            const std::shared_ptr<Driver>& msg1) override;
};
CYBER_REGISTER_COMPONENT(CommonComponentSample)
源文件

对于源文件 `common_component_example.cc`, `Init` 和 `Proc` 这两个函数需要实现。

#include "cyber/examples/common_component_example/common_component_example.h"
 
bool CommonComponentSample::Init() {
  AINFO << "Commontest component init";
  return true;
}
 
bool CommonComponentSample::Proc(const std::shared_ptr<Driver>& msg0,
                                 const std::shared_ptr<Driver>& msg1) {
  AINFO << "Start common component Proc [" << msg0->msg_id() << "] ["
        << msg1->msg_id() << "]";
  return true;
}

继续阅读“Apollo无人车的消息流转”本作品采用知识共享署名 4.0 国际许可协议进行许可。

Apollo二次规划算法(piecewise jerk path optimizer)解析

原创文章，转载请注明： 转载自慢慢的回味

本文链接地址: Apollo二次规划算法(piecewise jerk path optimizer)解析

Apollo里面最重要的模块就是感知和规划模块，规划模块最重要的是路径规划和速度规划任务，对应ROS机器人里面的局部规划。Apollo的规划模块首先根据当前的情况进行多场景（scenario）调度，每个场景又包含一个或者多个阶段（stage），每个阶段又由多个具有独立功能的小模块任务（task）完成。这篇文章就主要解读一下路径规划piecewise jerk path optimizer这个任务。任务最终会生成轨迹（trajectory）：每个点的位姿和速度信息，进而输出给控制模块去控制车辆。

Content:
  1. 任务代码调用入口
  2. 二次规划定义
  3. Apollo 中二次规划问题的构造
  4. Apollo代码的实现
  5. Python代码的实现
  6. 参考文献

任务代码调用入口

比如通过测试用例可得到如下的堆栈：

apollo::planning::PiecewiseJerkPathOptimizer::Process(apollo::planning::PiecewiseJerkPathOptimizer * const this, const apollo::planning::SpeedData & speed_data, const apollo::planning::ReferenceLine & reference_line, const apollo::common::TrajectoryPoint & init_point, const bool path_reusable, apollo::planning::PathData * const final_path_data) (piecewise_jerk_path_optimizer.cc:61)
apollo::planning::PathOptimizer::Execute(apollo::planning::PathOptimizer * const this, apollo::planning::Frame * frame, apollo::planning::ReferenceLineInfo * const reference_line_info) (path_optimizer.cc:45)
apollo::planning::scenario::lane_follow::LaneFollowStage::PlanOnReferenceLine(apollo::planning::scenario::lane_follow::LaneFollowStage * const this, const apollo::common::TrajectoryPoint & planning_start_point, apollo::planning::Frame * frame, apollo::planning::ReferenceLineInfo * reference_line_info) (lane_follow_stage.cc:167)
apollo::planning::scenario::lane_follow::LaneFollowStage::Process(apollo::planning::scenario::lane_follow::LaneFollowStage * const this, const apollo::common::TrajectoryPoint & planning_start_point, apollo::planning::Frame * frame) (lane_follow_stage.cc:116)
apollo::planning::scenario::Scenario::Process(apollo::planning::scenario::Scenario * const this, const apollo::common::TrajectoryPoint & planning_init_point, apollo::planning::Frame * frame) (scenario.cc:76)
apollo::planning::PublicRoadPlanner::Plan(apollo::planning::PublicRoadPlanner * const this, const apollo::common::TrajectoryPoint & planning_start_point, apollo::planning::Frame * frame, apollo::planning::ADCTrajectory * ptr_computed_trajectory) (public_road_planner.cc:38)
apollo::planning::OnLanePlanning::Plan(apollo::planning::OnLanePlanning * const this, const double current_time_stamp, const std::vector<apollo::common::TrajectoryPoint, std::allocator<apollo::common::TrajectoryPoint> > & stitching_trajectory, apollo::planning::ADCTrajectory * const ptr_trajectory_pb) (on_lane_planning.cc:572)
apollo::planning::OnLanePlanning::RunOnce(apollo::planning::OnLanePlanning * const this, const apollo::planning::LocalView & local_view, apollo::planning::ADCTrajectory * const ptr_trajectory_pb) (on_lane_planning.cc:417)
apollo::planning::PlanningTestBase::RunPlanning(apollo::planning::PlanningTestBase * const this, const std::__cxx11::string & test_case_name, int case_num, bool no_trajectory_point) (planning_test_base.cc:229)
apollo::planning::SunnyvaleBigLoopTest_keep_clear_02_Test::TestBody(apollo::planning::SunnyvaleBigLoopTest_keep_clear_02_Test * const this) (sunnyvale_big_loop_test.cc:191)

继续阅读“Apollo二次规划算法(piecewise jerk path optimizer)解析”本作品采用知识共享署名 4.0 国际许可协议进行许可。

使用VSCode 调试Apollo无人车代码

原创文章，转载请注明： 转载自慢慢的回味

本文链接地址: 使用VSCode 调试Apollo无人车代码

深入研究Apollo的代码是学习自动驾驶的很好途径。很多前沿科技，比如图像识别，激光雷达，多传感器融合，路径规划都可以直接完整的学习。能够直接调试代码是比读代码更能加深理解。本文就介绍怎么去调试Apollo的代码。

Content:
  1. 下载代码
  2. 安装Docker
  3. 安装无线网卡驱动（可选）
  4. 安装Nvidia驱动
  5. 禁止自动更新内核来避免重新安装驱动
  6. 安装Nvidia Docker
  7. 启动Apollo项目
  8. 调试一个车道线识别程序
  9. Windows下使用VS Code Dev Container

下载代码

系统使用Ubuntu 18.04版本。
https://gitee.com/ApolloAuto/apollo.git
比如目录为~/apollo。

安装Docker
curl -fsSL https://download.docker.com/linux/ubuntu/gpg | sudo gpg --dearmor -o /usr/share/keyrings/docker-archive-keyring.gpg
echo   "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/docker-archive-keyring.gpg] https://download.docker.com/linux/ubuntu \
$(lsb_release -cs) stable" | sudo tee /etc/apt/sources.list.d/docker.list > /dev/null
 
sudo apt-get update
sudo apt-get install docker-ce docker-ce-cli containerd.io
systemctl daemon-reload
systemctl restart docker
安装无线网卡驱动（可选）

因为笔者的网卡Ubuntu没有自带，需要自行安装驱动。
下载驱动：https://codeload.github.com/gnab/rtl8812au/zip/refs/heads/master

make dkms_install
echo 8812au | sudo tee -a /etc/modules
insmod 8812au.ko
安装Nvidia驱动

本人的显卡为GTX1060。Apollo项目需要Nvidia显卡，否则大部分模块无法编译运行。
如下为安装显卡驱动的脚步程序。cuda安装后的路径可能有所不同。

sudo apt-get install linux-headers-$(uname -r)
distribution=$(. /etc/os-release;echo $ID$VERSION_ID | sed -e 's/\.//g')
wget https://developer.download.nvidia.com/compute/cuda/repos/$distribution/x86_64/cuda-$distribution.pin
sudo mv cuda-$distribution.pin /etc/apt/preferences.d/cuda-repository-pin-600
#key的id可以会更新 https://developer.nvidia.com/blog/updating-the-cuda-linux-gpg-repository-key/
sudo apt-key adv --fetch-keys https://developer.download.nvidia.com/compute/cuda/repos/$distribution/x86_64/7fa2af80.pub
echo "deb http://developer.download.nvidia.com/compute/cuda/repos/$distribution/x86_64 /" | sudo tee /etc/apt/sources.list.d/cuda.list
sudo apt-get update
sudo apt-get -y install cuda-drivers
export PATH=$PATH:/usr/local/cuda-11.2/bin
export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/cuda-11.2/lib64:/usr/local/cuda/cuda/lib64

安装完成后，如下命令可以查看显卡信息。请确保cuda已正确安装。

nvidia-smi
禁止自动更新内核来避免重新安装驱动

1 查看自己使用的内核

derek@ubuntu:~$ uname -a
Linux ubuntu 5.4.0-150-generic #167~18.04.1-Ubuntu SMP Wed May 24 00:51:42 UTC 2023 x86_64 x86_64 x86_64 GNU/Linux

2 查看目前安装的内核，并锁定它们

derek@ubuntu:~$ dpkg --get-selections|grep linux|grep 150
linux-headers-5.4.0-150-generic			install
linux-hwe-5.4-headers-5.4.0-150			install
linux-image-5.4.0-150-generic			install
linux-modules-5.4.0-150-generic			install
linux-modules-extra-5.4.0-150-generic		install
 
derek@ubuntu:~$ sudo apt-mark hold linux-headers-5.4.0-150-generic
linux-headers-5.4.0-150-generic set on hold.
derek@ubuntu:~$ sudo apt-mark hold linux-hwe-5.4-headers-5.4.0-150
linux-hwe-5.4-headers-5.4.0-150 set on hold.
derek@ubuntu:~$ sudo apt-mark hold linux-image-5.4.0-150-generic
linux-image-5.4.0-150-generic set on hold.
derek@ubuntu:~$ sudo apt-mark hold linux-modules-5.4.0-150-generic
linux-modules-5.4.0-150-generic set on hold.
derek@ubuntu:~$ sudo apt-mark hold linux-modules-extra-5.4.0-150-generic
linux-modules-extra-5.4.0-150-generic set on hold
 
derek@ubuntu:~$ dpkg --get-selections|grep linux|grep 150
linux-headers-5.4.0-150-generic			hold
linux-hwe-5.4-headers-5.4.0-150			hold
linux-image-5.4.0-150-generic			hold
linux-modules-5.4.0-150-generic			hold
linux-modules-extra-5.4.0-150-generic		hold

3 修改软件自动更新
修改如下2项

安装Nvidia Docker

Apollo需要运行在Docker预安装环境，以加快开发运行环境的统一性。

distribution=$(. /etc/os-release;echo $ID$VERSION_ID)    && curl -s -L https://nvidia.github.io/nvidia-docker/gpgkey | sudo apt-key add -    && curl -s -L https://nvidia.github.io/nvidia-docker/$distribution/nvidia-docker.list | sudo tee /etc/apt/sources.list.d/nvidia-docker.list
sudo apt-get update
sudo apt-get install -y nvidia-docker2
sudo systemctl restart docker

安装完成后，可启动示例Dockers程序查看。

sudo docker run --rm --gpus all nvidia/cuda:11.0-base nvidia-smi
启动Apollo项目

在源码目录~/apollo中，执行如下命令，进入Apollo Docker：
脚本会把源文件里面的目录以volumn的形式挂载进Docker环境。

./docker/scripts/dev_start.sh 
./docker/scripts/dev_into.sh

以后进入，直接启动已有的Docker即可：

docker ps -a
docker start 671567b64765
./docker/scripts/dev_into.sh
调试一个车道线识别程序

创建一个Bazel编译配置：
~/apollo/modules/perception/camera/test/BUILD
内容为：

load("@rules_cc//cc:defs.bzl", "cc_library", "cc_test")
load("//tools:cpplint.bzl", "cpplint")
 
package(default_visibility = ["//visibility:public"])
 
cc_test(
    name = "camera_lib_lane_detector_darkscnn_lane_detector_test",
    size = "medium",
    srcs = ["camera_lib_lane_detector_darkscnn_lane_detector_test.cc"],
    deps = [
        "//cyber",
        "//modules/perception/base",
        "//modules/perception/camera/lib/lane/detector/darkSCNN:darkSCNN_lane_detector",
        "//modules/perception/common/io:io_util",
        "@com_google_googletest//:gtest_main",
        "@opencv//:core",
    ],
)
 
cpplint()

现在可以编译Apollo程序了：

./apollo.sh build_dbg

启动GDB Server，供Docker外面的VS Code进行远程调试：

export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/libtorch_gpu/lib/
apt update
apt install gdbserver
gdbserver 127.0.0.1:2222 bazel-bin/modules/perception/camera/test/camera_lib_lane_detector_darkscnn_lane_detector_test

启动VS Code，安装C++插件，然后点击Debug，添加如下配置即可调试Docker里面刚才启动的程序：(注意修改program的路径)

{
    "version": "0.2.0",
    "configurations": [
        {
            "name": "gdb Remote camera_lib_lane_postprocessor_darkscnn_lane_postprocessor_test",
            "type": "cppdbg",
            "request": "launch",
            "program": "~/apollo/.cache/bazel/540135163923dd7d5820f3ee4b306b32/execroot/apollo/bazel-out/k8-dbg/bin/modules/perception/camera/test/camera_lib_lane_detector_darkscnn_lane_detector_test",
            "args": ["myarg1", "myarg2", "myarg3"],
            "stopAtEntry": true,
            "environment": [],
            "externalConsole": false,
            "MIMode": "gdb",
            "miDebuggerPath": "gdb",
            "miDebuggerArgs": "gdb",
            "linux": {
                "MIMode": "gdb",
                "miDebuggerPath": "/usr/bin/gdb",
                "miDebuggerServerAddress": "127.0.0.1:2222",
            },
            "logging": {
                "moduleLoad": false,
                "engineLogging": false,
                "trace": false
            },
            "setupCommands": [
                {
                    "description": "Enable pretty-printing for gdb",
                    "text": "-enable-pretty-printing",
                    "ignoreFailures": true
                }
            ],
            "cwd": "${workspaceFolder}",
        }
    ]
}
Windows下使用VS Code Dev Container

Windows下使用VS Code Dev Container来调试也是比较方便的，Dev Container可以让你在Windows下的操作如同在Container里面。

在项目根目录创建文件：.devcontainer/Dockerfile

FROM registry.baidubce.com/apolloauto/apollo:dev-x86_64-18.04-20210914_1336
 
ENV SHELL /bin/bash

在项目根目录创建文件：.devcontainer/devcontainer.json

{
    "name": "Apollo Dev Container",
    "build": {
      "dockerfile": "./Dockerfile"
    },
    "remoteUser": "root",
    //docker run -v apollo_map_volume-sunnyvale_big_loop_root:/apollo/modules/map/data/sunnyvale_big_loop --rm registry.baidubce.com/apolloauto/apollo:map_volume-sunnyvale_big_loop-latest true
    //docker run -v apollo_map_volume-sunnyvale_loop-latest:/apollo/modules/map/data/sunnyvale_loop --rm registry.baidubce.com/apolloauto/apollo:map_volume-sunnyvale_loop-latest true
    //docker run -v apollo_audio_volume_root:/apollo/modules/audio/data/ --rm registry.baidubce.com/apolloauto/apollo:data_volume-audio_model-x86_64-latest true
    "runArgs": [
        "--privileged",
        "--net=host",
        "-v", "apollo_map_volume-sunnyvale_big_loop_root:/apollo/modules/map/data/sunnyvale_big_loop",
        "-v", "apollo_map_volume-sunnyvale_loop-latest:/apollo/modules/map/data/sunnyvale_loop",
        "-v", "apollo_audio_volume_root:/apollo/modules/audio/data/",
        "--env", "CROSS_PLATFORM=0",
        "--env", "USER=root",
        "--env", "DOCKER_USER=root",
        "--env", "DOCKER_USER_ID=0",
        "--env", "DOCKER_GRP=0",
        "--env", "DOCKER_GRP_ID=0",
        "--env", "DOCKER_IMG=apolloauto/apollo:dev-x86_64-18.04-20210914_1336",
        "--env", "USE_GPU_HOST=false", // Set to 'true' or 'false' depending on the script
        "--env", "NVIDIA_VISIBLE_DEVICES=all",
        "--env", "NVIDIA_DRIVER_CAPABILITIES=compute,video,graphics,utility",
        "--workdir", "/apollo",
        "--add-host", "in-dev-docker:127.0.0.1",
        "--add-host", "localhost:127.0.0.1",
        "--hostname", "in-dev-docker"
    ],
    "workspaceMount": "type=bind,source=${localWorkspaceFolder},target=/apollo",
    "customizations": {
      "vscode": {
        "settings.json": {
          "terminal.integrated.profiles.linux": { "bash": { "path": "/bin/bash" } }
        }
      }
    }
}

最后，安装上VS Code 的 Dev Container 插件后就可以 Open In Container 了。

本作品采用知识共享署名 4.0 国际许可协议进行许可。