far_planner学习笔记

目录

far_planner简介

far_planner编译安装

far_planner遥控小车

far_planner跑自己的rosbag

其他问题

---

far_planner简介

FAR Planner 使用动态更新的可见度图来快速重新规划。规划器用多边形建立环境模型，并在导航的同时建立全局可见度图。规划器能够处理已知和未知环境。在已知环境中，路径是根据先前的地图规划的。在未知环境中，则会根据导航过程中观察到的环境尝试多条路径，引导车辆到达目标。当存在动态障碍物时，FAR Planner 会断开被动态障碍物阻挡的可见性边缘，并在恢复可见性后重新连接。软件实现使用两个 CPU 线程，一个用于动态更新可见性图，使用约 20% 的线程，另一个用于路径搜索。

该版本库已在 Ubuntu 18.04 和 Ubuntu 20.04 中分别使用 ROS Melodic 和 ROS Noetic 进行了测试。

far_planner编译安装

楼主的系统是ubuntu20.04，以下以此系统作为研究基础。

首先应下载Autonomous Exploration Development Environment，far_planner基于该自主探索研发环境运行，该环境旨在利用系统开发和机器人部署，进行地面自主导航和探索。它包含各种模拟环境、自主导航模块（如避免碰撞、地形可穿越性分析、航点跟踪等）和一套可视化工具，用户可以开发自主导航系统，然后将这些系统移植到真实机器人上进行部署。

使用以下命令行安装依赖项

sudo apt update
sudo apt install libusb-dev

克隆开源资源库

git clone https://github.com/HongbiaoZ/autonomous_exploration_development_environment.git

在终端中进入文件夹，选中与电脑设置相匹配的分支。将 "distribution "替换为 "melodic "或 "noetic"。然后编译。

cd autonomous_exploration_development_environment
git checkout distribution
catkin_make

运行脚本下载模拟环境（约 500MB）。这可能需要几分钟时间。如果脚本没有启动下载，用户可以下载模拟环境并解压缩到 "src/vehicle_simulator/meshes"。

./src/vehicle_simulator/mesh/download_environments.sh

然后再下载far_planner。

git clone https://github.com/MichaelFYang/far_planner

在终端中，进入文件夹并编译。

cd far_planner
catkin_make

完成安装后，要运行代码，请开启两个终端，一个终端运行

source devel/setup.sh
roslaunch vehicle_simulator system_<environment>.launch

其中，<environment>可替换为自主研发环境中自带的仿真环境，如campus,forest,garage,indoor,tunnel。

如果要在真实机器人上运行，应启动以下代码，这将不会启动仿真环境。

roslaunch vehicle_simulator system_real_robot.launch

另一个终端运行

source devel/setup.sh
roslaunch far_planner far_planner.launch

现在，用户可以通过按下 RVIZ 中的 "目标点 "按钮发送目标，然后点击一个点来设置目标。车辆将导航至目标点，并沿途绘制能见度图（青色）。可见度图所覆盖的区域将成为自由空间。在自由空间中导航时，规划器会使用建立的可见度图；在未知空间中导航时，规划器会尝试探索通往目标的道路。按下 "重置可见度图 "按钮，规划器将重新初始化可见度图。取消选中 "可尝试规划 "复选框后，规划器将首先尝试在自由空间中寻找路径。路径将显示为绿色。如果不存在这样的路径，规划器将一并考虑未知空间。路径将显示为蓝色。取消选中 "更新可见度图 "复选框，规划器将停止更新可见度图。要从文件中读取/保存可见度图，请按 "读取"/"保存 "按钮。室内环境的能见度图文件示例位于 "src/far_planner/data/indoor.vgh"。

用户也可以运行 ROS 节点来发送一系列航点。在另一个终端中，进入文件夹并获取 ROS 工作区的源代码，然后使用下面的命令行运行 ROS 节点。ROS 节点还会发送导航边界和速度。车辆将按照航点在边界内导航。在autonomous_exploration_development_environment文件夹下运行：

roslaunch waypoint_example waypoint_example_garage.launch

far_planner遥控小车

在导航过程中，用户可以随时使用控制面板，通过点击黑框中的按钮来导航车辆。系统将切换到智能操纵杆模式--车辆会尝试执行虚拟操纵杆指令，同时避免碰撞。要恢复 FAR Planner 导航，请按 "恢复导航至目标 "按钮或使用 "目标点 "按钮设置新目标。请注意，用户可以使用 PS3/4 或 Xbox 控制器代替虚拟操纵杆。

考虑到大多数人没有游戏手柄，可以考虑通过使用键盘节点来连接/cmd_vel,从而实现键盘控制仿真环境中的小车运动。

首先下载键盘控制节点

sudo apt-get install ros-noetic-teleop-twist-keyboard

 但是far_planner的/cmd_vel的消息格式是geometry_msgs/TwistStamped，而teleop_twist_keyboard的消息类型是geometry_msgs/Twist，因此需要做一个消息类型的转换。感谢github上的热心网友添加的代码，我们可以打开/opt/ros/noetic/lib/teleop_twist_keyboard/teleop_twist_keyboard.py，将下面的代码直接覆盖在原文件上。

https://github.com/AndreasNagel/teleop_twist_keyboard/blob/08198a3b20e2245ee160ee274b6dad80b75e2af0/teleop_twist_keyboard.py

 因为opt文件夹下的文件可能是没有权限进行更改的，可以打开终端更改其权限

sudo chmod 777 文件名

之后，在py文件的第22行将Twist改为TwistStamped，第204行的False改为True.

以上更改之后，开启导航环境和far_planner后，开启键盘节点,

rosrun teleop_twist_keyboard teleop_twist_keyboard.py

通过q加速，i前进，j，l左右转，即可进行小车遥控。

far_planner跑自己的rosbag

跑rosbag属于在真实的机器人上使用，所以需要使用真实机器人命令打开

roslaunch vehicle_simulator system_real_robot.launch

在开启雷达话题并传输点云信息后，需要一个点云数据处理算法来衔接至far_planner.可以使用的几个算法如下：

1.LOAM：GitHub - cuitaixiang/LOAM_NOTED: loam code noted in Chinese（loam中文注解版）

注意：按原样使用‘src/loam_interface/launch/loam_interface.launch’包来桥接状态估计输出。

2.A-LOAM：GitHub - HKUST-Aerial-Robotics/A-LOAM: Advanced implementation of LOAM

注意：在“src/loam_interface/slaunch/loam_interface=launch”中，设置“stateEstimationTopic”=
/aft_mapped_to_init_high_frec”、“flipStateEstimation=false”和“flipRegisteredScan=false”。

3.LeGO-LOAM：GitHub - RobustFieldAutonomyLab/LeGO-LOAM: LeGO-LOAM: Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain

注意：在“src/loam_interface/slaunch/loam_interface=launch”中，设置“registerdScanTopic”=
/registered_cloud’。 

4.LIO-SAM：GitHub - TixiaoShan/LIO-SAM: LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping

注意：在“src/loam_interface/slaunch/loam_interface=launch”中，设置“stateEstimationTopic”=
/lio_sam/mapping/odometric'，'registeredScanTopic=/lio_sam/mapping/cloud_registered'，
“flipStateEstimation=false”和“flipRegisteredScan=false”。 

5.LIO-mapping：GitHub - hyye/lio-mapping: Implementation of Tightly Coupled 3D Lidar Inertial Odometry and Mapping (LIO-mapping)

注意：在“src/loam_interface/slaunch/loam_interface=launch”中，设置“stateEstimationTopic”=
/lio_map_builder/aft_mapped_to_init'，'registeredScanTopic=
/lio_map_builder/cloud_registered'、'flipStateEstimation=false'和'flipRegisteredScan=
false。 

6.FAST-LIO2：GitHub - hku-mars/FAST_LIO: A computationally efficient and robust LiDAR-inertial odometry (LIO) package

注意：在“src/loam_interface/slaunch/loam_interface=launch”中，设置“stateEstimationTopic”=
/Odometry”、“registerdScanTopic=/cloud_registered”、“flipStateEstimation=false”，以及
'flipRegisteredScan=false'。 

7.Faster-LIO：GitHub - gaoxiang12/faster-lio: Faster-LIO: Lightweight Tightly Coupled Lidar-inertial Odometry using Parallel Sparse Incremental Voxels

注意：在“src/loam_interface/slaunch/loam_interface=launch”中，设置“stateEstimationTopic”=
/Odometry”、“registerdScanTopic=/cloud_registered”、“flipStateEstimation=false”，以及
'flipRegisteredScan=false'。 

使用以上其中任何一个包时，都需要把‘use_sim_time’设置为‘true’才行，否则会出现

Invalid argument "/camera" passed to canTransform argument source_frame in tf2 frame_ids cannot start with a '/' like:

主要问题是时间戳对不上导致tf树对不上。

然后就可以用‘--clock’播放rosbag包了。

rosbag play --clock filename.bag

同时可以调整“src/local_planner/slaunch/local_planner.slaunch”中的“obstacleHeightThre”以定义
地形云厚度。

效果如下：

其他问题

1.开始跑far_planner时，/camera/image的相机图像总是打不开，urdf和xacro都没有错的情况下，我重新升级了一些gazebo的功能包，修复好了问题。

sudo apt-get install ros-noetic-rviz

sudo apt-get install ros-noetic-robot-state-publisher

sudo apt-get install ros-noetic-gazebo-ros-pkgs ros-noetic-gazebo-ros-control