9984e0157be44cc597a518f1cd590e8beb09990e
ros.org
| ... | ... | @@ -1,1199 +0,0 @@ |
| 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 |
