02.9 충돌 회피(패키지 이용) - pineland/px4 GitHub Wiki

참고 : https://github.com/PX4/avoidance

1.gazebo 시뮬레이션

1.1 geographiclib_datasets 설치

$ sudo ./install_geographiclib_datasets.sh
    GeographicLib geoids dataset egm96-5 already exists, skipping
    Installing GeographicLib gravity egm96
    Installing GeographicLib magnetic emm2015

1.2 충돌 회피 모듈 의존성 관련 패키지(pointcloud library and octomap) 설치

$ sudo apt install libpcl1 ros-melodic-octomap-*

1.3 충돌 회피 ros 노드 생성을 위한 리포지토리 복제

$ cd ~/catkin_ws/src
$ git clone https://github.com/PX4/avoidance.git
    Cloning into 'avoidance'...
    remote: Enumerating objects: 130, done.
    remote: Counting objects: 100% (130/130), done.
    remote: Compressing objects: 100% (92/92), done.
    remote: Total 11297 (delta 61), reused 70 (delta 27), pack-reused 11167
    Receiving objects: 100% (11297/11297), 5.92 MiB | 1.70 MiB/s, done.
    Resolving deltas: 100% (7571/7571), done.

1.4 충돌 회피 노드 빌드

$ cd ~/catkin_ws $$ catkin_make

1.5 Catkin 환경 설정(옵션)

$ echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc
$ source ~/.bashrc

1.6 충돌회피 Gazebo Simulation 실행

아래는 Gazebo 시뮬레이션 설치 및 실행 과정을 안내합니다.

1.6.1 PX4 dependencies 설치

$ cd /src/Firmware

# Install PX4 "common" dependencies.
$ sudo ./Tools/setup/ubuntu.sh --no-sim-tools --no-nuttx      <-- 내부에 사전에 설치된 패키지들은 삭제될 수 있음.

# Gstreamer plugins (for Gazebo camera)
$ sudo apt install gstreamer1.0-plugins-bad gstreamer1.0-plugins-base gstreamer1.0-plugins-good gstreamer1.0-plugins-ugly libgstreamer-plugins-base1.0-dev

1.6.2 Gazebo 용 SDF 모델 파일을 생성하기위해 펌웨어를 한번 빌드한다.

이 단계는 실제로 시뮬레이션을 실행합니다.(즉시 닫을 수 있음)

# Qt 관련 오류를 방지하기 위해 필요 (feel free to try to omit it)
export QT_X11_NO_MITSHM=1

# 시뮬레이션 빌드 및 실행
make px4_sitl_default gazebo

# 시뮬레이션 종료 (Ctrl+C)

# Gazebo 관련 환경 변수를 추가 설정
$ . /src/Firmware/Tools/setup_gazebo.bash /src/Firmware /Firmware/build/px4_sitl_default

1.6.3 ROS가 PX4를 시작할 수 있도록 펌웨어 디렉토리를 ROS_PACKAGE_PATH에 추가

export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:/src/Firmware

1.6.4 ~/.bashrc에 GAZEBO_MODEL_PATH를 추가

echo "export GAZEBO_MODEL_PATH=${GAZEBO_MODEL_PATH}:~/catkin_ws/src/avoidance/avoidance/sim/models:~/catkin_ws/src/avoidance/avoidance/sim/worlds" >> ~/.bashrc

1.6.5 위 마지막 세단계(6.2, 6.3, 6.4)는 ~/catkin_ws/devel/setup.bash와 함께 새 터미널 창이 열릴 때마다 반복되어야 한다.

이제 로컬 또는 글로벌 플래너를 사용하여 시뮬레이션을 실행할 준비가 되었다.

1.7. local planner (기본, 많은 비행 테스트)

이 섹션에서는 local_planner를 시작하고 미션 또는 오프 보드 모드에서 충돌회피하기 위해 사용하는 방법을 보여준다.
플래너는 3DVFH+ 알고리즘을 기반으로 한다.

1.7.1 ros의 이미지 시현을 위한 추가 패키지를 설치한다.

$ sudo apt install ros-melodic-stereo-image-proc ros-melodic-image-view

1.7.2 local planner 실행

다음 세 가지 실행 파일 스크립트 중 하나를 사용하여 로컬 플래너를 실행할 수 있다.
※ 스크립트는 동일한 플래너를 실행하지만 다른 센서/카메라 설정을 시뮬레이션 한다. 이들은 모두 장애물 회피 및 충돌 방지를 가능하게 한다.

(방법1) local_planner_stereo OpenCV의 블록 일치 알고리즘 (기본적으로 SGBM)을 사용하여 depth 정보를 생성하는 스테레오 카메라로 vehicle을 시뮬레이션 한다.

$ cd ~/catkin_ws && roslaunch local_planner local_planner_stereo.launch

[참고] stereo-image-proc의 disparity map은 rviz 또는 rqt에서 지원되지 않는 stereo_msgs/DisparityImage 메시지로 publish된다. 메시지를 시각화하려면 먼저 새 터미널을 열고 필요한 환경 변수를 설정한다.

$ source ~/catkin_ws/devel/setup.bash

그런 다음, 다음 중 하나를 수행한다.

i. 실행:

$ rosrun image_view stereo_view stereo:=/stereo image:=image_rect_color

ii. 간단한 sensor_msgs/Image로 DisparityImage를 publish:

$ rosrun topic_tools transform /stereo/disparity /stereo/disparity_image sensor_msgs/Image 'm.image'

그런 다음, 토픽 /stereo/disparity_image에 대해 rviz 또는 rqt로 시차 맵(disparity map)을 시각화 할 수 있다.

(방법2) ocal_planner_depth_camera : 전방을 향한 하나의 kinect 센서로 vehicle 시뮬레이션 ★★★

$ roslaunch local_planner local_planner_depth-camera.launch

(방법3) local_planner_sitl_3cam : 3 개의 kinect 센서 (왼쪽, 오른쪽, 앞)로 vehicle 시뮬레이션 ★★★

$ roslaunch local_planner local_planner_sitl_3cam.launch

Gazebo world에서 iris 드론이 unarmed 상태인 것을 볼 수 있다. 비행을 시작하려면 off-board 또는 mission 모드의 두 가지 옵션이 있다.

◎ OFFBOARD의 경우, 다음을 실행:

# In another terminal 
$ rosrun mavros mavsys mode -c OFFBOARD
$ rosrun mavros mavsafety arm

드론은 먼저 목표 높이에 도달하기 위해 고도를 변경한다.rqt_reconfigure GUI를 사용하여 목표 고도를 수정할 수 있다.

$ sudo apt install ros-melodic-rqt-reconfigure
$ rosrun rqt_reconfigure rqt_reconfigure

그러면 드론이 목표를 향해 움직이기 시작한다. Rviz에서 2D Nav Goal 버튼을 클릭 한 다음 시각화 된 회색 공간을 클릭하여 새 목표 x 및 y 위치를 선택하여 기본 x, y 목표 위치를 변경할 수 있다. 목표가 올바르게 설정되면 회색 세계에서 클릭 한 곳에 노란색 구가 나타난다.

◎ 미션의 경우, QGroundControl을 열고 여기에 설명 된대로 미션을 계획한다.

COM_OBS_AVOID 매개 변수를 true로 설정한다. 미션을 시작하면 vehicle은 비행 경로를 동적으로 재계산하여 충돌이 발생하지 않도록 하면서 mission waypoints를 날아다닌다.

1.8 global planner (고급, 비행 테스트되지 않음)

이 섹션에서는 global_planner를 시작하고 OFFBOARD 모드에서 충돌회피하는 방법을 설명한다.

$ roslaunch global_planner global_planner_stereo.launch

이제 아래의 그림처럼 숲 환경에서 무인 항공기가 지상에서 비무장 상태로 표시된다.
비행을 시작하려면 드론을 OFFBOARD 모드에 놓고 무장한다. 그런 다음 충돌회피노드가 그것을 제어한다.

# In another terminal
$ rosrun mavros mavsys mode -c OFFBOARD
$ rosrun mavros mavsafety arm

처음에는 드론이 3.5m 고도로 호버링한다. 원하는 경우 명령 행에서 gazebo가 드론을 따르도록 할 수도 있다.

$ gz camera --camera-name=gzclient_camera --follow=iris

rviz의 2D Nav Goal 버튼으로 새로운 목표를 설정하여 새로운 경로를 계획 할 수 있다. 계획된 경로는 rviz에 나타나며 드론은 경로를 따라 가고 장애물이 감지되면 경로를 업데이트한다.장애물 회피를 사용하지 않고 목표를 설정할 수도 있다.(예: 드론이 목표에 직진하여 장애물과 충돌 할 수 있음). 이렇게 하려면 rviz의 2D 자세 추정 버튼(2D Pose Estimate button)으로 위치를 설정한다.

1.9 안전 착륙 플래너

이 섹션에서는 safe_landing_planner를 시작하고 이를 사용하여 임무 또는 자동 착륙 모드에서 안전하게 착륙하는 방법을 설명한다. 노드를 실행하려면 다음을 수행한다.

$ roslaunch safe_landing_planner safe_landing_planner.launch

지상에 unarmed vehicle을 볼 수 있다. QGroundControl을 열고 Land 유형의 마지막 항목으로 임무를 계획하거나 위치 제어에서 world를 비행하고 착륙하려는 좌측의 Land 버튼을 클릭한다. land 위치에서, vehicle은 ground/obstacle에서 loiter_height에 도달 할 때까지 지면을 향해 내려 가기 시작한다. 그런 다음 땅 밑바닥을 평가하기 위해 배회가 시작된다. 지면이 평평하면 vehicle은 계속 착륙합니다. 그렇지 않으면 착륙하기에 충분한 지면을 찾을 때까지 squared spiral pattern으로 지형에 의한 근접도를 평가한다.

2. 하드웨어에서의 실행

2.1 전제 조건

2.1.1 카메라

두 플래너 모두 sensor_msgs::PointCloud2 유형의 3D 포인트 클라우드가 필요하다. 이러한 데이터를 제공할 수 있는 모든 카메라는 호환된다. 공식적으로 지원되는 카메라는 Intel Realsense D435 이다. 펌웨어 버전 5.9.13.0 을 사용하는 것이 좋다.