b5de62ce3852b97198899eeee0b6821394f34172
ros.md
| ... | ... | @@ -0,0 +1,1199 @@ |
| 1 | +#+STARTUP: showeverything |
|
| 2 | +#+AUTHOR: Donghee Park |
|
| 3 | +# Creative Commons, Share-Alike (cc) |
|
| 4 | +#+EMAIL: dongheepark@gmail.com |
|
| 5 | +#+TITLE: PX4 and ROS Programming Day 1 |
|
| 6 | +#+HTML_HEAD_EXTRA: <style type="text/css">img { width: auto ; max-width: 100% ; height: auto ;} </style> |
|
| 7 | +#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="https://gongzhitaao.org/orgcss/org.css"/> |
|
| 8 | + |
|
| 9 | +* 수업 |
|
| 10 | + - 목표: 드론 제어 소프트웨어에 대해 이해할 수 있고, 프로그래밍 환경을 구축할 수 있다. |
|
| 11 | + - 교재: https://learn.dronemap.io/ |
|
| 12 | + - 코치: 박동희 dongheepark@gmail.com |
|
| 13 | + |
|
| 14 | +* 드론 제어 소프트웨어 소개 및 설치 |
|
| 15 | + |
|
| 16 | +1. 워크숍 소개, 참가자 소개 |
|
| 17 | + |
|
| 18 | +2. PX4 소개, ROS 소개 |
|
| 19 | + |
|
| 20 | +3. Linux 사용하기 |
|
| 21 | + |
|
| 22 | +* PX4 소개 |
|
| 23 | + |
|
| 24 | + - https://px4.io/ |
|
| 25 | + - Pixhawk |
|
| 26 | + - PX4 Firmware |
|
| 27 | + - Mavlink |
|
| 28 | + - QGroundControl |
|
| 29 | + - https://learn.dronemap.io/px4-workbook/px4-intro.html |
|
| 30 | + - 소프트웨어 아키텍처 |
|
| 31 | + - 시뮬레이터 포트 구성 |
|
| 32 | + |
|
| 33 | +** PX4 미들웨어 및 앱 구조 소개 |
|
| 34 | + - 디렉토리 구조 소개 |
|
| 35 | + - uORB |
|
| 36 | + - PX4 shell 사용하기 |
|
| 37 | + - hello-skyworld |
|
| 38 | + |
|
| 39 | +* Linux 사용하기 |
|
| 40 | + - Ubuntu 18.04 설치 |
|
| 41 | + - 주요 명령어 소개(파일 조작, 프로그램설치, 쉘스크립트, git) |
|
| 42 | + |
|
| 43 | +ls: 파일 또는 디렉토리의 목록을 출력 |
|
| 44 | +#+BEGIN_SRC |
|
| 45 | +ls |
|
| 46 | +ls -al |
|
| 47 | +#+END_SRC |
|
| 48 | + |
|
| 49 | +cd: 디렉토리 이동 |
|
| 50 | +#+BEGIN_SRC |
|
| 51 | +cd ~ |
|
| 52 | +cd ~/Downloads |
|
| 53 | +#+END_SRC |
|
| 54 | + |
|
| 55 | +pwd: 현재 디렉토리 출력 |
|
| 56 | +#+BEGIN_SRC |
|
| 57 | +pwd |
|
| 58 | +#+END_SRC |
|
| 59 | + |
|
| 60 | +mkdir: 디렉토리 생성 |
|
| 61 | +#+BEGIN_SRC |
|
| 62 | +mkdir tmp |
|
| 63 | +#+END_SRC |
|
| 64 | + |
|
| 65 | +rm: 파일 또는 디렉토리 지우기 |
|
| 66 | +#+BEGIN_SRC |
|
| 67 | +rm -rf tmp |
|
| 68 | +#+END_SRC |
|
| 69 | + |
|
| 70 | +cat: 파일 입력 또는 출력 |
|
| 71 | +#+BEGIN_SRC |
|
| 72 | +cat ~/.bashrc |
|
| 73 | +cat > ~/.hello.c |
|
| 74 | +#+END_SRC |
|
| 75 | + |
|
| 76 | +cp: 파일 또는 디렉토리 복사 |
|
| 77 | +#+BEGIN_SRC |
|
| 78 | +cp hello.c world.c |
|
| 79 | +#+END_SRC |
|
| 80 | + |
|
| 81 | +chmod: 파일의 퍼미션 지정 |
|
| 82 | +#+BEGIN_SRC |
|
| 83 | +chmod +x hello |
|
| 84 | +#+END_SRC |
|
| 85 | + |
|
| 86 | +wget: url에서 파일 다운로드 |
|
| 87 | +#+BEGIN_SRC |
|
| 88 | +wget https://google.com |
|
| 89 | +#+END_SRC |
|
| 90 | + |
|
| 91 | +source: 현재 쉘에서 파일을 읽고 실행 |
|
| 92 | +#+BEGIN_SRC |
|
| 93 | +source ~/.bashrc |
|
| 94 | +#+END_SRC |
|
| 95 | + |
|
| 96 | + |
|
| 97 | +* PX4 개발환경 구성 (+ROS) |
|
| 98 | + |
|
| 99 | +https://dev.px4.io/master/en/setup/dev_env_linux_ubuntu.html |
|
| 100 | + |
|
| 101 | +#+BEGIN_SRC |
|
| 102 | +cd ~ |
|
| 103 | +wget https://raw.githubusercontent.com/PX4/Devguide/master/build_scripts/ubuntu_sim_ros_melodic.sh |
|
| 104 | +chmod +x ubuntu_sim_ros_melodic.sh |
|
| 105 | +./ubuntu_sim_ros_melodic.sh |
|
| 106 | +#+END_SRC |
|
| 107 | + |
|
| 108 | +* PX4 Firmware Build |
|
| 109 | + - https://dev.px4.io |
|
| 110 | + - Gazebo 사용하기 |
|
| 111 | + - Qgroundcontrol |
|
| 112 | + |
|
| 113 | +** PX4 v.10.1 다운로드 |
|
| 114 | +#+BEGIN_SRC |
|
| 115 | +cd ~ |
|
| 116 | +git clone https://github.com/PX4/Firmware.git --recursive |
|
| 117 | +cd ~/Firmware |
|
| 118 | +bash ./Tools/setup/ubuntu.sh |
|
| 119 | +git checkout v1.10.1 |
|
| 120 | +git submodule update --init --recursive |
|
| 121 | +#+END_SRC |
|
| 122 | + |
|
| 123 | +** Gazebo 실행 |
|
| 124 | +#+BEGIN_SRC |
|
| 125 | +cd ~/Firmware |
|
| 126 | +make px4_sitl gazebo |
|
| 127 | +#+END_SRC |
|
| 128 | + |
|
| 129 | +** QGroundControl 사용 |
|
| 130 | + |
|
| 131 | +다운로드: https://docs.qgroundcontrol.com/en/getting_started/download_and_install.html |
|
| 132 | + |
|
| 133 | +QGroundControl 다운로드 및 실행 |
|
| 134 | +#+BEGIN_SRC |
|
| 135 | +sudo usermod -a -G dialout $USER |
|
| 136 | +sudo apt-get remove modemmanager -y |
|
| 137 | +sudo apt install gstreamer1.0-plugins-bad gstreamer1.0-libav gstreamer1.0-gl -y |
|
| 138 | + |
|
| 139 | +cd ~/Downloads |
|
| 140 | +chmod +x ./QGroundControl.AppImage |
|
| 141 | +./QGroundControl.AppImage |
|
| 142 | +#+END_SRC |
|
| 143 | + |
|
| 144 | + |
|
| 145 | +* ROS 프로그래밍 |
|
| 146 | + |
|
| 147 | +** ROS |
|
| 148 | + - Robot Operating System: 로봇 빌드에 사용되는 라이브러리 어플리케이션 모음 http://www.ros.org/ |
|
| 149 | + - 목표: 로봇을 만들때 기존의 재활용 하고 공유하자. |
|
| 150 | + - History: |
|
| 151 | + - 2000s: Standford Artificial intelligence |
|
| 152 | + - 2007: Willow Garage |
|
| 153 | + - 2013: Open Source Robotics Foundation |
|
| 154 | + - 사용 분야: Drone, Kinematic ARMS(로봇암), Wheeled(바퀴), Bi-pedal(이족) |
|
| 155 | + |
|
| 156 | +** ROS Nodes and Topics |
|
| 157 | + |
|
| 158 | +# https://misohena.jp/article/emacs_org_textfigures/dot.html |
|
| 159 | + |
|
| 160 | +#+begin_src dot :file ros_nodes_and_topics1.png :cmdline -Kdot -Tpng |
|
| 161 | +digraph { |
|
| 162 | + rankdir=LR |
|
| 163 | + graph [fontname="MS Gothic"]; |
|
| 164 | + node [shape=rect, color="#40e0d0"] |
|
| 165 | + edge [fontname="MS Gothic"]; |
|
| 166 | + label = "Robot Communication Sequence"; |
|
| 167 | + "Perception" -> "Dicesion Making"; |
|
| 168 | + "Dicesion Making" -> "Actuation"; |
|
| 169 | +} |
|
| 170 | +#+end_src |
|
| 171 | + |
|
| 172 | +**** Perception: Sense |
|
| 173 | + - Sensor Fusion |
|
| 174 | + - Filtering |
|
| 175 | + - Localization |
|
| 176 | + |
|
| 177 | +**** Dicesion Making: Decide |
|
| 178 | + - Path Planning |
|
| 179 | + - Prediction |
|
| 180 | + - Behavior Planning |
|
| 181 | + |
|
| 182 | +**** Actuation: Act |
|
| 183 | + - PID Control |
|
| 184 | + - Model Predictive Control |
|
| 185 | + |
|
| 186 | +*** ROS Master Process |
|
| 187 | + |
|
| 188 | +노드 관리 |
|
| 189 | + |
|
| 190 | +#+begin_src dot :file ros_master_process1.png :cmdline -Kdot -Tpng |
|
| 191 | +digraph { |
|
| 192 | + graph [fontname="MS Gothic"]; |
|
| 193 | + node [shape=box, color="#40e0d0"] |
|
| 194 | + edge [fontname="MS Gothic"]; |
|
| 195 | + label = "ROS Master Process"; |
|
| 196 | + |
|
| 197 | + subgraph cluster_perception { |
|
| 198 | + node [shape=rect, style="rounded"] |
|
| 199 | + label = "Perception"; |
|
| 200 | + Camera; |
|
| 201 | + "Wheel Encoder"; |
|
| 202 | + "Positon Estimator"; |
|
| 203 | + } |
|
| 204 | + |
|
| 205 | + subgraph cluster_dicesion_making { |
|
| 206 | + node [shape=rect, style="rounded"] |
|
| 207 | + label = "Dicesion Making"; |
|
| 208 | + "Behavior Execution"; |
|
| 209 | + } |
|
| 210 | + |
|
| 211 | + subgraph cluster_actuation { |
|
| 212 | + node [shape=rect, style="rounded"] |
|
| 213 | + label = "Actuation"; |
|
| 214 | + "Motor Control"; |
|
| 215 | + } |
|
| 216 | +} |
|
| 217 | +#+end_src |
|
| 218 | + |
|
| 219 | +*** Topics |
|
| 220 | + |
|
| 221 | +노드간 통신 인터페이스. 구독 발행의 이름 |
|
| 222 | + |
|
| 223 | +#+begin_src dot :file ros_topic1.png :cmdline -Kdot -Tpng |
|
| 224 | +digraph { |
|
| 225 | + rankdir=LR |
|
| 226 | + node [color="#40e0d0"] |
|
| 227 | + edge [fontname="MS Gothic"]; |
|
| 228 | + node1 [label= ""] |
|
| 229 | + node2 [label= ""] |
|
| 230 | + |
|
| 231 | + node1 -> node2 [label="/topic_name"]; |
|
| 232 | +} |
|
| 233 | + |
|
| 234 | +#+end_src |
|
| 235 | + |
|
| 236 | +*** Publish and Subscribe |
|
| 237 | + |
|
| 238 | +발행과 구독. 신문/잡지 발행 구독에 비유 |
|
| 239 | + |
|
| 240 | +#+begin_src dot :file ros_publish_and_subscribe1.png :cmdline -Kdot -Tpng |
|
| 241 | +digraph { |
|
| 242 | + rankdir=LR |
|
| 243 | + node [color="#40e0d0"] |
|
| 244 | + edge [fontname="MS Gothic"]; |
|
| 245 | + label = " PUBLISH SUBSCRIBER"; |
|
| 246 | + node1 [label= ""] |
|
| 247 | + node2 [label= ""] |
|
| 248 | + |
|
| 249 | + node1 -> node2 |
|
| 250 | + node2 -> node1 |
|
| 251 | +} |
|
| 252 | +#+end_src |
|
| 253 | + |
|
| 254 | +실제 예제 |
|
| 255 | + |
|
| 256 | +#+begin_src dot :file ros_publish_and_subscribe2.png :cmdline -Kdot -Tpng |
|
| 257 | +digraph { |
|
| 258 | + node [color="#40e0d0"] |
|
| 259 | + edge [fontname="MS Gothic"]; |
|
| 260 | + |
|
| 261 | + "Wheel Encoder" -> "Positon Estimator" [label="/wheel_encoder\lrotation"] |
|
| 262 | + "Behavior Executor" -> "Motor Controller" [label="/motor_controller\lvelocity_cmd"] |
|
| 263 | + "Camera" -> "Behavior Executor" [label="/camera_images\limage"] |
|
| 264 | + "Positon Estimator" -> "Behavior Executor" [label="/position_estimate\lpose"] |
|
| 265 | +} |
|
| 266 | +#+end_src |
|
| 267 | + |
|
| 268 | +** ROS Message Passing |
|
| 269 | + |
|
| 270 | +메시지: 노드간 통신할때 이동하는 실제 데이터 |
|
| 271 | + - 메시지는 텍스트로 구성. 메시지를 이해하기 쉽다. |
|
| 272 | + |
|
| 273 | +미리 정의된 메시지 타입 : |
|
| 274 | + - http://wiki.ros.org/common_msgs |
|
| 275 | + - https://github.com/ros/common_msgs |
|
| 276 | + |
|
| 277 | +** ROS Services |
|
| 278 | + |
|
| 279 | + - Request-Response, 1:1 통신 |
|
| 280 | + - PubSub이 필요 없는 경우 사용, 요청 할때만 데이터가 제공. 네트워크 부하가 적다. |
|
| 281 | + |
|
| 282 | +*** 예시: 카메라 이미지 얻기 |
|
| 283 | + |
|
| 284 | +#+begin_src dot :file ros_services1.png :cmdline -Kdot -Tpng |
|
| 285 | +digraph { |
|
| 286 | + rankdir=LR; |
|
| 287 | + node [color="#40e0d0"]; |
|
| 288 | + edge [fontname="MS Gothic"]; |
|
| 289 | + label = "Publicate and Subscribe"; |
|
| 290 | + |
|
| 291 | + "Camera" -> "Behavior Executor" [label="/camera_images\limage"] |
|
| 292 | +} |
|
| 293 | +#+end_src |
|
| 294 | + |
|
| 295 | +#+begin_src dot :file ros_services2.png :cmdline -Kdot -Tpng |
|
| 296 | +digraph { |
|
| 297 | + rankdir=LR; |
|
| 298 | + node [color="#40e0d0"]; |
|
| 299 | + edge [fontname="MS Gothic", style=dotted]; |
|
| 300 | + label = "Request-Response"; |
|
| 301 | + |
|
| 302 | + |
|
| 303 | + "Behavior Executor" -> "Camera" [label="/capture_image\lrequest: exposure time"] |
|
| 304 | + "Camera" -> "Behavior Executor" [label="\nresponse: image"] |
|
| 305 | +} |
|
| 306 | +#+end_src |
|
| 307 | + |
|
| 308 | +** ROS Turtlesim |
|
| 309 | + |
|
| 310 | +Turtle |
|
| 311 | + |
|
| 312 | +#+ATTR_HTML: width="200px" |
|
| 313 | +[[https://i.imgur.com/0r46gFH.png]] |
|
| 314 | + |
|
| 315 | +*** Turtlesim 실행하기 |
|
| 316 | + |
|
| 317 | +[[https://d17h27t6h515a5.cloudfront.net/topher/2017/March/58d9820b_running-turtlesim/running-turtlesim.png]] |
|
| 318 | + |
|
| 319 | + |
|
| 320 | +1. 환경 변수 설정 |
|
| 321 | + |
|
| 322 | +#+begin_src sh |
|
| 323 | +source /opt/ros/melodic/setup.bash |
|
| 324 | +#+end_src |
|
| 325 | + |
|
| 326 | +2. roscore 실행 |
|
| 327 | + - roscore: Master + rosout + parameter server |
|
| 328 | + - Master: 네임 서비스 |
|
| 329 | + - rosout: stdout/stderr 로깅 |
|
| 330 | + - parameter server: 파라미터 저장 서버 |
|
| 331 | + |
|
| 332 | +#+begin_src sh |
|
| 333 | +roscore |
|
| 334 | +#+end_src |
|
| 335 | + |
|
| 336 | +3. turtlesim 패키지의 ~turtlesim_node~ 실행 |
|
| 337 | +#+begin_src sh |
|
| 338 | +rosrun turtlesim turtlesim_node |
|
| 339 | +#+end_src |
|
| 340 | + |
|
| 341 | +4. turtlesim 패키지의 ~turtle_teleop_key~ 실행 |
|
| 342 | +#+begin_src sh |
|
| 343 | +rosrun turtlesim turtle_teleop_key |
|
| 344 | +#+end_src |
|
| 345 | + |
|
| 346 | +*** Turtlesim 노드 목록 |
|
| 347 | + |
|
| 348 | +#+begin_src sh |
|
| 349 | +rosnode list |
|
| 350 | +#+end_src |
|
| 351 | + |
|
| 352 | +/rosout : ROS 메시지 로깅. |
|
| 353 | + |
|
| 354 | +*** Turtlesim 토픽 목록 |
|
| 355 | +#+begin_src sh |
|
| 356 | +rostopic list |
|
| 357 | +#+end_src |
|
| 358 | + |
|
| 359 | +*** Turtlesim 토픽 정보 |
|
| 360 | + |
|
| 361 | +#+begin_src sh |
|
| 362 | +rostopic info /turtle1/cmd_vel |
|
| 363 | + |
|
| 364 | +#+end_src |
|
| 365 | + |
|
| 366 | +*** Turtlesim 메시지 정보 |
|
| 367 | + |
|
| 368 | +#+begin_src sh |
|
| 369 | +$ rosmsg info geometry_msgs/Twist |
|
| 370 | +geometry_msgs/Vector3 linear |
|
| 371 | + float64 x |
|
| 372 | + float64 y |
|
| 373 | + float64 z |
|
| 374 | +geometry_msgs/Vector3 angular |
|
| 375 | + float64 x |
|
| 376 | + float64 y |
|
| 377 | + float64 z |
|
| 378 | +#+end_src |
|
| 379 | + |
|
| 380 | +or |
|
| 381 | + |
|
| 382 | +#+begin_src sh |
|
| 383 | +rosed geometry_msgs Twist.msg |
|
| 384 | +#+end_src |
|
| 385 | + |
|
| 386 | +*** Turtlesim Echo a Topic |
|
| 387 | + |
|
| 388 | +디버깅시 편리 |
|
| 389 | + |
|
| 390 | +#+begin_src sh |
|
| 391 | +rostopic echo /turtle1/cmd_vel |
|
| 392 | +#+end_src |
|
| 393 | + |
|
| 394 | +*** ~rqt_graph~ |
|
| 395 | + |
|
| 396 | +#+begin_src sh |
|
| 397 | +rqt_graph |
|
| 398 | +#+end_src |
|
| 399 | + |
|
| 400 | +[[http://wiki.ros.org/rqt_graph?action=AttachFile&do=get&target=snap_rqt_graph_moveit_demo.png]] |
|
| 401 | + |
|
| 402 | +** MavROS |
|
| 403 | + |
|
| 404 | +http://wiki.ros.org/mavros mavlink ros wrapper |
|
| 405 | + |
|
| 406 | +[[https://i.imgur.com/9z8DEIn.png]] |
|
| 407 | + |
|
| 408 | +*** MavROS 설치 및 실행 (이미 자동설치됨, 안해도됨) |
|
| 409 | + |
|
| 410 | +#+begin_src sh |
|
| 411 | + |
|
| 412 | +## Create catkin workspace (ROS build system) |
|
| 413 | +mkdir -p ~/catkin_ws/src |
|
| 414 | +cd ~/catkin_ws |
|
| 415 | + |
|
| 416 | +## Install dependencies |
|
| 417 | +sudo apt-get install python-wstool python-rosinstall-generator python-catkin-tools -y |
|
| 418 | + |
|
| 419 | +## Initialise wstool |
|
| 420 | +wstool init ~/catkin_ws/src |
|
| 421 | + |
|
| 422 | +## Build MAVROS |
|
| 423 | +### Get source (upstream - released) |
|
| 424 | +rosinstall_generator --upstream mavros --rosdistro kinetic | tee /tmp/mavros.rosinstall |
|
| 425 | +### Get latest released mavlink package |
|
| 426 | +rosinstall_generator mavlink --rosdistro kinetic | tee -a /tmp/mavros.rosinstall |
|
| 427 | +### Setup workspace & install deps |
|
| 428 | +wstool merge -t src /tmp/mavros.rosinstall |
|
| 429 | +wstool update -t src |
|
| 430 | +rosdep install --from-paths src --ignore-src --rosdistro kinetic -y |
|
| 431 | + |
|
| 432 | +### Install GeographicLib datasets |
|
| 433 | +./src/mavros/mavros/scripts/install_geographiclib_datasets.sh |
|
| 434 | + |
|
| 435 | +### Build source |
|
| 436 | +catkin build |
|
| 437 | + |
|
| 438 | +### source setup.bash |
|
| 439 | +source devel/setup.bash |
|
| 440 | + |
|
| 441 | +#+end_src |
|
| 442 | + |
|
| 443 | +환경변수 설정: workspace |
|
| 444 | + |
|
| 445 | +#+begin_src |
|
| 446 | +cd ~/catkin_ws |
|
| 447 | +source devel/setup.bash |
|
| 448 | +#+end_src |
|
| 449 | + |
|
| 450 | +~mavros_node~ 실행 |
|
| 451 | + |
|
| 452 | +#+begin_src sh |
|
| 453 | +rosrun mavros mavros_node _fcu_url:="udp://:14540@127.0.0.1:14557" _gcs_url:="udp://@127.0.0.1" |
|
| 454 | +#+end_src |
|
| 455 | + |
|
| 456 | +~mavros_node~ 실행 (roslaunch 사용하는 방법) |
|
| 457 | +#+begin_src sh |
|
| 458 | +# px4.launch 이용하여 mavros node 실행. fcu ip주소는 127.0.0.1 ip주소를 바꾸면 다른 컴퓨터의 fcu도 연결 가능. |
|
| 459 | +cd ~/catkin_ws |
|
| 460 | +roslaunch mavros px4.launch fcu_url:="udp://:14540@127.0.0.1:14557" _gcs_url:"udp://@127.0.0.1" |
|
| 461 | +#+end_src |
|
| 462 | + |
|
| 463 | +** Gazebo 실행 |
|
| 464 | +시뮬레이터의 홈 위치(위도 경도 해발고도) 지정 |
|
| 465 | + |
|
| 466 | +#+begin_src sh |
|
| 467 | +export PX4_HOME_LAT=35.9012382 |
|
| 468 | +export PX4_HOME_LON=128.854495337 |
|
| 469 | +export PX4_HOME_ALT=71 |
|
| 470 | + |
|
| 471 | +cd ~/Firmware |
|
| 472 | +make px4_sitl gazebo |
|
| 473 | +#+end_src |
|
| 474 | + |
|
| 475 | + |
|
| 476 | +* ROS 노드 관리 |
|
| 477 | + |
|
| 478 | +** ROS 노드 실행 및 관리 |
|
| 479 | + |
|
| 480 | +**** ROS Core 노드 실행 |
|
| 481 | +#+begin_src sh |
|
| 482 | +roscore |
|
| 483 | +#+end_src |
|
| 484 | + |
|
| 485 | +**** MAVROS 노드 실행 |
|
| 486 | +#+begin_src sh |
|
| 487 | +rosrun mavros mavros_node _fcu_url:="udp://:14540@127.0.0.1:14557" _gcs_url:="udp://@127.0.0.1" |
|
| 488 | +#+end_src |
|
| 489 | + |
|
| 490 | +# J120 UART2에 PX4 telemetry 2 연결 |
|
| 491 | +# roslaunch mavros px4.launch fcu_url:="/dev/ttyTHS1:921600" gcs_url:="udp://@192.168.88.53" |
|
| 492 | + |
|
| 493 | +**** 토픽 목록 |
|
| 494 | +#+begin_src sh |
|
| 495 | +rostopic list |
|
| 496 | +#+end_src |
|
| 497 | + |
|
| 498 | +**** 토픽 내용 보기 |
|
| 499 | + |
|
| 500 | +***** 메시지 타입 보기 |
|
| 501 | +#+begin_src sh |
|
| 502 | +rostopic info /mavros/state |
|
| 503 | +#+end_src |
|
| 504 | + |
|
| 505 | +타입 내부 보기 |
|
| 506 | +#+begin_src sh |
|
| 507 | +rostopic type /mavros/state | rosmsg show |
|
| 508 | +#+end_src |
|
| 509 | + |
|
| 510 | +***** 메시지 내용 |
|
| 511 | +#+begin_src sh |
|
| 512 | +rostopic echo /mavros/state |
|
| 513 | +#+end_src |
|
| 514 | + |
|
| 515 | +***** 토픽 publish 주기 보기 |
|
| 516 | +#+begin_src sh |
|
| 517 | +rostopic hz /mavros/state |
|
| 518 | +#+end_src |
|
| 519 | + |
|
| 520 | +***** 실행 노드 확인 |
|
| 521 | +#+begin_src sh |
|
| 522 | +rqt_graph |
|
| 523 | +#+end_src |
|
| 524 | + |
|
| 525 | +** ROS 노드 명령 실행하기. (MAVROS 위주) |
|
| 526 | + |
|
| 527 | +http://wiki.ros.org/ROS/Tutorials/UnderstandingTopics |
|
| 528 | + |
|
| 529 | +*** Subscribe |
|
| 530 | + |
|
| 531 | +#+begin_src sh |
|
| 532 | +rostopic echo [topic] |
|
| 533 | +#+end_src |
|
| 534 | + |
|
| 535 | +**** ~STATE~ |
|
| 536 | +#+begin_src sh |
|
| 537 | +rostopic echo /mavros/state |
|
| 538 | +#+end_src |
|
| 539 | + |
|
| 540 | +**** ~LOCAL_POSITION~ 확인 |
|
| 541 | + |
|
| 542 | +#+begin_src sh |
|
| 543 | +rostopic echo /mavros/local_position/pose |
|
| 544 | +#+end_src |
|
| 545 | + |
|
| 546 | +*** Services |
|
| 547 | + |
|
| 548 | +서비스 목록 |
|
| 549 | + |
|
| 550 | +#+begin_src sh |
|
| 551 | +rosservice list |
|
| 552 | +#+end_src |
|
| 553 | + |
|
| 554 | +서비스 실행(call) 하기 |
|
| 555 | + |
|
| 556 | +#+begin_src sh |
|
| 557 | +rosservice call [topic] [msg_type] [args] |
|
| 558 | +#+end_src |
|
| 559 | + |
|
| 560 | +**** ~SET_MODE~ |
|
| 561 | + |
|
| 562 | +#+begin_src sh |
|
| 563 | +# https://github.com/mavlink/mavros/blob/master/mavros_msgs/srv/SetMode.srv |
|
| 564 | +# http://wiki.ros.org/mavros/CustomModes |
|
| 565 | +# Manual Mode |
|
| 566 | +rosservice call /mavros/set_mode "base_mode: 64 |
|
| 567 | +custom_mode: ''" |
|
| 568 | + |
|
| 569 | +rosservice call /mavros/set_mode "base_mode: 0 |
|
| 570 | +custom_mode: 'MANUAL'" |
|
| 571 | + |
|
| 572 | +rosservice call /mavros/set_mode "base_mode: 0 |
|
| 573 | +custom_mode: 'POSCTL'" |
|
| 574 | + |
|
| 575 | +rosservice call /mavros/set_mode "base_mode: 0 |
|
| 576 | +custom_mode: 'OFFBOARD'" |
|
| 577 | + |
|
| 578 | +rosservice call /mavros/set_mode "base_mode: 0 |
|
| 579 | +custom_mode: 'AUTO.LAND'" |
|
| 580 | +#+end_src |
|
| 581 | + |
|
| 582 | +**** ~ARMING~ |
|
| 583 | +#+begin_src sh |
|
| 584 | +rosservice call /mavros/cmd/arming "value: true" |
|
| 585 | +#+end_src |
|
| 586 | + |
|
| 587 | +**** ~TAKEOFF~ |
|
| 588 | + |
|
| 589 | +#+begin_src sh |
|
| 590 | +rosservice call /mavros/cmd/takeoff "{min_pitch: 0.0, yaw: 0.0, latitude: 47.3977508, longitude: 8.5456074, altitude: 2.5}" |
|
| 591 | +#+end_src |
|
| 592 | + |
|
| 593 | +**** ~Land~ |
|
| 594 | + |
|
| 595 | +#+begin_src sh |
|
| 596 | +rosservice call /mavros/cmd/land "{min_pitch: 0.0, yaw: 0.0, latitude: 47.3977508, longitude: 8.5456074, altitude: 0.0}" |
|
| 597 | +#+end_src |
|
| 598 | + |
|
| 599 | +*** Publish |
|
| 600 | + |
|
| 601 | +#+begin_src sh |
|
| 602 | +rostopic pub [topic] [msg_type] [args] |
|
| 603 | +#+end_src |
|
| 604 | + |
|
| 605 | +**** ~SETPOINT_POSITION~ |
|
| 606 | + |
|
| 607 | +OFFBOARD 모드에서 동작 |
|
| 608 | + |
|
| 609 | +#+begin_src sh |
|
| 610 | +rostopic pub -r 10 /mavros/setpoint_position/local geometry_msgs/PoseStamped "header: |
|
| 611 | + auto |
|
| 612 | +pose: |
|
| 613 | + position: |
|
| 614 | + x: 5.0 |
|
| 615 | + y: 0.0 |
|
| 616 | + z: 1.0 |
|
| 617 | + orientation: |
|
| 618 | + x: 0.0 |
|
| 619 | + y: 0.0 |
|
| 620 | + z: 0.0 |
|
| 621 | + w: 0.0" |
|
| 622 | +#+end_src |
|
| 623 | + |
|
| 624 | +**** ~SETPOINT_VELOCITY~ |
|
| 625 | + |
|
| 626 | +OFFBOARD 모드에서 동작 |
|
| 627 | + |
|
| 628 | +#+begin_src sh |
|
| 629 | +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}}}" |
|
| 630 | +#+end_src |
|
| 631 | + |
|
| 632 | +*** 실습 |
|
| 633 | + |
|
| 634 | +준비: 순서대로 실행 |
|
| 635 | + - Gazebo 실행: ~cd ~/Firmware; make px4_sitl gazebo~ |
|
| 636 | + - 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"~ \\ |
|
| 637 | + 또는 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"~ |
|
| 638 | + - QGroundControl 실행: PX4 Parameter ~COM_OF_LOSS_T~ 파라미터 15초로 바꾸기. Failsafe timeout을 15초로 바꾸어야 커맨드라인에서 드론을 조정하기 편하다. |
|
| 639 | + |
|
| 640 | +\\ |
|
| 641 | + |
|
| 642 | +해보기: 커맨드 라인에서 다음 명령을 수행하여, QGroundControl에 아래와 같은 자취를 남겨보자. |
|
| 643 | + - 1. ARM |
|
| 644 | + - 2. TAKEOFF 하고. (옵션) |
|
| 645 | + - 3. 현재 위치를 지정한다. ~/mavros/setpoint_position~ 이용하여, (0,0,0) 위치를 10hz로 지정 |
|
| 646 | + - 3-1. MODE 변환. OFFBOARD |
|
| 647 | + - 4. 20m 간격으로 정사각형을 따라 움직인다. ~/mavros/setpoint_position~ 이용 |
|
| 648 | + - 5. HOME 자리에 오면 LAND 한다. |
|
| 649 | + - 6. DISARM |
|
| 650 | + |
|
| 651 | +[[https://i.imgur.com/4IjvTca.png]] |
|
| 652 | + |
|
| 653 | +더해보기: 드론의 머리방향이 진행 방향을 향하도록 하자. |
|
| 654 | + |
|
| 655 | + - Body 3-2-1 순서 오일러->쿼터니언 변환 |
|
| 656 | + |
|
| 657 | +#+HTML_MATHJAX: align: left indent: 5em tagside: left font: Neo-Euler |
|
| 658 | +\begin{align} |
|
| 659 | +\begin{bmatrix} |
|
| 660 | +x \\ |
|
| 661 | +y \\ |
|
| 662 | +z \\ |
|
| 663 | +w \\ |
|
| 664 | +\end{bmatrix} |
|
| 665 | +& = |
|
| 666 | +\begin{bmatrix} |
|
| 667 | +\cos (\phi /2) \cos (\theta /2) \cos (\psi /2) + \sin (\phi /2) \sin (\theta /2) \sin (\psi /2) \\ |
|
| 668 | +\sin (\phi /2) \cos (\theta /2) \cos (\psi /2) - \cos (\phi /2) \sin (\theta /2) \sin (\psi /2) \\ |
|
| 669 | +\cos (\phi /2) \sin (\theta /2) \cos (\psi /2) + \sin (\phi /2) \cos (\theta /2) \sin (\psi /2) \\ |
|
| 670 | +\cos (\phi /2) \cos (\theta /2) \sin (\psi /2) - \sin (\phi /2) \sin (\theta /2) \cos (\psi /2) \\ |
|
| 671 | +\end{bmatrix} \\ |
|
| 672 | +\end{align} |
|
| 673 | + |
|
| 674 | +변환 코드(python): https://gist.github.com/donghee/e3b4fa8ec789cec0e287bf3b91ddb79e |
|
| 675 | + |
|
| 676 | +# https://www.andre-gaschler.com/rotationconverter/ |
|
| 677 | + |
|
| 678 | +*** 유용한 mavros 명령(노드) 모음 |
|
| 679 | + |
|
| 680 | +mavros 패키지의 mavsafety 노드: arm, disarm, safetyarea |
|
| 681 | + |
|
| 682 | +#+begin_src |
|
| 683 | +rosrun mavros mavsafety arm |
|
| 684 | +#+end_src |
|
| 685 | + |
|
| 686 | +mavcmd 노드: |
|
| 687 | + |
|
| 688 | +예시: takeoff from current position (10도 각도 피치, 90도 방향 보고, 5m 위로 takeoff) |
|
| 689 | +#+begin_src |
|
| 690 | +rosrun mavros mavcmd takeoffcur 10 90 5 |
|
| 691 | +#+end_src |
|
| 692 | + |
|
| 693 | +예시: home 지정(RTL 위치, 위도 35.9012382 경도 128.85449537 해발고도 71m) |
|
| 694 | +google earth: https://earth.google.com/web/search/35.9012382+128.85449537 |
|
| 695 | +#+begin_src |
|
| 696 | +rosrun mavros mavcmd sethome 35.9012382 128.854495337 71 |
|
| 697 | +#+end_src |
|
| 698 | + |
|
| 699 | +mavsetp 노드: setpoint 한번 보내기 (setpoint 테스트용, position, velocity, acceleration 가능) |
|
| 700 | + |
|
| 701 | +예시: x=1m, y=1m, z=1m, yaw=90도 setpoint 보내기 |
|
| 702 | +#+begin_src |
|
| 703 | +rosrun mavros mavsetp local -p 1 1 2 90 |
|
| 704 | +#+end_src |
|
| 705 | + |
|
| 706 | +mavsys 노드: change mode |
|
| 707 | +#+begin_src |
|
| 708 | +rosrun mavros mavsys mode -c OFFBOARD |
|
| 709 | +#+end_src |
|
| 710 | + |
|
| 711 | +mavparam 노드: parameter set, get, load, dump |
|
| 712 | + |
|
| 713 | +예시: 파라미터 덤프 |
|
| 714 | +#+begin_src |
|
| 715 | +rosrun mavros mavparam dump /tmp/params |
|
| 716 | +#+end_src |
|
| 717 | + |
|
| 718 | +mavftp 노드: px4의 파일 시스템 접근 |
|
| 719 | + |
|
| 720 | +예시: 로그 다운로드 |
|
| 721 | +#+begin_src |
|
| 722 | +rosrun mavros mavftp download log/2020-08-03/14_37_15.ul |
|
| 723 | +#+end_src |
|
| 724 | + |
|
| 725 | + |
|
| 726 | +** 토픽 레코드: rosbag |
|
| 727 | +리뷰할때 유용 |
|
| 728 | + |
|
| 729 | +토픽 저장하기 |
|
| 730 | +#+begin_src |
|
| 731 | +rostopic list -v |
|
| 732 | +mkdir ~/bagfiles |
|
| 733 | +cd ~/bagfiles |
|
| 734 | +rosbag record -O iris_default_1 /mavros/local_position/pose |
|
| 735 | +rosbag info iris_default_1.bag |
|
| 736 | +rqt_bag |
|
| 737 | +#+end_src |
|
| 738 | + |
|
| 739 | +** 참고 |
|
| 740 | + - https://github.com/mavlink/mavros/tree/master/mavros |
|
| 741 | + |
|
| 742 | +* ROS 노드 만들기 |
|
| 743 | + |
|
| 744 | +** 새로운 노드 만들기 |
|
| 745 | + |
|
| 746 | +*** 패키지 만들기 |
|
| 747 | + |
|
| 748 | +#+begin_src sh |
|
| 749 | +source ~/catkin_ws/devel/setup.bash |
|
| 750 | +cd ~/catkin_ws/src |
|
| 751 | +catkin_create_pkg drone_control mavros sensor_msgs roscpp rospy |
|
| 752 | +#+end_src |
|
| 753 | + |
|
| 754 | +*** 노드 코드 작성 |
|
| 755 | + |
|
| 756 | +~~/catkin_ws/src/drone_control/src/drone_state.cpp~ |
|
| 757 | +#+begin_src c++ |
|
| 758 | +#include "ros/ros.h" |
|
| 759 | +#include "sensor_msgs/Imu.h" |
|
| 760 | + |
|
| 761 | +void imuDataCallback(const sensor_msgs::Imu::ConstPtr& msg){ |
|
| 762 | + ROS_INFO("\nlinear acceleration\ |
|
| 763 | + \nx: [%f]\ny:[%f]\nz:[%f]", msg->linear_acceleration.x, |
|
| 764 | + msg->linear_acceleration.y, msg->linear_acceleration.z); |
|
| 765 | +} |
|
| 766 | + |
|
| 767 | +int main(int argc, char **argv){ |
|
| 768 | + ros::init(argc, argv, "drone_state"); |
|
| 769 | + ros::NodeHandle nh; |
|
| 770 | + ros::Subscriber sub = nh.subscribe("/mavros/imu/data", 1000, imuDataCallback); |
|
| 771 | + ros::spin(); |
|
| 772 | + return 0; |
|
| 773 | +} |
|
| 774 | +#+end_src |
|
| 775 | + |
|
| 776 | +빌드 스크립트 추가 |
|
| 777 | + |
|
| 778 | +~~/catkin_ws/src/drone_control/CMakeLists.txt~ 파일 끝에 다음 3줄 추가 |
|
| 779 | + |
|
| 780 | +#+begin_src cmake |
|
| 781 | +include_directories(include ${catkin_INCLUDE_DIRS}) |
|
| 782 | +add_executable(drone_state src/drone_state.cpp) |
|
| 783 | +target_link_libraries(drone_state ${catkin_LIBRARIES}) |
|
| 784 | +#+end_src |
|
| 785 | + |
|
| 786 | +환경 변수 다시 로드! |
|
| 787 | +#+begin_src sh |
|
| 788 | +source ~/catkin_ws/devel/setup.bash |
|
| 789 | +#+end_src |
|
| 790 | + |
|
| 791 | +*** 패키지 빌드 |
|
| 792 | + |
|
| 793 | +#+begin_src sh |
|
| 794 | +cd ~/catkin_ws |
|
| 795 | +catkin build |
|
| 796 | +#+end_src |
|
| 797 | + |
|
| 798 | +*** 패키지 노드 실행 |
|
| 799 | + |
|
| 800 | +~drone_control~ 패키지의 ~drone_state~ 노드 실행 |
|
| 801 | +#+begin_src sh |
|
| 802 | +rosrun drone_control drone_state |
|
| 803 | +#+end_src |
|
| 804 | + |
|
| 805 | + |
|
| 806 | +*** 해보기: /mavros/state 읽어서 1초마다 비행 mode 한번씩 출력 |
|
| 807 | + - ~/mavros/state~ 타입 체크하여 헤더 include |
|
| 808 | + |
|
| 809 | +#+BEGIN_SRC c++ |
|
| 810 | + |
|
| 811 | +#include "ros/ros.h" |
|
| 812 | +#include "mavros_msgs/State.h" |
|
| 813 | + |
|
| 814 | +void droneStateCallback(const mavros_msgs::State::ConstPtr& msg){ |
|
| 815 | + ROS_INFO("\nDrone mode: %s", msg->mode.c_str()); |
|
| 816 | +} |
|
| 817 | + |
|
| 818 | +int main(int argc, char **argv){ |
|
| 819 | + ros::init(argc, argv, "drone_state"); |
|
| 820 | + ros::NodeHandle nh; |
|
| 821 | + ros::Subscriber sub = nh.subscribe("/mavros/state", 1000, droneStateCallback); |
|
| 822 | + ros::spin(); |
|
| 823 | + return 0; |
|
| 824 | +} |
|
| 825 | +#+END_SRC |
|
| 826 | + |
|
| 827 | +*** 해보기 결과: |
|
| 828 | + |
|
| 829 | +#+begin_src c++ |
|
| 830 | +$ rosrun drone_control drone_state |
|
| 831 | +[ INFO] [1539297808.077868114]: |
|
| 832 | +Drone mode: OFFBOARD |
|
| 833 | +[ INFO] [1539297808.525173697]: |
|
| 834 | +Drone mode: OFFBOARD |
|
| 835 | +[ INFO] [1539297809.565387356]: |
|
| 836 | +Drone mode: OFFBOARD |
|
| 837 | +#+end_src |
|
| 838 | + |
|
| 839 | +~rqt_graph~ |
|
| 840 | + |
|
| 841 | +[[https://i.imgur.com/CGHQVwc.png]] |
|
| 842 | + |
|
| 843 | + |
|
| 844 | +** 새로운 노드 만들기: 드론 이륙 착륙 |
|
| 845 | + |
|
| 846 | +#+begin_src dot :file mavros_takeoff_and_land1.png :cmdline -Kdot -Tpng |
|
| 847 | +digraph { |
|
| 848 | + rankdir=LR |
|
| 849 | + graph [fontname="NanumSquare"]; |
|
| 850 | + node [shape=rect, color="#40e0d0"] |
|
| 851 | + edge [style=dashed, fontname="NanumSquare"]; |
|
| 852 | + node1 [label= "/takeoff_and_land"] |
|
| 853 | + node2 [label= "/mavros"] |
|
| 854 | + |
|
| 855 | + node1 -> node2 [label="1. /mavros/cmd/arming"]; |
|
| 856 | + node1 -> node2 [label="2. /mavros/cmd/takeoff"]; |
|
| 857 | + node1 -> node2 [label="3. /mavros/cmd/land"]; |
|
| 858 | +} |
|
| 859 | +#+end_src |
|
| 860 | + |
|
| 861 | +~drone_control~ 패키지에 ~takeoff_and_land~ 노드를 만들어 보자. |
|
| 862 | + |
|
| 863 | +2.5m 이륙후 10초 있다가 착륙 |
|
| 864 | + |
|
| 865 | +~~/catkin_ws/src/drone_control/src/takeoff_and_land.cpp~ |
|
| 866 | +#+begin_src c++ |
|
| 867 | +#include <cstdlib> |
|
| 868 | + |
|
| 869 | +#include <ros/ros.h> |
|
| 870 | +#include <mavros_msgs/CommandBool.h> |
|
| 871 | +#include <mavros_msgs/CommandTOL.h> |
|
| 872 | +#include <mavros_msgs/SetMode.h> |
|
| 873 | + |
|
| 874 | +#include <geometry_msgs/PoseStamped.h> |
|
| 875 | + |
|
| 876 | +int main(int argc, char **argv) |
|
| 877 | +{ |
|
| 878 | + |
|
| 879 | + int rate = 20; |
|
| 880 | + |
|
| 881 | + ros::init(argc, argv, "takeoff_and_land"); |
|
| 882 | + ros::NodeHandle n; |
|
| 883 | + |
|
| 884 | + ros::Rate r(rate); |
|
| 885 | + |
|
| 886 | + ///////////////////ARM////////////////////// |
|
| 887 | + ros::ServiceClient arming_client = n.serviceClient<mavros_msgs::CommandBool>("/mavros/cmd/arming"); |
|
| 888 | + mavros_msgs::CommandBool arm_cmd; |
|
| 889 | + arm_cmd.request.value = true; |
|
| 890 | + |
|
| 891 | + if (arming_client.call(arm_cmd) && arm_cmd.response.success) |
|
| 892 | + { |
|
| 893 | + ROS_INFO("Vehicle armed"); |
|
| 894 | + } else { |
|
| 895 | + ROS_ERROR("Failed arming or disarming"); |
|
| 896 | + } |
|
| 897 | + |
|
| 898 | + /////////////////TAKEOFF//////////////////// |
|
| 899 | + ros::ServiceClient takeoff_client = n.serviceClient<mavros_msgs::CommandTOL>("/mavros/cmd/takeoff"); |
|
| 900 | + mavros_msgs::CommandTOL takeoff_cmd; |
|
| 901 | + takeoff_cmd.request.altitude = 10; |
|
| 902 | + takeoff_cmd.request.latitude = 47.3977508; |
|
| 903 | + takeoff_cmd.request.longitude = 8.5456074; |
|
| 904 | + takeoff_cmd.request.min_pitch = 0; |
|
| 905 | + takeoff_cmd.request.yaw = 0; |
|
| 906 | + if(takeoff_client.call(takeoff_cmd) && takeoff_cmd.response.success){ |
|
| 907 | + ROS_INFO("Okay Takeoff"); |
|
| 908 | + }else{ |
|
| 909 | + ROS_ERROR("Failed Takeoff"); |
|
| 910 | + } |
|
| 911 | + |
|
| 912 | + /////////////////DO STUFF/////////////////// |
|
| 913 | + sleep(10); |
|
| 914 | + |
|
| 915 | + |
|
| 916 | + ///////////////////LAND///////////////////// |
|
| 917 | + ros::ServiceClient land_client = n.serviceClient<mavros_msgs::CommandTOL>("/mavros/cmd/land"); |
|
| 918 | + mavros_msgs::CommandTOL land_cmd; |
|
| 919 | + land_cmd.request.altitude = 0; |
|
| 920 | + land_cmd.request.latitude = 47.3977508; |
|
| 921 | + land_cmd.request.longitude = 8.5456074; |
|
| 922 | + land_cmd.request.min_pitch = 0; |
|
| 923 | + land_cmd.request.yaw = 0; |
|
| 924 | + if(land_client.call(land_cmd) && land_cmd.response.success){ |
|
| 925 | + ROS_INFO("Okay Land"); |
|
| 926 | + }else{ |
|
| 927 | + ROS_ERROR("Failed Land"); |
|
| 928 | + } |
|
| 929 | + |
|
| 930 | + while (n.ok()) |
|
| 931 | + { |
|
| 932 | + ros::spinOnce(); |
|
| 933 | + r.sleep(); |
|
| 934 | + } |
|
| 935 | + |
|
| 936 | + return 0; |
|
| 937 | + |
|
| 938 | +} |
|
| 939 | +#+end_src |
|
| 940 | + |
|
| 941 | +~\~/catkin_ws/src/drone_control/CMakeLists.txt~ 파일 끝에 다음 2줄 추가 |
|
| 942 | + |
|
| 943 | +#+begin_src cmake |
|
| 944 | +add_executable(takeoff_and_land src/takeoff_and_land.cpp) |
|
| 945 | +target_link_libraries(takeoff_and_land ${catkin_LIBRARIES}) |
|
| 946 | +#+end_src |
|
| 947 | + |
|
| 948 | + * 컴파일: ~cd ~/catkin_ws; source devel/setup.bash; catkin build~ |
|
| 949 | + * 실행: ~rosrun drone_control takeoff_and_land~ |
|
| 950 | + |
|
| 951 | +** 새로운 노드 만들기: ~offb_node~ |
|
| 952 | + |
|
| 953 | +~drone_control~ 패키지에 ~offb_node~ 노드를 만들어 보자. |
|
| 954 | + |
|
| 955 | +2m 이륙. |
|
| 956 | + |
|
| 957 | +#+begin_src c++ |
|
| 958 | +/** |
|
| 959 | + * @file offb_node.cpp |
|
| 960 | + * @brief Offboard control example node, written with MAVROS version 0.19.x, PX4 Pro Flight |
|
| 961 | + * Stack and tested in Gazebo SITL |
|
| 962 | + */ |
|
| 963 | + |
|
| 964 | +#include <ros/ros.h> |
|
| 965 | +#include <geometry_msgs/PoseStamped.h> |
|
| 966 | +#include <mavros_msgs/CommandBool.h> |
|
| 967 | +#include <mavros_msgs/SetMode.h> |
|
| 968 | +#include <mavros_msgs/State.h> |
|
| 969 | + |
|
| 970 | +mavros_msgs::State current_state; |
|
| 971 | +void state_cb(const mavros_msgs::State::ConstPtr& msg){ |
|
| 972 | + current_state = *msg; |
|
| 973 | +} |
|
| 974 | + |
|
| 975 | +int main(int argc, char **argv) |
|
| 976 | +{ |
|
| 977 | + ros::init(argc, argv, "offb_node"); |
|
| 978 | + ros::NodeHandle nh; |
|
| 979 | + |
|
| 980 | + ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State> |
|
| 981 | + ("mavros/state", 10, state_cb); |
|
| 982 | + ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped> |
|
| 983 | + ("mavros/setpoint_position/local", 10); |
|
| 984 | + ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool> |
|
| 985 | + ("mavros/cmd/arming"); |
|
| 986 | + ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode> |
|
| 987 | + ("mavros/set_mode"); |
|
| 988 | + |
|
| 989 | + //the setpoint publishing rate MUST be faster than 2Hz |
|
| 990 | + ros::Rate rate(20.0); |
|
| 991 | + |
|
| 992 | + // wait for FCU connection |
|
| 993 | + while(ros::ok() && !current_state.connected){ |
|
| 994 | + ros::spinOnce(); |
|
| 995 | + rate.sleep(); |
|
| 996 | + } |
|
| 997 | + |
|
| 998 | + geometry_msgs::PoseStamped pose; |
|
| 999 | + pose.pose.position.x = 0; |
|
| 1000 | + pose.pose.position.y = 0; |
|
| 1001 | + pose.pose.position.z = 2; |
|
| 1002 | + |
|
| 1003 | + //send a few setpoints before starting |
|
| 1004 | + for(int i = 100; ros::ok() && i > 0; --i){ |
|
| 1005 | + local_pos_pub.publish(pose); |
|
| 1006 | + ros::spinOnce(); |
|
| 1007 | + rate.sleep(); |
|
| 1008 | + } |
|
| 1009 | + |
|
| 1010 | + mavros_msgs::SetMode offb_set_mode; |
|
| 1011 | + offb_set_mode.request.custom_mode = "OFFBOARD"; |
|
| 1012 | + |
|
| 1013 | + mavros_msgs::CommandBool arm_cmd; |
|
| 1014 | + arm_cmd.request.value = true; |
|
| 1015 | + |
|
| 1016 | + ros::Time last_request = ros::Time::now(); |
|
| 1017 | + |
|
| 1018 | + while(ros::ok()){ |
|
| 1019 | + if( current_state.mode != "OFFBOARD" && |
|
| 1020 | + (ros::Time::now() - last_request > ros::Duration(5.0))){ |
|
| 1021 | + if( set_mode_client.call(offb_set_mode) && |
|
| 1022 | + offb_set_mode.response.mode_sent){ |
|
| 1023 | + ROS_INFO("Offboard enabled"); |
|
| 1024 | + } |
|
| 1025 | + last_request = ros::Time::now(); |
|
| 1026 | + } else { |
|
| 1027 | + if( !current_state.armed && |
|
| 1028 | + (ros::Time::now() - last_request > ros::Duration(5.0))){ |
|
| 1029 | + if( arming_client.call(arm_cmd) && |
|
| 1030 | + arm_cmd.response.success){ |
|
| 1031 | + ROS_INFO("Vehicle armed"); |
|
| 1032 | + } |
|
| 1033 | + last_request = ros::Time::now(); |
|
| 1034 | + } |
|
| 1035 | + } |
|
| 1036 | + |
|
| 1037 | + local_pos_pub.publish(pose); |
|
| 1038 | + |
|
| 1039 | + ros::spinOnce(); |
|
| 1040 | + rate.sleep(); |
|
| 1041 | + } |
|
| 1042 | + |
|
| 1043 | + return 0; |
|
| 1044 | +} |
|
| 1045 | +#+end_src |
|
| 1046 | + |
|
| 1047 | + * 실행: ~rosrun drone_control offb_node~ |
|
| 1048 | + |
|
| 1049 | +*** launch 파일을 이용하여 노드 한번에 실행 (옵션) |
|
| 1050 | +roscore, gazebo, mavros, offb_node 노드를 한번에 실행하기 위해서 roslaunch를 이용해보자. |
|
| 1051 | + |
|
| 1052 | +~~/catkin_ws/src/drone_control/launch/offb_node.launch~ 파일에 다음 내용 추가 |
|
| 1053 | +#+begin_src |
|
| 1054 | +<launch> |
|
| 1055 | + <node name="offb_node" pkg="drone_control" type="offb_node"/> |
|
| 1056 | + <include file="$(find px4)/launch/mavros_posix_sitl.launch"> |
|
| 1057 | + <arg name="vehicle" value="iris"/> |
|
| 1058 | + </include> |
|
| 1059 | +</launch> |
|
| 1060 | +#+end_src |
|
| 1061 | + |
|
| 1062 | +~~/.bashrc~ 파일의 끝에 다음 내용을 추가 |
|
| 1063 | +환경 변수 추가 |
|
| 1064 | +#+begin_src |
|
| 1065 | +PX4_SRC_DIR=$HOME/Firmware |
|
| 1066 | +source $PX4_SRC_DIR/Tools/setup_gazebo.bash $PX4_SRC_DIR $PX4_SRC_DIR/build/px4_sitl_default > /dev/null |
|
| 1067 | +export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$PX4_SRC_DIR:$PX4_SRC_DIR/Tools/sitl_gazebo |
|
| 1068 | +#+end_src |
|
| 1069 | + |
|
| 1070 | +#+begin_src sh |
|
| 1071 | +source ~/.bashrc |
|
| 1072 | +catkin build |
|
| 1073 | +#+end_src |
|
| 1074 | + |
|
| 1075 | +#+begin_src sh |
|
| 1076 | +roslaunch drone_control offb_node.launch |
|
| 1077 | +#+end_src |
|
| 1078 | + |
|
| 1079 | + |
|
| 1080 | +** 새로운 노드 만들기: ~circle~ |
|
| 1081 | + |
|
| 1082 | +출처: https://github.com/Jaeyoung-Lim/modudculab_ros/blob/master/src/pub_setpoints_traj.cpp |
|
| 1083 | + |
|
| 1084 | +#+begin_src |
|
| 1085 | +/** |
|
| 1086 | + * @file circle.cpp |
|
| 1087 | + * @brief offboard example node, written with mavros version 0.14.2, px4 flight |
|
| 1088 | + * stack and tested in Gazebo SITL |
|
| 1089 | + */ |
|
| 1090 | + |
|
| 1091 | +#include <ros/ros.h> |
|
| 1092 | +#include <geometry_msgs/PoseStamped.h> |
|
| 1093 | +#include <mavros_msgs/CommandBool.h> |
|
| 1094 | +#include <mavros_msgs/SetMode.h> |
|
| 1095 | +#include <mavros_msgs/State.h> |
|
| 1096 | +#include "math.h" |
|
| 1097 | + |
|
| 1098 | +double r; |
|
| 1099 | +double theta; |
|
| 1100 | +double count=0.0; |
|
| 1101 | +double wn; |
|
| 1102 | + |
|
| 1103 | +mavros_msgs::State current_state; |
|
| 1104 | +void state_cb(const mavros_msgs::State::ConstPtr& msg){ |
|
| 1105 | + current_state = *msg; |
|
| 1106 | +} |
|
| 1107 | + |
|
| 1108 | +int main(int argc, char **argv) |
|
| 1109 | +{ |
|
| 1110 | + ros::init(argc, argv, "circle"); |
|
| 1111 | + ros::NodeHandle nh; |
|
| 1112 | + |
|
| 1113 | + ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State> |
|
| 1114 | + ("mavros/state", 10, state_cb); |
|
| 1115 | + ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped> |
|
| 1116 | + ("mavros/setpoint_position/local", 10); |
|
| 1117 | + ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool> |
|
| 1118 | + ("mavros/cmd/arming"); |
|
| 1119 | + ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode> |
|
| 1120 | + ("mavros/set_mode"); |
|
| 1121 | + |
|
| 1122 | + //the setpoint publishing rate MUST be faster than 2Hz |
|
| 1123 | + ros::Rate rate(20.0); |
|
| 1124 | + |
|
| 1125 | + |
|
| 1126 | + nh.param("pub_setpoints_traj/wn", wn, 1.0); |
|
| 1127 | + nh.param("pub_setpoints_traj/r", r, 1.0); |
|
| 1128 | + // wait for FCU connection |
|
| 1129 | + while(ros::ok() && current_state.connected){ |
|
| 1130 | + ros::spinOnce(); |
|
| 1131 | + rate.sleep(); |
|
| 1132 | + } |
|
| 1133 | + |
|
| 1134 | + geometry_msgs::PoseStamped pose; |
|
| 1135 | + pose.pose.position.x = 0; |
|
| 1136 | + pose.pose.position.y = 0; |
|
| 1137 | + pose.pose.position.z = 2; |
|
| 1138 | + |
|
| 1139 | + //send a few setpoints before starting |
|
| 1140 | + for(int i = 100; ros::ok() && i > 0; --i){ |
|
| 1141 | + local_pos_pub.publish(pose); |
|
| 1142 | + ros::spinOnce(); |
|
| 1143 | + rate.sleep(); |
|
| 1144 | + } |
|
| 1145 | + |
|
| 1146 | + mavros_msgs::SetMode offb_set_mode; |
|
| 1147 | + offb_set_mode.request.custom_mode = "OFFBOARD"; |
|
| 1148 | + |
|
| 1149 | + mavros_msgs::CommandBool arm_cmd; |
|
| 1150 | + arm_cmd.request.value = true; |
|
| 1151 | +j |
|
| 1152 | + ros::Time last_request = ros::Time::now(); |
|
| 1153 | + |
|
| 1154 | + while(ros::ok()){ |
|
| 1155 | + if( current_state.mode != "OFFBOARD" && |
|
| 1156 | + (ros::Time::now() - last_request > ros::Duration(5.0))){ |
|
| 1157 | + if( set_mode_client.call(offb_set_mode) && |
|
| 1158 | + offb_set_mode.response.mode_sent){ |
|
| 1159 | + ROS_INFO("Offboard enabled"); |
|
| 1160 | + } |
|
| 1161 | + last_request = ros::Time::now(); |
|
| 1162 | + } else { |
|
| 1163 | + if( !current_state.armed && |
|
| 1164 | + (ros::Time::now() - last_request > ros::Duration(5.0))){ |
|
| 1165 | + if( arming_client.call(arm_cmd) && |
|
| 1166 | + arm_cmd.response.success){ |
|
| 1167 | + ROS_INFO("Vehicle armed"); |
|
| 1168 | + } |
|
| 1169 | + last_request = ros::Time::now(); |
|
| 1170 | + } |
|
| 1171 | + } |
|
| 1172 | + |
|
| 1173 | + theta = wn*count*0.05; |
|
| 1174 | + |
|
| 1175 | + pose.pose.position.x = r*sin(theta); |
|
| 1176 | + pose.pose.position.y = r*cos(theta); |
|
| 1177 | + pose.pose.position.z = 2; |
|
| 1178 | + |
|
| 1179 | + count++; |
|
| 1180 | + |
|
| 1181 | + local_pos_pub.publish(pose); |
|
| 1182 | + ros::spinOnce(); |
|
| 1183 | + rate.sleep(); |
|
| 1184 | + } |
|
| 1185 | + |
|
| 1186 | + return 0; |
|
| 1187 | +} |
|
| 1188 | +#+end_src |
|
| 1189 | + |
|
| 1190 | + * 실행: ~rosrun drone_control circle~ |
|
| 1191 | +*** 해보기: 원의 너비와 속도를 바꾸어 보자. 힌트 (wn, r) |
|
| 1192 | + |
|
| 1193 | +** 과제: 키보드로 OFFBOARD 모드 제어하기 |
|
| 1194 | + - ~offb_node~ 코드를 참고하여, 키보드로 x,y,z 위치를 제어하여 보자. |
|
| 1195 | + - 참고: http://wiki.ros.org/teleop_twist_keyboard_cpp |
|
| 1196 | + |
|
| 1197 | +** 참고 |
|
| 1198 | + - https://github.com/mavlink/mavros/tree/master/mavros |
|
| 1199 | + - https://github.com/Jaeyoung-Lim/modudculab_ros/blob/master/src/pub_setpoints_traj.cpp |
