01.3 전체 진행과정_Nav전까지 - pineland/px4 GitHub Wiki

mkdir ~/src; cd ~/src   
git clone https://github.com/PX4/Firmware.git  
cd Firmware  
git submodule update --init --recursive  <- 확인 필요  
make posix_sitl_default gazebo  <- 에러 발생 시, 위의 git clone 다시 실행 후, 실행`  

cd ~/catkin_ws/src  
git clone https://github.com/Jaeyoung-Lim/modudculab_ros.git  
catkin build modudculab_ros  <- 에러 발생 시, 에러 해결 노하우 위키 확인`  

A terminal : roscore  
B terminal : cd ~/src/Firmware && make posix_sitl_default gazebo  
C terminal : roslaunch modudculab_ros ctrl_pos_gazebo.launch <- 에러 발생 시, 에러 해결 노하우 위키 확인  
D terminal : rosrun mavros mavsafety arm && rosrun mavros mavsys mode -c OFFBOARD  

• 출처 : https://docs.qgroundcontrol.com/en/getting_started/download_and_install.html

sudo apt-get install espeak libespeak-dev libudev-dev libsdl2-dev  

tar jxf QGroundControl.tar.bz2  
cd qgroundcontrol  
./qgroundcontrol-start.sh   
cd Firmware_clone
git submodule update --init --recursive
make posix_sitl_default gazebo
source your environment,
source Tools/setup_gazebo.bash $(pwd) $(pwd)/build/posix_sitl_default
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$(pwd)
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$(pwd)/Tools/sitl_gazebo
run launch file,

roscore  

cd ~/src/Firmware  
git submodule update --init --recursive  
;(생략) make posix_sitl_default gazebo  
; source your environment  
source Tools/setup_gazebo.bash $(pwd) $(pwd)/build/posix_sitl_default  
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$(pwd)  
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$(pwd)/Tools/sitl_gazebo  
; run launch file  
roslaunch px4 multi_uav_mavros_sitl.launch   <- two vehicles appear, 에러 발생 시, 에러 해결 노하우 위키 확인

save in the mavros directory(~/catkin_ws/src/mavros/mavros/src)

; add the following sentences to CMake file(~/catkin_ws/src/mavros/mavros/CMakeLists.txt)
add_executable(pub_setpoints_pos src/pub_setpoints_pos.cpp)
target_link_libraries(pub_setpoints_pos ${catkin_LIBRARIES})
add_dependencies(pub_setpoints_pos mavros)
~/src/catkin_ws/catkin build
; you can find the execution file in the directory(~/catkin_ws/devel/lib/mavros)

9.4 Change the launch file and execute simulation

• add the following senstences to the launch file(~/catkin_ws/src/mavros/mavros/launch/node.launch)
  <node name="offb_node" pkg="mavros" type="offb_node" output="screen"> </node>
• execute simulation
  1 terminal : roscore
  2 terminal :
  cd ~/src/Firmware
  source Tools/setup_gazebo.bash $(pwd) $(pwd)/build/posix_sitl_default
  export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$(pwd)
  export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$(pwd)/Tools/sitl_gazebo
  roslaunch px4 multi_uav_mavros_sitl.launch

• gazebo의 센서추가 사이트(http://gazebosim.org/tutorials?tut=add_laser) 참조

10.1 iris 모델에 camera 모듈 추가 vi ~/src/Firmware/Tools/sitl_gazebo/models/iris/iris.sdf
맨 아래쪽 </model> 위에 다음의 내용을 추가한다.
<include>
<uri>model://flow_cam</uri>
<pose>-0.12 0.05 0 0 1.5708 1.5708</pose>
</include>
<joint name="flow_joint" type="revolute">
<child>camera::link</child>
<parent>iris::base_link</parent>
<axis>
<xyz>0 0 1</xyz>
<limit>
<upper>0</upper>
<lower>0</lower>
</limit>
</axis>
</joint>

(해설1) 센서의 모델 이름을 적어준다.(센서의 sdf파일 안에 모델 이름이 들어있다)
model://flow_cam
(해설2) 센서를 붙일 위치를 정한다.
-0.12 0.05 0 0 1.5708 1.5708
(해설3) joint의 이름을 정해주고 (이건 자기 마음대로)

(해설4) child와 parent를 정의해주는데 link와 link를 joint로 연결한 것으므로 각각 옵티컬 플로우 센서의
링크와 iris 드론의 베이스 링크(아마도 드론의 밑면이지 싶다)를 적어준다.
<child>camera::link</child>
<parent>iris::base_link</parent>
※ 아래에 make가 아닌 launch 파일을 이용하여 ros를 작동시키는 경우는 urdf나 다른 파일을 변경한다.
-> launch 파일을 이용하는 경우, sdf 파일 변경사항이 반영이 안됨.

10.2 실행
//Terminal 1
roscore

//Terminal 2
cd ~/src/Firmware
no_sim=1 make posix_sitl_default gazebo

//Terminal 3
export GAZEBO_MODEL_PATH=${GAZEBO_MODEL_PATH}:~/src/Firmware/Tools/sitl_gazebo/models
export GAZEBO_PLUGIN_PATH=${GAZEBO_PLUGIN_PATH}:~/src/Firmware/Tools/sitl_gazebo/Build
roslaunch gazebo_ros empty_world.launch

(해설1) no_sim=1을 앞에 붙여야 기존에 사용하던 mavros 패키지 안에 있는 topic들 말고
다른 정보들을 gazebo에서 토픽으로 받아볼 수 있다.
이렇게 하면 gazebo가 바로 실행되지는 않는다.
(해설2) 환경변수(plugin/model)에 sitl_gazebo 폴더를 포함
(해설3) default로 empty world를 부르고 gazebo 안에서 모델을 불러올 것이다.

10.3 gazebo 내에서 드론 불러오기
gazebo 왼쪽 insert 항목에서 /home/youngsoo/src/Firmware ...에 추가해준 model들이 있다.
(이 부분이 없다면 환경설정이 안 된 것이므로 그 부분을 다시 실행하고 가제보를 실행하길!)
클릭하여 Scene으로 긁어오면 된다. Box나 다른 모델들도 불러와도 된다.

10.4 Iris에게 명령주기
Iris 드론을 import 한 순간 px4와 연결이 되므로 Takeoff명령을 줄 수가 있다.
no_sim=1 make posix_sitl_default gazebo를 실행시킨 터미널 창에서 쉘이 뜬다.
pxh>commander takeoff

이 때 쉘이 나오지 않는 경우는 아래를 실행시켜준다.
cd ~/src/Firmware/Tools/sitl_gazebo/Build
cmake ..
make

10.5 iris 모델에 small camera 모듈 추가
위의 카메라는 너무 커서 iris가 중심을 잘 못잡기 때문에 iris_optical_flow를 변형해서 다음과 같이
iris_cameraSmallBox.sdf를 작성하고 model.config에서 iris_cameraSmallBox.sdf를 불러준다.
iris.sdf의 맨 아래쪽 위에 다음의 내용을 추가한 후, iris_cameraSmallBox.sdf로 저장한다.
(이때, sdf 안의 모델명도 파일명과 통일시켜준다.)
<link name='camera_ys'>
<pose frame=''>0.1 0 0 0 0 0</pose>
<inertial>
<pose frame=''>0.01 0.025 0.025 0 -0 0</pose>
<mass>0.01</mass>
<inertia>
<ixx>4.15e-06</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>2.407e-06</iyy>
<iyz>0</iyz>
<izz>2.407e-06</izz>
</inertia>
</inertial>
<self_collide>0</self_collide>
<kinematic>0</kinematic>
<sensor name='PX4Flow' type='camera'>
<update_rate>10</update_rate>
<camera name='__default__'>
<horizontal_fov>0.6</horizontal_fov>
<image>
<width>256</width>
<height>256</height>
</image>
<clip>
<near>0.1</near>
<far>100</far>
</clip>
</camera>
</sensor>
<visual name='visual'>
<geometry>
<box>
<size>0.026012 0.029339 0.024291</size>
</box>
</geometry>
<pose frame=''>0 0 0 0 -0 0</pose>
<cast_shadows>1</cast_shadows>
<transparency>0</transparency>
</visual>
</link>

<joint name="camera_ys_joint" type="revolute">
<child>camera_ys</child>
<parent>base_link</parent>
<axis>
<xyz>0 0 1</xyz>
<limit>
<upper>0</upper>
<lower>0</lower>
</limit>
</axis>
</joint>

10.4 카메라 데이터를 gazebo 안에서 띄우기
드론에 달려있는 카메라는 gazebo 내에서 topic으로 발행되고 있다.
topic은 gazebo 내에서 ctrl+t를 누르면 볼 수 있다.
발행되고 있는 topic 중에서 /gazebo/default/iris/camera/link/camera/image을
더블 클릭 해 보면 가제보 내에서 카메라 화면을 띄울 수 있다.
Insert항목에서 House와 Asphalt plane 등을 클릭하여 원하는 위치에 설치해주면
다양한 화면을 볼 수 있다.

10.5 gazebo topic으로부터 카메라 데이터 받아오기
아래의 명령을 통해 현재 가제보 내에서 실행되고 있는 토픽들을 볼 수 있다.
gz topic -l

이 중에서 관심이 있는 토픽의 데이터를 받아보려면 아래와 같이 입력해주면 된다.
gz topic -e /gazebo/default/iris/camera/link/camera/image
-> 이미지 데이터는 픽셀 값들이 흘러 나타난다.

10.6 gazebo topic(카메라 데이터)을 ROS topic으로 받아오기
gazebo에서 발행되는 topic을 ROS topic으로 받아오려면 해당되는 플러그이 있어야 한다.
http://gazebosim.org/tutorials?tut=ros_gzplugins

여기서 플러그인을 추가하려면 sdf 파일에 다음과 같이 플러그인을 추가하는 코드를 넣어야 한다.
iris.cameraSmallBox을 열어 </camera> 밑에 아래의 내용을 추가한다.

<plugin name="camera_controller" filename="libgazebo_ros_camera.so">
<alwaysOn>true</alwaysOn>
<updateRate>0.0</updateRate>
<cameraName>rrbot/camera1</cameraName>
<imageTopicName>image_raw</imageTopicName>
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
<frameName>camera_link</frameName>
<hackBaseline>0.07</hackBaseline>
<distortionK1>0.0</distortionK1>
<distortionK2>0.0</distortionK2>
<distortionK3>0.0</distortionK3>
<distortionT1>0.0</distortionT1>
<distortionT2>0.0</distortionT2>
</plugin>

여기서 ROS topic으로 발행이 되고 있는 이미지 파일을 볼 수 있는 ros node가 있다.
다음과 같이 실행시켜주면 이미지를 창으로 볼 수 있다.

rosrun image_view image_view image:=/iris_cameraSmallBoxWithPlugin/flow_camera/image_raw

이때, iris_cameraSmallBoxWithPlugin은 모델명이고, flow_camera는 plugin에서 명시해준 cameraName,
image_raw는 imageTopicName이다. sdf파일명과 모델명은 가급적 iris_cameraSmallBoxWithPlugin으로 통일시켜준다.