+STARTUP: showeverything

+AUTHOR: Donghee Park

Creative Commons, Share-Alike (cc)

+EMAIL: dongheepark@gmail.com

+TITLE: PX4 and ROS Programming Day 1

+HTML_HEAD_EXTRA:

+HTML_HEAD:

수업
- 목표: 드론 제어 소프트웨어에 대해 이해할 수 있고, 프로그래밍 환경을 구축할 수 있다.
- 교재: https://learn.dronemap.io/
- 코치: 박동희 dongheepark@gmail.com
드론 제어 소프트웨어 소개 및 설치

워크숍 소개, 참가자 소개
PX4 소개, ROS 소개
Linux 사용하기

PX4 소개
- https://px4.io/
  - Pixhawk
- PX4 Firmware
- Mavlink
- QGroundControl
- https://learn.dronemap.io/px4-workbook/px4-intro.html
  - 소프트웨어 아키텍처
- 시뮬레이터 포트 구성

** PX4 미들웨어 및 앱 구조 소개

디렉토리 구조 소개
uORB
PX4 shell 사용하기
hello-skyworld

Linux 사용하기
- Ubuntu 18.04 설치
- 주요 명령어 소개(파일 조작, 프로그램설치, 쉘스크립트, git)

ls: 파일 또는 디렉토리의 목록을 출력

+BEGIN_SRC

ls ls -al

+END_SRC

cd: 디렉토리 이동

+BEGIN_SRC

cd ~ cd ~/Downloads

+END_SRC

pwd: 현재 디렉토리 출력

+BEGIN_SRC

pwd

+END_SRC

mkdir: 디렉토리 생성

+BEGIN_SRC

mkdir tmp

+END_SRC

rm: 파일 또는 디렉토리 지우기

+BEGIN_SRC

rm -rf tmp

+END_SRC

cat: 파일 입력 또는 출력

+BEGIN_SRC

cat ~/.bashrc cat > ~/.hello.c

+END_SRC

cp: 파일 또는 디렉토리 복사

+BEGIN_SRC

cp hello.c world.c

+END_SRC

chmod: 파일의 퍼미션 지정

+BEGIN_SRC

chmod +x hello

+END_SRC

wget: url에서 파일 다운로드

+BEGIN_SRC

wget https://google.com

+END_SRC

source: 현재 쉘에서 파일을 읽고 실행

+BEGIN_SRC

source ~/.bashrc

+END_SRC

PX4 개발환경 구성 (+ROS)

https://dev.px4.io/master/en/setup/dev_env_linux_ubuntu.html

+BEGIN_SRC

cd ~ wget https://raw.githubusercontent.com/PX4/Devguide/master/build_scripts/ubuntu_sim_ros_melodic.sh chmod +x ubuntu_sim_ros_melodic.sh ./ubuntu_sim_ros_melodic.sh

+END_SRC

PX4 Firmware Build
- https://dev.px4.io
- Gazebo 사용하기
- Qgroundcontrol

** PX4 v.10.1 다운로드

+BEGIN_SRC

cd ~ git clone https://github.com/PX4/Firmware.git --recursive cd ~/Firmware bash ./Tools/setup/ubuntu.sh git checkout v1.10.1 git submodule update --init --recursive

+END_SRC

** Gazebo 실행

+BEGIN_SRC

cd ~/Firmware make px4_sitl gazebo

+END_SRC

** QGroundControl 사용

다운로드: https://docs.qgroundcontrol.com/en/getting_started/download_and_install.html

QGroundControl 다운로드 및 실행

+BEGIN_SRC

sudo usermod -a -G dialout $USER sudo apt-get remove modemmanager -y sudo apt install gstreamer1.0-plugins-bad gstreamer1.0-libav gstreamer1.0-gl -y

cd ~/Downloads chmod +x ./QGroundControl.AppImage ./QGroundControl.AppImage

+END_SRC

ROS 프로그래밍

** ROS

Robot Operating System: 로봇 빌드에 사용되는 라이브러리 어플리케이션 모음 http://www.ros.org/
목표: 로봇을 만들때 기존의 재활용 하고 공유하자.
History:
- 2000s: Standford Artificial intelligence
- 2007: Willow Garage
- 2013: Open Source Robotics Foundation
사용 분야: Drone, Kinematic ARMS(로봇암), Wheeled(바퀴), Bi-pedal(이족)

** ROS Nodes and Topics

https://misohena.jp/article/emacs_org_textfigures/dot.html

+begin_src dot :file ros_nodes_and_topics1.png :cmdline -Kdot -Tpng

digraph { rankdir=LR graph [fontname="MS Gothic"]; node [shape=rect, color="#40e0d0"] edge [fontname="MS Gothic"]; label = "Robot Communication Sequence"; "Perception" -> "Dicesion Making"; "Dicesion Making" -> "Actuation"; }

+end_src

**** Perception: Sense - Sensor Fusion - Filtering - Localization

**** Dicesion Making: Decide - Path Planning - Prediction - Behavior Planning

**** Actuation: Act - PID Control - Model Predictive Control

*** ROS Master Process

노드 관리

+begin_src dot :file ros_master_process1.png :cmdline -Kdot -Tpng

digraph { graph [fontname="MS Gothic"]; node [shape=box, color="#40e0d0"] edge [fontname="MS Gothic"]; label = "ROS Master Process";

subgraph cluster_perception { node [shape=rect, style="rounded"] label = "Perception"; Camera; "Wheel Encoder"; "Positon Estimator"; }

subgraph cluster_dicesion_making { node [shape=rect, style="rounded"] label = "Dicesion Making"; "Behavior Execution"; }

subgraph cluster_actuation { node [shape=rect, style="rounded"] label = "Actuation"; "Motor Control"; } }

+end_src

*** Topics

노드간 통신 인터페이스. 구독 발행의 이름

+begin_src dot :file ros_topic1.png :cmdline -Kdot -Tpng

digraph { rankdir=LR node [color="#40e0d0"] edge [fontname="MS Gothic"]; node1 [label= ""] node2 [label= ""]

node1 -> node2 [label="/topic_name"]; }

+end_src

*** Publish and Subscribe

발행과 구독. 신문/잡지 발행 구독에 비유

+begin_src dot :file ros_publish_and_subscribe1.png :cmdline -Kdot -Tpng

digraph { rankdir=LR node [color="#40e0d0"] edge [fontname="MS Gothic"]; label = " PUBLISH SUBSCRIBER"; node1 [label= ""] node2 [label= ""]

node1 -> node2 node2 -> node1 }

+end_src

실제 예제

+begin_src dot :file ros_publish_and_subscribe2.png :cmdline -Kdot -Tpng

digraph { node [color="#40e0d0"] edge [fontname="MS Gothic"];

"Wheel Encoder" -> "Positon Estimator" [label="/wheel_encoder\lrotation"] "Behavior Executor" -> "Motor Controller" [label="/motor_controller\lvelocity_cmd"] "Camera" -> "Behavior Executor" [label="/camera_images\limage"] "Positon Estimator" -> "Behavior Executor" [label="/position_estimate\lpose"] }

+end_src

** ROS Message Passing

메시지: 노드간 통신할때 이동하는 실제 데이터

메시지는 텍스트로 구성. 메시지를 이해하기 쉽다.

미리 정의된 메시지 타입 :

** ROS Services

Request-Response, 1:1 통신
PubSub이 필요 없는 경우 사용, 요청 할때만 데이터가 제공. 네트워크 부하가 적다.

*** 예시: 카메라 이미지 얻기

+begin_src dot :file ros_services1.png :cmdline -Kdot -Tpng

digraph { rankdir=LR; node [color="#40e0d0"]; edge [fontname="MS Gothic"]; label = "Publicate and Subscribe";

"Camera" -> "Behavior Executor" [label="/camera_images\limage"] }

+end_src

+begin_src dot :file ros_services2.png :cmdline -Kdot -Tpng

digraph { rankdir=LR; node [color="#40e0d0"]; edge [fontname="MS Gothic", style=dotted]; label = "Request-Response";

"Behavior Executor" -> "Camera" [label="/capture_image\lrequest: exposure time"] "Camera" -> "Behavior Executor" [label="\nresponse: image"] }

+end_src

** ROS Turtlesim

Turtle

+ATTR_HTML: width="200px"

*** Turtlesim 실행하기

환경 변수 설정

+begin_src sh

source /opt/ros/melodic/setup.bash

+end_src

roscore 실행
- roscore: Master + rosout + parameter server
- Master: 네임 서비스
- rosout: stdout/stderr 로깅
- parameter server: 파라미터 저장 서버

+begin_src sh

roscore

+end_src

turtlesim 패키지의 ~turtlesim_node~ 실행

+begin_src sh

rosrun turtlesim turtlesim_node

+end_src
turtlesim 패키지의 ~turtle_teleop_key~ 실행

+begin_src sh

rosrun turtlesim turtle_teleop_key

+end_src

*** Turtlesim 노드 목록

+begin_src sh

rosnode list

+end_src

/rosout : ROS 메시지 로깅.

*** Turtlesim 토픽 목록

+begin_src sh

rostopic list

+end_src

*** Turtlesim 토픽 정보

+begin_src sh

rostopic info /turtle1/cmd_vel

+end_src

*** Turtlesim 메시지 정보

+begin_src sh

$ rosmsg info geometry_msgs/Twist geometry_msgs/Vector3 linear float64 x float64 y float64 z geometry_msgs/Vector3 angular float64 x float64 y float64 z

+end_src

+begin_src sh

rosed geometry_msgs Twist.msg

+end_src

*** Turtlesim Echo a Topic

디버깅시 편리

+begin_src sh

rostopic echo /turtle1/cmd_vel

+end_src

*** ~rqt_graph~

+begin_src sh

rqt_graph

+end_src

** MavROS

http://wiki.ros.org/mavros mavlink ros wrapper

*** MavROS 설치 및 실행 (이미 자동설치됨, 안해도됨)

+begin_src sh

Create catkin workspace (ROS build system)

mkdir -p ~/catkin_ws/src cd ~/catkin_ws

Install dependencies

sudo apt-get install python-wstool python-rosinstall-generator python-catkin-tools -y

Initialise wstool

wstool init ~/catkin_ws/src

Build MAVROS

Get source (upstream - released)

rosinstall_generator --upstream mavros --rosdistro kinetic | tee /tmp/mavros.rosinstall

Get latest released mavlink package

rosinstall_generator mavlink --rosdistro kinetic | tee -a /tmp/mavros.rosinstall

Setup workspace & install deps

wstool merge -t src /tmp/mavros.rosinstall wstool update -t src rosdep install --from-paths src --ignore-src --rosdistro kinetic -y

Install GeographicLib datasets

./src/mavros/mavros/scripts/install_geographiclib_datasets.sh

Build source

catkin build

source setup.bash

source devel/setup.bash

+end_src

환경변수 설정: workspace

+begin_src

cd ~/catkin_ws source devel/setup.bash

+end_src

~mavros_node~ 실행

+begin_src sh

rosrun mavros mavros_node _fcu_url:="udp://:14540@127.0.0.1:14557" _gcs_url:="udp://@127.0.0.1"

+end_src

~mavros_node~ 실행 (roslaunch 사용하는 방법)

+begin_src sh

px4.launch 이용하여 mavros node 실행. fcu ip주소는 127.0.0.1 ip주소를 바꾸면 다른 컴퓨터의 fcu도 연결 가능.

cd ~/catkin_ws roslaunch mavros px4.launch fcu_url:="udp://:14540@127.0.0.1:14557" _gcs_url:"udp://@127.0.0.1"

+end_src

** Gazebo 실행 시뮬레이터의 홈 위치(위도 경도 해발고도) 지정

+begin_src sh

export PX4_HOME_LAT=35.9012382 export PX4_HOME_LON=128.854495337 export PX4_HOME_ALT=71

cd ~/Firmware make px4_sitl gazebo

+end_src

ROS 노드 관리

** ROS 노드 실행 및 관리

**** ROS Core 노드 실행

+begin_src sh

roscore

+end_src

**** MAVROS 노드 실행

+begin_src sh

rosrun mavros mavros_node _fcu_url:="udp://:14540@127.0.0.1:14557" _gcs_url:="udp://@127.0.0.1"

+end_src

J120 UART2에 PX4 telemetry 2 연결

roslaunch mavros px4.launch fcu_url:="/dev/ttyTHS1:921600" gcs_url:="udp://@192.168.88.53"

**** 토픽 목록

+begin_src sh

rostopic list

+end_src

**** 토픽 내용 보기

***** 메시지 타입 보기

+begin_src sh

rostopic info /mavros/state

+end_src

타입 내부 보기

+begin_src sh

rostopic type /mavros/state | rosmsg show

+end_src

***** 메시지 내용

+begin_src sh

rostopic echo /mavros/state

+end_src

***** 토픽 publish 주기 보기

+begin_src sh

rostopic hz /mavros/state

+end_src

***** 실행 노드 확인

+begin_src sh

rqt_graph

+end_src

** ROS 노드 명령 실행하기. (MAVROS 위주)

http://wiki.ros.org/ROS/Tutorials/UnderstandingTopics

*** Subscribe

+begin_src sh

rostopic echo [topic]

+end_src

**** ~STATE~

+begin_src sh

rostopic echo /mavros/state

+end_src

**** ~LOCAL_POSITION~ 확인

+begin_src sh

rostopic echo /mavros/local_position/pose

+end_src

*** Services

서비스 목록

+begin_src sh

rosservice list

+end_src

서비스 실행(call) 하기

+begin_src sh

rosservice call [topic] [msg_type] [args]

+end_src

**** ~SET_MODE~

+begin_src sh

https://github.com/mavlink/mavros/blob/master/mavros_msgs/srv/SetMode.srv

http://wiki.ros.org/mavros/CustomModes

Manual Mode

rosservice call /mavros/set_mode "base_mode: 64 custom_mode: ''"

rosservice call /mavros/set_mode "base_mode: 0 custom_mode: 'MANUAL'"

rosservice call /mavros/set_mode "base_mode: 0 custom_mode: 'POSCTL'"

rosservice call /mavros/set_mode "base_mode: 0 custom_mode: 'OFFBOARD'"

rosservice call /mavros/set_mode "base_mode: 0 custom_mode: 'AUTO.LAND'"

+end_src

**** ~ARMING~

+begin_src sh

rosservice call /mavros/cmd/arming "value: true"

+end_src

**** ~TAKEOFF~

+begin_src sh

rosservice call /mavros/cmd/takeoff "{min_pitch: 0.0, yaw: 0.0, latitude: 47.3977508, longitude: 8.5456074, altitude: 2.5}"

+end_src

**** ~Land~

+begin_src sh

rosservice call /mavros/cmd/land "{min_pitch: 0.0, yaw: 0.0, latitude: 47.3977508, longitude: 8.5456074, altitude: 0.0}"

+end_src

*** Publish

+begin_src sh

rostopic pub [topic] [msg_type] [args]

+end_src

**** ~SETPOINT_POSITION~

OFFBOARD 모드에서 동작

+begin_src sh

rostopic pub -r 10 /mavros/setpoint_position/local geometry_msgs/PoseStamped "header: auto pose: position: x: 5.0 y: 0.0 z: 1.0 orientation: x: 0.0 y: 0.0 z: 0.0 w: 0.0"

+end_src

**** ~SETPOINT_VELOCITY~

OFFBOARD 모드에서 동작

+begin_src sh

rostopic pub -r 10 /mavros/setpoint_velocity/cmd_vel geometry_msgs/TwistStamped "{header: auto, twist: {linear: {x: 10.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}}"

+end_src

*** 실습

준비: 순서대로 실행

Gazebo 실행: ~cd ~/Firmware; make px4_sitl gazebo~
PC의 MAVROS를 PC(127.0.0.1)의 Gazebo와 연결 ~rosrun mavros mavros_node _fcu_url:="udp://:14540@127.0.0.1:14557" _gcs_url:="udp://@127.0.0.1"~ \ 또는 Raspberry PI의 MAVROS를 PC(192.168.88.53)의 Gazebo와 연결. Raspberry PI에서 다음 명령을 실행 ~roslaunch mavros px4.launch fcu_url:="udp://:14540@192.168.88.53:14557" gcs_url:="udp://@192.168.88.53"~
QGroundControl 실행: PX4 Parameter ~COM_OF_LOSS_T~ 파라미터 15초로 바꾸기. Failsafe timeout을 15초로 바꾸어야 커맨드라인에서 드론을 조정하기 편하다.

해보기: 커맨드 라인에서 다음 명령을 수행하여, QGroundControl에 아래와 같은 자취를 남겨보자.

1. ARM
2. TAKEOFF 하고. (옵션)
3. 현재 위치를 지정한다. ~/mavros/setpoint_position~ 이용하여, (0,0,0) 위치를 10hz로 지정
3-1. MODE 변환. OFFBOARD
4. 20m 간격으로 정사각형을 따라 움직인다. ~/mavros/setpoint_position~ 이용
5. HOME 자리에 오면 LAND 한다.
6. DISARM

더해보기: 드론의 머리방향이 진행 방향을 향하도록 하자.

Body 3-2-1 순서 오일러->쿼터니언 변환

+HTML_MATHJAX: align: left indent: 5em tagside: left font: Neo-Euler

\begin{align} \begin{bmatrix} x \ y \ z \ w \ \end{bmatrix} & = \begin{bmatrix} \cos (\phi /2) \cos (\theta /2) \cos (\psi /2) + \sin (\phi /2) \sin (\theta /2) \sin (\psi /2) \ \sin (\phi /2) \cos (\theta /2) \cos (\psi /2) - \cos (\phi /2) \sin (\theta /2) \sin (\psi /2) \ \cos (\phi /2) \sin (\theta /2) \cos (\psi /2) + \sin (\phi /2) \cos (\theta /2) \sin (\psi /2) \ \cos (\phi /2) \cos (\theta /2) \sin (\psi /2) - \sin (\phi /2) \sin (\theta /2) \cos (\psi /2) \ \end{bmatrix} \ \end{align}

변환 코드(python): https://gist.github.com/donghee/e3b4fa8ec789cec0e287bf3b91ddb79e

https://www.andre-gaschler.com/rotationconverter/

*** 유용한 mavros 명령(노드) 모음

mavros 패키지의 mavsafety 노드: arm, disarm, safetyarea

+begin_src

rosrun mavros mavsafety arm

+end_src

mavcmd 노드:

예시: takeoff from current position (10도 각도 피치, 90도 방향 보고, 5m 위로 takeoff)

+begin_src

rosrun mavros mavcmd takeoffcur 10 90 5

+end_src

예시: home 지정(RTL 위치, 위도 35.9012382 경도 128.85449537 해발고도 71m) google earth: https://earth.google.com/web/search/35.9012382+128.85449537

+begin_src

rosrun mavros mavcmd sethome 35.9012382 128.854495337 71

+end_src

mavsetp 노드: setpoint 한번 보내기 (setpoint 테스트용, position, velocity, acceleration 가능)

예시: x=1m, y=1m, z=1m, yaw=90도 setpoint 보내기

+begin_src

rosrun mavros mavsetp local -p 1 1 2 90

+end_src

mavsys 노드: change mode

+begin_src

rosrun mavros mavsys mode -c OFFBOARD

+end_src

mavparam 노드: parameter set, get, load, dump

예시: 파라미터 덤프

+begin_src

rosrun mavros mavparam dump /tmp/params

+end_src

mavftp 노드: px4의 파일 시스템 접근

예시: 로그 다운로드

+begin_src

rosrun mavros mavftp download log/2020-08-03/14_37_15.ul

+end_src

** 토픽 레코드: rosbag 리뷰할때 유용

토픽 저장하기

+begin_src

rostopic list -v mkdir ~/bagfiles cd ~/bagfiles rosbag record -O iris_default_1 /mavros/local_position/pose rosbag info iris_default_1.bag rqt_bag

+end_src

** 참고

https://github.com/mavlink/mavros/tree/master/mavros

ROS 노드 만들기

** 새로운 노드 만들기

*** 패키지 만들기

+begin_src sh

source ~/catkin_ws/devel/setup.bash cd ~/catkin_ws/src catkin_create_pkg drone_control mavros sensor_msgs roscpp rospy

+end_src

*** 노드 코드 작성

~~/catkin_ws/src/drone_control/src/drone_state.cpp~

+begin_src c++

include "ros/ros.h"

include "sensor_msgs/Imu.h"

void imuDataCallback(const sensor_msgs::Imu::ConstPtr& msg){ ROS_INFO("\nlinear acceleration\ \nx: [%f]\ny:[%f]\nz:[%f]", msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z); }

int main(int argc, char **argv){ ros::init(argc, argv, "drone_state"); ros::NodeHandle nh; ros::Subscriber sub = nh.subscribe("/mavros/imu/data", 1000, imuDataCallback); ros::spin(); return 0; }

+end_src

빌드 스크립트 추가

~~/catkin_ws/src/drone_control/CMakeLists.txt~ 파일 끝에 다음 3줄 추가

+begin_src cmake

include_directories(include ${catkin_INCLUDE_DIRS}) add_executable(drone_state src/drone_state.cpp) target_link_libraries(drone_state ${catkin_LIBRARIES})

+end_src

환경 변수 다시 로드!

+begin_src sh

source ~/catkin_ws/devel/setup.bash

+end_src

*** 패키지 빌드

+begin_src sh

cd ~/catkin_ws catkin build

+end_src

*** 패키지 노드 실행

~drone_control~ 패키지의 ~drone_state~ 노드 실행

+begin_src sh

rosrun drone_control drone_state

+end_src

*** 해보기: /mavros/state 읽어서 1초마다 비행 mode 한번씩 출력

~/mavros/state~ 타입 체크하여 헤더 include

+BEGIN_SRC c++

include "ros/ros.h"

include "mavros_msgs/State.h"

void droneStateCallback(const mavros_msgs::State::ConstPtr& msg){ ROS_INFO("\nDrone mode: %s", msg->mode.c_str()); }

int main(int argc, char **argv){ ros::init(argc, argv, "drone_state"); ros::NodeHandle nh; ros::Subscriber sub = nh.subscribe("/mavros/state", 1000, droneStateCallback); ros::spin(); return 0; }

+END_SRC

*** 해보기 결과:

+begin_src c++

$ rosrun drone_control drone_state [ INFO] [1539297808.077868114]: Drone mode: OFFBOARD [ INFO] [1539297808.525173697]: Drone mode: OFFBOARD [ INFO] [1539297809.565387356]: Drone mode: OFFBOARD

+end_src

~rqt_graph~

** 새로운 노드 만들기: 드론 이륙 착륙

+begin_src dot :file mavros_takeoff_and_land1.png :cmdline -Kdot -Tpng

digraph { rankdir=LR graph [fontname="NanumSquare"]; node [shape=rect, color="#40e0d0"] edge [style=dashed, fontname="NanumSquare"]; node1 [label= "/takeoff_and_land"] node2 [label= "/mavros"]

node1 -> node2 [label="1. /mavros/cmd/arming"]; node1 -> node2 [label="2. /mavros/cmd/takeoff"]; node1 -> node2 [label="3. /mavros/cmd/land"]; }

+end_src

~drone_control~ 패키지에 ~takeoff_and_land~ 노드를 만들어 보자.

2.5m 이륙후 10초 있다가 착륙

~~/catkin_ws/src/drone_control/src/takeoff_and_land.cpp~

+begin_src c++

include

int main(int argc, char **argv) {

int rate = 20;

ros::init(argc, argv, "takeoff_and_land");
ros::NodeHandle n;

ros::Rate r(rate);

///////////////////ARM//////////////////////
ros::ServiceClient arming_client = n.serviceClient<mavros_msgs::CommandBool>("/mavros/cmd/arming");
mavros_msgs::CommandBool arm_cmd;
arm_cmd.request.value = true;

if (arming_client.call(arm_cmd) && arm_cmd.response.success)
{
    ROS_INFO("Vehicle armed");
} else {
    ROS_ERROR("Failed arming or disarming");
}

/////////////////TAKEOFF////////////////////
ros::ServiceClient takeoff_client = n.serviceClient<mavros_msgs::CommandTOL>("/mavros/cmd/takeoff");
mavros_msgs::CommandTOL takeoff_cmd;
takeoff_cmd.request.altitude = 10;
takeoff_cmd.request.latitude = 47.3977508;
takeoff_cmd.request.longitude = 8.5456074;
takeoff_cmd.request.min_pitch = 0;
takeoff_cmd.request.yaw = 0;
if(takeoff_client.call(takeoff_cmd) && takeoff_cmd.response.success){
    ROS_INFO("Okay Takeoff");
}else{
    ROS_ERROR("Failed Takeoff");
}

/////////////////DO STUFF///////////////////
sleep(10);


///////////////////LAND/////////////////////
ros::ServiceClient land_client = n.serviceClient<mavros_msgs::CommandTOL>("/mavros/cmd/land");
mavros_msgs::CommandTOL land_cmd;
land_cmd.request.altitude = 0;
land_cmd.request.latitude = 47.3977508;
land_cmd.request.longitude = 8.5456074;
land_cmd.request.min_pitch = 0;
land_cmd.request.yaw = 0;
if(land_client.call(land_cmd) && land_cmd.response.success){
    ROS_INFO("Okay Land");
}else{
    ROS_ERROR("Failed Land");
}

while (n.ok())
{
  ros::spinOnce();
  r.sleep();
}

return 0;

}

+end_src

~~/catkin_ws/src/drone_control/CMakeLists.txt~ 파일 끝에 다음 2줄 추가

+begin_src cmake

add_executable(takeoff_and_land src/takeoff_and_land.cpp) target_link_libraries(takeoff_and_land ${catkin_LIBRARIES})

+end_src

컴파일: ~cd ~/catkin_ws; source devel/setup.bash; catkin build~
실행: ~rosrun drone_control takeoff_and_land~

** 새로운 노드 만들기: ~offb_node~

~drone_control~ 패키지에 ~offb_node~ 노드를 만들어 보자.

2m 이륙.

+begin_src c++

/**

@file offb_node.cpp
@brief Offboard control example node, written with MAVROS version 0.19.x, PX4 Pro Flight
Stack and tested in Gazebo SITL */

include

mavros_msgs::State current_state; void state_cb(const mavros_msgs::State::ConstPtr& msg){ current_state = *msg; }

int main(int argc, char **argv) { ros::init(argc, argv, "offb_node"); ros::NodeHandle nh;

ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
        ("mavros/state", 10, state_cb);
ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
        ("mavros/setpoint_position/local", 10);
ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
        ("mavros/cmd/arming");
ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
        ("mavros/set_mode");

//the setpoint publishing rate MUST be faster than 2Hz
ros::Rate rate(20.0);

// wait for FCU connection
while(ros::ok() && !current_state.connected){
    ros::spinOnce();
    rate.sleep();
}

geometry_msgs::PoseStamped pose;
pose.pose.position.x = 0;
pose.pose.position.y = 0;
pose.pose.position.z = 2;

//send a few setpoints before starting
for(int i = 100; ros::ok() && i > 0; --i){
    local_pos_pub.publish(pose);
    ros::spinOnce();
    rate.sleep();
}

mavros_msgs::SetMode offb_set_mode;
offb_set_mode.request.custom_mode = "OFFBOARD";

mavros_msgs::CommandBool arm_cmd;
arm_cmd.request.value = true;

ros::Time last_request = ros::Time::now();

while(ros::ok()){
    if( current_state.mode != "OFFBOARD" &&
        (ros::Time::now() - last_request > ros::Duration(5.0))){
        if( set_mode_client.call(offb_set_mode) &&
            offb_set_mode.response.mode_sent){
            ROS_INFO("Offboard enabled");
        }
        last_request = ros::Time::now();
    } else {
        if( !current_state.armed &&
            (ros::Time::now() - last_request > ros::Duration(5.0))){
            if( arming_client.call(arm_cmd) &&
                arm_cmd.response.success){
                ROS_INFO("Vehicle armed");
            }
            last_request = ros::Time::now();
        }
    }

    local_pos_pub.publish(pose);

    ros::spinOnce();
    rate.sleep();
}

return 0;

}

+end_src

실행: ~rosrun drone_control offb_node~

*** launch 파일을 이용하여 노드 한번에 실행 (옵션) roscore, gazebo, mavros, offb_node 노드를 한번에 실행하기 위해서 roslaunch를 이용해보자.

~~/catkin_ws/src/drone_control/launch/offb_node.launch~ 파일에 다음 내용 추가

+begin_src

+end_src

~~/.bashrc~ 파일의 끝에 다음 내용을 추가 환경 변수 추가

+begin_src

PX4_SRC_DIR=$HOME/Firmware source $PX4_SRC_DIR/Tools/setup_gazebo.bash $PX4_SRC_DIR $PX4_SRC_DIR/build/px4_sitl_default > /dev/null export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$PX4_SRC_DIR:$PX4_SRC_DIR/Tools/sitl_gazebo

+end_src

+begin_src sh

source ~/.bashrc catkin build

+end_src

+begin_src sh

roslaunch drone_control offb_node.launch

+end_src

** 새로운 노드 만들기: ~circle~

출처: https://github.com/Jaeyoung-Lim/modudculab_ros/blob/master/src/pub_setpoints_traj.cpp

+begin_src

/**

@file circle.cpp
@brief offboard example node, written with mavros version 0.14.2, px4 flight
stack and tested in Gazebo SITL */

include

include "math.h"

double r; double theta; double count=0.0; double wn;

mavros_msgs::State current_state; void state_cb(const mavros_msgs::State::ConstPtr& msg){ current_state = *msg; }

int main(int argc, char **argv) { ros::init(argc, argv, "circle"); ros::NodeHandle nh;

ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
        ("mavros/state", 10, state_cb);
ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
        ("mavros/setpoint_position/local", 10);
ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
        ("mavros/cmd/arming");
ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
        ("mavros/set_mode");

//the setpoint publishing rate MUST be faster than 2Hz
ros::Rate rate(20.0);


nh.param("pub_setpoints_traj/wn", wn, 1.0);
nh.param("pub_setpoints_traj/r", r, 1.0);
// wait for FCU connection
while(ros::ok() && current_state.connected){
    ros::spinOnce();
    rate.sleep();
}

geometry_msgs::PoseStamped pose;
pose.pose.position.x = 0;
pose.pose.position.y = 0;
pose.pose.position.z = 2;

//send a few setpoints before starting
for(int i = 100; ros::ok() && i > 0; --i){
    local_pos_pub.publish(pose);
    ros::spinOnce();
    rate.sleep();
}

mavros_msgs::SetMode offb_set_mode;
offb_set_mode.request.custom_mode = "OFFBOARD";

mavros_msgs::CommandBool arm_cmd;
arm_cmd.request.value = true;

j ros::Time last_request = ros::Time::now();

while(ros::ok()){
    if( current_state.mode != "OFFBOARD" &&
        (ros::Time::now() - last_request > ros::Duration(5.0))){
        if( set_mode_client.call(offb_set_mode) &&
            offb_set_mode.response.mode_sent){
            ROS_INFO("Offboard enabled");
        }
        last_request = ros::Time::now();
    } else {
        if( !current_state.armed &&
            (ros::Time::now() - last_request > ros::Duration(5.0))){
            if( arming_client.call(arm_cmd) &&
                arm_cmd.response.success){
                ROS_INFO("Vehicle armed");
            }
            last_request = ros::Time::now();
        }
    }

    theta = wn*count*0.05;

    pose.pose.position.x = r*sin(theta);
    pose.pose.position.y = r*cos(theta);
    pose.pose.position.z = 2;

    count++;

    local_pos_pub.publish(pose);
    ros::spinOnce();
    rate.sleep();
}

return 0;

}

+end_src

실행: ~rosrun drone_control circle~ *** 해보기: 원의 너비와 속도를 바꾸어 보자. 힌트 (wn, r)

** 과제: 키보드로 OFFBOARD 모드 제어하기

~offb_node~ 코드를 참고하여, 키보드로 x,y,z 위치를 제어하여 보자.
참고: http://wiki.ros.org/teleop_twist_keyboard_cpp

** 참고