一、论文情况

  1. 论文
    [1] HAN Z, WU Y, LI T, 等. An efficient spatial-temporal trajectory planner for autonomous vehicles in unstructured environments[J/OL]. IEEE Transactions On Intelligent Transportation Systems, 2024, 25(2): 1797-1814. DOI:10.1109/TITS.2023.3315320.

  2. 论文链接
    https://arxiv.org/abs/2208.13160

  3. 源码链接
    https://github.com/ZJU-FAST-Lab/Dftpav?tab=readme-ov-file

二、复现

0. 笔者环境

Ubuntu; GPU: RTX 5060;
（关于电脑性能，本实验要求并不高；但在编译过程以及运行 roslaunch 指令时，可能需要稍微调整。 ）
使用 docker 容器复现实验。

1. 创建 docker 容器

使用 ROS Melodic 的镜像；（docker pull osrf/ros:melodic-desktop-full）
step1. 下载 github 项目的 zip 文件，将 zip 文件解压缩后，存储到挂载目录下
step2. 创建 docker 容器

# -v <宿主机目录>:<容器内目录> 将宿主机上的目录挂载到容器内部的目录
docker run -it \
--name=Dftpav \
--gpus=all \
-e NVIDIA_DRIVER_CAPABILITIES=all \
--network=host \
--privileged=true \
-e DISPLAY=$DISPLAY \
-e WAYLAND_DISPLAY=$WAYLAND_DISPLAY \
-e XDG_RUNTIME_DIR=$XDG_RUNTIME_DIR \
-e PULSE_SERVER=$PULSE_SERVER \
-v /tmp/.X11-unix:/tmp/.X11-unix \
-v /home/xiaobai/projects/Dftpav:/catkin_ws/src/Dftpav \
osrf/ros:melodic-desktop-full \
/bin/bash

2. 安装依赖

step1. apt 安装基础依赖

sudo apt update && apt install -y libgoogle-glog-dev libdw-dev libopenblas-dev gfortran wget unzip

step2. 安装 protobuf
使用 protoc --version 指令，可以发现 osrf/ros:melodic-desktop-full 已经安装好了 libprotoc 3.0.0，因此无需再次安装。

step3. 安装其他依赖

# 安装缺失的 ompl 和 msgs
apt install -y libompl* ros-melodic-geographic-msgs ros-melodic-tf2-sensor-msgs

注意：这里一定一定 要安装和 ros 无关的 ompl 库（libompl*，星号不可以省略）！


3. 编译

source /opt/ros/melodic/setup.bash
cd /catkin_ws
catkin_make -DCMAKE_BUILD_TYPE=Release

此处，编译若是遇到下列问题，请参考本文的第三部分去寻找解决方法。

① 找不到 ompl
② 内存不足（c++: internal compiler error: Killed(program cppplus), Invoking “make -j16 -l16” failed）

4. 运行

cd /catkin_ws
source devel/setup.bash
cd /catkin_ws/src/Dftpav/
./run.sh

在 Rviz 界面点击 2D Nav Goal 发布点，可以激活规划程序。

在这里插入图片描述

这里，若是遇到 “No map is update” 的问题，参考本文的第三部分解决。


ps：最后，为了避免每次运行项目都要 source，将 source 语句追加到 bashrc 文件中

echo -e "\n# ROS\nsource /opt/ros/melodic/setup.bash\nsource /catkin_ws/devel/setup.bash" >> /root/.bashrc

此外，笔者为了以后开发方便，将环境打包成镜像：

docker commit Dftpav dftpav:v0
docker save -o dftpav.tar dftpav:v0

最终的镜像 tar 文件保存在 /home/xiaobai/projects 目录。

三、遇到的问题记录

1. ompl 相关问题

在进行 catkin 编译时，遇到问题：

root@docker-desktop:/catkin_ws# catkin_make -DCMAKE_BUILD_TYPE=Release
Base path: /catkin_ws
Source space: /catkin_ws/src
Build space: /catkin_ws/build
Devel space: /catkin_ws/devel
Install space: /catkin_ws/install
Creating symlink “/catkin_ws/src/CMakeLists.txt” pointing to “/opt/ros/melodic/share/catkin/cmake/toplevel.cmake”

Running command: “cmake /catkin_ws/src -DCMAKE_BUILD_TYPE=Release -DCATKIN_DEVEL_PREFIX=/catkin_ws/devel -DCMAKE_INSTALL_PREFIX=/catkin_ws/install -G Unix Makefiles” in “/catkin_ws/build”

– The C compiler identification is GNU 7.5.0
– The CXX compiler identification is GNU 7.5.0
– Check for working C compiler: /usr/bin/cc
– Check for working C compiler: /usr/bin/cc – works
– Detecting C compiler ABI info
– Detecting C compiler ABI info - done
– Detecting C compile features
– Detecting C compile features - done
– Check for working CXX compiler: /usr/bin/c++
– Check for working CXX compiler: /usr/bin/c++ – works
– Detecting CXX compiler ABI info
– Detecting CXX compiler ABI info - done
– Detecting CXX compile features
– Detecting CXX compile features - done
– Using CATKIN_DEVEL_PREFIX: /catkin_ws/devel
– Using CMAKE_PREFIX_PATH: /opt/ros/melodic
– This workspace overlays: /opt/ros/melodic
– Found PythonInterp: /usr/bin/python2 (found suitable version “2.7.17”, minimum required is “2”)
– Using PYTHON_EXECUTABLE: /usr/bin/python2
– Using Debian Python package layout
– Using empy: /usr/bin/empy
– Using CATKIN_ENABLE_TESTING: ON
– Call enable_testing()
– Using CATKIN_TEST_RESULTS_DIR: /catkin_ws/build/test_results
– Found gtest sources under ‘/usr/src/googletest’: gtests will be built
– Found gmock sources under ‘/usr/src/googletest’: gmock will be built
– Found PythonInterp: /usr/bin/python2 (found version “2.7.17”)
– Looking for pthread.h
– Looking for pthread.h - found
– Looking for pthread_create
– Looking for pthread_create - not found
– Looking for pthread_create in pthreads
– Looking for pthread_create in pthreads - not found
– Looking for pthread_create in pthread
– Looking for pthread_create in pthread - found
– Found Threads: TRUE
– Using Python nosetests: /usr/bin/nosetests-2.7
– catkin 0.7.29
– BUILD_SHARED_LIBS is on
– BUILD_SHARED_LIBS is on
– ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
– ~~ traversing 19 packages in topological order:
– ~~ - catkin_simple
– ~~ - decomp_ros_msgs
– ~~ - playgrounds
– ~~ - common
– ~~ - decomp_ros_utils
– ~~ - vehicle_model
– ~~ - forward_simulator
– ~~ - motion_predictor
– ~~ - vehicle_msgs
– ~~ - aux_tools
– ~~ - phy_simulator
– ~~ - semantic_map_manager
– ~~ - route_planner
– ~~ - behavior_planner
– ~~ - ai_agent_planner
– ~~ - eudm_planner
– ~~ - ssc_planner
– ~~ - traj_planner
– ~~ - planning_integrated
– ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
– +++ processing catkin package: ‘catkin_simple’
– ==> add_subdirectory(Dftpav/src/Plan/utils/catkin_simple)
– +++ processing catkin package: ‘decomp_ros_msgs’
– ==> add_subdirectory(Dftpav/src/Plan/utils/DecompROS/decomp_ros_msgs)
– Using these message generators: gencpp;geneus;genlisp;gennodejs;genpy
– decomp_ros_msgs: 4 messages, 0 services
– +++ processing catkin package: ‘playgrounds’
– ==> add_subdirectory(Dftpav/src/Sim/core/playgrounds)
– +++ processing catkin package: ‘common’
– ==> add_subdirectory(Dftpav/src/Sim/core/common)
– Using these message generators: gencpp;geneus;genlisp;gennodejs;genpy
– Found Protobuf: /usr/local/lib/libprotobuf.so;-lpthread (found version “3.21.12”)
– Found Glog: /usr/include
– Found glog (include: /usr/include, library: /usr/lib/x86_64-linux-gnu/libglog.so)
– Found OpenCV: /usr (found version “3.2.0”)
– Found OOQP include dirs: /catkin_ws/src/Dftpav/src/Sim/core/common/thirdparty/ooqp/include/ooqp
– Found OOQP libraries: /catkin_ws/src/Dftpav/src/Sim/core/common/thirdparty/ooqp/lib/libooqpgensparse.a;/catkin_ws/src/Dftpav/src/Sim/core/common/thirdparty/ooqp/lib/libooqpsparse.a;/catkin_ws/src/Dftpav/src/Sim/core/common/thirdparty/ooqp/lib/libooqpgondzio.a;/catkin_ws/src/Dftpav/src/Sim/core/common/thirdparty/ooqp/lib/libooqpbase.a;/catkin_ws/src/Dftpav/src/Sim/core/common/thirdparty/ooqp/lib/libma27.a;blas;gfortran
– Found OOQP
– Found NLOPT include dirs: /catkin_ws/src/Dftpav/src/Sim/core/common/thirdparty/nlopt/include
– Found NLOPT libraries: /catkin_ws/src/Dftpav/src/Sim/core/common/thirdparty/nlopt/lib/libnlopt.so
– Found NLOPT
– +++ processing catkin package: ‘decomp_ros_utils’
– ==> add_subdirectory(Dftpav/src/Plan/utils/DecompROS/decomp_ros_utils)
– Using these message generators: gencpp;geneus;genlisp;gennodejs;genpy
– Using Qt5 based on the rviz_QT_VERSION: 5.9.5
– Marking SHARED_LIBRARY “decomp_rviz_plugins” of package “decomp_ros_utils” for installation
– Marking HEADER FILES in “include” folder of package “decomp_ros_utils” for installation
– +++ processing catkin package: ‘vehicle_model’
– ==> add_subdirectory(Dftpav/src/Sim/core/vehicle_model)
– Using these message generators: gencpp;geneus;genlisp;gennodejs;genpy
– +++ processing catkin package: ‘forward_simulator’
– ==> add_subdirectory(Dftpav/src/Sim/core/forward_simulator)
– Using these message generators: gencpp;geneus;genlisp;gennodejs;genpy
– +++ processing catkin package: ‘motion_predictor’
– ==> add_subdirectory(Dftpav/src/Sim/core/motion_predictor)
– Using these message generators: gencpp;geneus;genlisp;gennodejs;genpy
– +++ processing catkin package: ‘vehicle_msgs’
– ==> add_subdirectory(Dftpav/src/Sim/core/vehicle_msgs)
– Using these message generators: gencpp;geneus;genlisp;gennodejs;genpy
– vehicle_msgs: 17 messages, 0 services
– +++ processing catkin package: ‘aux_tools’
– ==> add_subdirectory(Dftpav/src/Sim/aux_tools)
– Using these message generators: gencpp;geneus;genlisp;gennodejs;genpy
– +++ processing catkin package: ‘phy_simulator’
– ==> add_subdirectory(Dftpav/src/Sim/core/phy_simulator)
– Using these message generators: gencpp;geneus;genlisp;gennodejs;genpy
– +++ processing catkin package: ‘semantic_map_manager’
– ==> add_subdirectory(Dftpav/src/Sim/core/semantic_map_manager)
– Using these message generators: gencpp;geneus;genlisp;gennodejs;genpy
– +++ processing catkin package: ‘route_planner’
– ==> add_subdirectory(Dftpav/src/Sim/core/route_planner)
– Using these message generators: gencpp;geneus;genlisp;gennodejs;genpy
– +++ processing catkin package: ‘behavior_planner’
– ==> add_subdirectory(Dftpav/src/Sim/core/behavior_planner)
– Using these message generators: gencpp;geneus;genlisp;gennodejs;genpy
– +++ processing catkin package: ‘ai_agent_planner’
– ==> add_subdirectory(Dftpav/src/Sim/util/ai_agent_planner)
– Using these message generators: gencpp;geneus;genlisp;gennodejs;genpy
– +++ processing catkin package: ‘eudm_planner’
– ==> add_subdirectory(Dftpav/src/Sim/util/eudm_planner)
– Using these message generators: gencpp;geneus;genlisp;gennodejs;genpy
CMake Warning at /opt/ros/melodic/share/catkin/cmake/catkin_package.cmake:418 (message):
catkin_package() include dir
‘/catkin_ws/build/Dftpav/src/Sim/util/eudm_planner’ should be placed in the
devel space instead of the build space
Call Stack (most recent call first):
/opt/ros/melodic/share/catkin/cmake/catkin_package.cmake:102 (_catkin_package)
Dftpav/src/Sim/util/eudm_planner/CMakeLists.txt:30 (catkin_package)
– +++ processing catkin package: ‘ssc_planner’
– ==> add_subdirectory(Dftpav/src/Sim/util/ssc_planner)
– Found OpenMP_C: -fopenmp (found version “4.5”)
– Found OpenMP_CXX: -fopenmp (found version “4.5”)
– Found OpenMP: TRUE (found version “4.5”)
– Using these message generators: gencpp;geneus;genlisp;gennodejs;genpy
CMake Warning at /opt/ros/melodic/share/catkin/cmake/catkin_package.cmake:418 (message):
catkin_package() include dir
‘/catkin_ws/build/Dftpav/src/Sim/util/ssc_planner’ should be placed in the
devel space instead of the build space
Call Stack (most recent call first):
/opt/ros/melodic/share/catkin/cmake/catkin_package.cmake:102 (_catkin_package)
Dftpav/src/Sim/util/ssc_planner/CMakeLists.txt:28 (catkin_package)
– +++ processing catkin package: ‘traj_planner’
– ==> add_subdirectory(Dftpav/src/Plan/traj_planner)
– Found OpenMP_C: -fopenmp
– Found OpenMP_CXX: -fopenmp
– Found OpenMP: TRUE
– Using these message generators: gencpp;geneus;genlisp;gennodejs;genpy
CMake Error at Dftpav/src/Plan/traj_planner/CMakeLists.txt:16 (find_package):
By not providing “Findompl.cmake” in CMAKE_MODULE_PATH this project has
asked CMake to find a package configuration file provided by “ompl”, but
CMake did not find one.
Could not find a package configuration file provided by “ompl” with any of
the following names:
omplConfig.cmake
ompl-config.cmake
Add the installation prefix of “ompl” to CMAKE_PREFIX_PATH or set
“ompl_DIR” to a directory containing one of the above files. If “ompl”
provides a separate development package or SDK, be sure it has been
installed.
– Configuring incomplete, errors occurred!
See also “/catkin_ws/build/CMakeFiles/CMakeOutput.log”.
See also “/catkin_ws/build/CMakeFiles/CMakeError.log”.
Invoking “cmake” failed

在这里插入图片描述

这是因为 OMPL 库缺失，一开始只想到安装 ros 相关的 ompl（AI 推荐）

apt-get install -y ros-melodic-ompl

但是，安装后，重新编译，又遇到下列问题：

– +++ processing catkin package: ‘planning_integrated’
– ==> add_subdirectory(Dftpav/src/Sim/app/planning_integrated)
– Could NOT find geographic_msgs (missing: geographic_msgs_DIR)
– Could not find the required component ‘geographic_msgs’. The following CMake error indicates that you either need to install the package with the same name or change your environment so that it can be found.
CMake Error at /opt/ros/melodic/share/catkin/cmake/catkinConfig.cmake:83 (find_package):
Could not find a package configuration file provided by “geographic_msgs”
with any of the following names:
geographic_msgsConfig.cmake
geographic_msgs-config.cmake
Add the installation prefix of “geographic_msgs” to CMAKE_PREFIX_PATH or
set “geographic_msgs_DIR” to a directory containing one of the above files.
If “geographic_msgs” provides a separate development package or SDK, be
sure it has been installed.
Call Stack (most recent call first):
Dftpav/src/Sim/app/planning_integrated/CMakeLists.txt:11 (find_package)
– Configuring incomplete, errors occurred!
See also “/catkin_ws/build/CMakeFiles/CMakeOutput.log”.
See also “/catkin_ws/build/CMakeFiles/CMakeError.log”.
Invoking “cmake” failed

在这里插入图片描述

缺失很多包，一开始推测该项目很有可能是在 ROS Neotic 环境下实现的。
后面重新在 noetic 环境下运行该项目，还是遇到了同样的问题，缺失一些库。
说明无论是 ros melodic 还是 ros neotic 容器都缺少这些包。

安装好缺失的库

apt install -y ros-melodic-ompl ros-melodic-geographic-msgs ros-melodic-tf2-sensor-msgs

再次重新编译，还是遇到问题：

In file included from /catkin_ws/src/Dftpav/src/Plan/traj_planner/src/kino_astar.cpp:1:
/catkin_ws/src/Dftpav/src/Plan/traj_planner/include/path_searching/kino_astar.h:29:10: fatal errof95r: ompl/base/spaces/ReedsSheppStateSpace.h: No such file or directory
29 | #include <ompl/base/spaces/ReedsSheppStateSpace.h>
| ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
compilation terminated.
make[2]: *** [Dftpav/src/Plan/traj_planner/CMakeFiles/traj_planner.dir/build.make:113: Dftpav/src/Plan/traj_planner/CMakeFiles/traj_planner.dir/src/kino_astar.cpp.o] Error 1
make[2]: *** Waiting for unfinished jobs…
In file included from /catkin_ws/src/Dftpav/src/Plan/traj_planner/include/plan_manage/traj_manager.h:33,
from /catkin_ws/src/Dftpav/src/Plan/traj_planner/src/traj_manager.cpp:1:
/catkin_ws/src/Dftpav/src/Plan/traj_planner/include/path_searching/kino_astar.h:29:10: fatal error: ompl/base/spaces/ReedsSheppStateSpace.h: No such file or directory
29 | #include <ompl/base/spaces/ReedsSheppStateSpace.h>
| ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
compilation terminated.
make[2]: *** [Dftpav/src/Plan/traj_planner/CMakeFiles/traj_planner.dir/build.make:87: Dftpav/src/Plan/traj_planner/CMakeFiles/traj_planner.dir/src/traj_manager.cpp.o] Error 1
make[1]: *** [CMakeFiles/Makefile2:4922: Dftpav/src/Plan/traj_planner/CMakeFiles/traj_planner.dir/all] Error 2
make: *** [Makefile:141: all] Error 2
Invoking “make -j16 -l16” failed

在这里插入图片描述

若只是在 include_directories 中添加 ${OMPL_INCLUDE_DIRS}，则显示如下：

在这里插入图片描述

这说明头文件找不到的问题已经解决了！现在遇到的是链接错误，说明编译通过了，但在链接阶段找不到某些符号。

继续在 target_link_libraries 中添加 ${OMPL_LIBRARIES}，但会遇到下图的问题
①

在这里插入图片描述

这是因为内存不足，导致编译器进程被系统杀死。

②

在这里插入图片描述
CMake 找到了 OMPL 的头文件，但链接时找不到对应的库文件 libompl.so。

已经在 CMakeLists.txt 中通过 message 确定了 OMPL 相关变量的位置正确，但还是有问题。

message(WARNING "========== OMPL_INCLUDE_DIRS: ${OMPL_INCLUDE_DIRS} ==============")
# ${OMPL_INCLUDE_DIRS} 为 /opt/ros/melodic/include/ompl-1.4

message(WARNING "========== OMPL_LIBRARIES: ${OMPL_LIBRARIES} ==============")
# /opt/ros/melodic/lib/libompl.so

说明这里的链接应该是没问题的。

在 Dftpav github 的 issues 中发现下列提问：

在这里插入图片描述

在这里插入图片描述

那么，需要 卸载 ros 版本的 ompl，直接通过 apt 安装和 ros 无关的 libompl* ！

# 卸载 ros-melodic-ompl
apt purge ros-melodic-ompl -y
apt autoremove

# 安装 libompl
apt install libompl* -y

注意，libompl 后面需要添加星号 *，否则会显示 E: Unable to locate package libompl

将修改后的 CMakeLists.txt 还原（注释掉添加的两行）；

2. 内存不足

（续接上文）
再删除掉之前的编译信息，重新编译：

rm -rf /catkin_ws/build/ /catkin_ws/devel/
catkin_make -DCMAKE_BUILD_TYPE=Release

此时，出现的问题是

在这里插入图片描述
在这里插入图片描述

这是因为 docker 容器的内存不足，导致编译器进程被系统杀死。
因此，采用更少的线程进行编译（缺点：耗时更长）。

rm -rf /catkin_ws/build/ /catkin_ws/devel/
catkin_make -DCMAKE_BUILD_TYPE=Release -j4 -l4

最终编译成功！

在这里插入图片描述

3. No map is update

（续接上文）
但在尝试运行时，还是遇到问题：

在这里插入图片描述
在这里插入图片描述

幸运的是，github issues 中有记载这个问题！

在这里插入图片描述

因此，直接在 run.sh 中将 sleep 1 修改成 sleep 5。
问题成功解决，点击 2D Nav Goal 启动规划，出现仿真界面如下：

在这里插入图片描述

在代码中，可以发现 “No map is update” 出现在 src/Plan/traj_planner/src/traj_server_ros.cpp 的 line140 处，在 TrajPlannerServer 没有获取到最新的 smm 时，会打印该语句。
实际上，park.cc 会启动 2 个规划器 Server，另外一个是 BehaviorPlannerServer，但是它的逻辑是：接收到最新的 smm 才开始后续的规划，不同于 TrajPlannerServer 判断出没接收到最新 smm 就立即 return。

（这接连两个问题，我认为可能是电脑的性能不够，在其它性能好的电脑上未必会出现这些问题。）
（确实，第二次换了一台 RTX 5060 的电脑，可以直接编译，直接运行！）
（第一次在 RTX 3060 的电脑上需要用更少的线程去编译、run.sh 中每条 roslaunch 指令需要的 sleep 时间也更长。）
（不过也有可能是因为 WSL 无法使用电脑全部资源的原因？因为第一次是在 wsl 环境，第二次直接在 linux 系统）

# docker # c++
Logo
AtomGit开源社区

AtomGit 是由开放原子开源基金会联合 CSDN 等生态伙伴共同推出的新一代开源与人工智能协作平台。平台坚持“开放、中立、公益”的理念，把代码托管、模型共享、数据集托管、智能体开发体验和算力服务整合在一起，为开发者提供从开发、训练到部署的一站式体验。

更多推荐

cover

开源鸿蒙跨平台直播｜napigen 开源鸿蒙跨端实践：打通 Flutter 与 ArkTS

avatar AtomGit开源社区
cover

玩转 AtomCode！AtomGit「码动四季・开源同行」夏季征稿获奖名单出炉！

avatar AtomGit开源社区
cover

榜单出炉！AtomGit「源启盛夏」暑期开源成长计划正式收官

avatar AtomGit开源社区