# Drone Simulation with PX4 and Mavros

PX4 flight modes
https://docs.px4.io/main/en/getting_started/flight_modes.html

## MAVROS

**NOTE**: Follow all the steps for successful installation
First the binary installation and then source installation needs to be done.

Video Reference is [this](https://www.youtube.com/watch?v=jBTikChu02E&list=PLYy2pGCdhu7xEaNN8krzAKxv74L1mD4OV&index=5)

### BINARY INSTALLATION

>
sudo apt-get install ros-noetic-mavros ros-noetic-mavros-extras ros-noetic-mavros-msgs

>
wget https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh

`sudo bash ./install_geographiclib_datasets.sh`

### SOURCE INSTALLATION

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

Create a new catkin workspace outside your original one.
`mkdir catkin_ws_2`
`cd ~/catkin_ws_2`
`catkin init`
`wstool init src`
`sudo apt-get install python-catkin-tools python-rosinstall-generator -y`

`rosinstall_generator --rosdistro noetic mavlink | tee /tmp/mavros.rosinstall`

`rosinstall_generator --upstream mavros | tee -a /tmp/mavros.rosinstall`

`wstool merge -t src /tmp/mavros.rosinstall`
`wstool update -t src -j4`

After running the above command, if any error occurs which says update failed to some package/folder, then remove the respective folder from workspace, go to [ROS repos](http://repositories.ros.org/status_page/ros_kinetic_default.html?q=mavlink) and clone the latest repos of those packages.

`rosdep install --from-paths src --ignore-src -y`

At this moment if an error related to something version, is encountered then refer [this](https://discuss.px4.io/t/package-version-version-does-not-follow-version-conventions/30520)

`sudo ./src/mavros/mavros/scripts/install_geographiclib_datasets.sh`

`catkin build`

### PX4 Environment Setup

Reference [Video](https://www.youtube.com/watch?v=OtValQdAdrU&list=PLYy2pGCdhu7xEaNN8krzAKxv74L1mD4OV&index=4)

Steps -

Clone the repo outside your catkin workspace
`git clone https://github.com/PX4/Firmware.git --recursive`

`cd Firmware/`
`bash ./Tools/setup/ubuntu.sh`

`sudo reboot now`
`make px4_sitl gazebo-classic`

### MAVSDK Installation

>
wget https://github.com/mavlink/MAVSDK/releases/download/v0.37.0/mavsdk_0.37.0_ubuntu20.04_amd64.deb

`sudo dpkg -i mavsdk_0.37.0_ubuntu20.04_amd64.deb`

Install **QGroundControl** from [here](https://docs.qgroundcontrol.com/master/en/getting_started/download_and_install.html)

`sudo apt-get update`
`sudo apt-get install build-essential cmake git`

Clone this repo in the catkin_ws containing mavros
`cd catkin_ws_2/src`
`git clone https://github.com/mavlink/MAVSDK.git`
`git submodule update --init --recursive`
`cd MAVSDK`

`cmake -DCMAKE_BUILD_TYPE=Debug -Bbuild/default -H.`
`cmake --build build/default -j8`
`cmake -Bbuild/default -DCMAKE_BUILD_TYPE=Release -H.`

`cmake --build build/default -j8`

`sudo cmake --build build/default --target install`

#### Run an example mission in Gazebo
In a new terminal
`cd Firmware`
`make px4_sitl gazebo-classic`

In another terminal, run QGroundControl
`./QGroundControl.AppImage`

Now in a third terminal
`cd catkin_ws_2/src/MAVSDK/examples/fly_mission`
`cmake -Bbuild -H.`
`cmake --build build -j4`
`build/fly_mission udp://:14540`

### MAVSDK-Python Setup
`sudo apt update`
`pip3 install mavsdk`

Clone this repo in the directory where you have installed MAVSDK initially
`git clone https://github.com/mavlink/MAVSDK-Python.git`

#### Run an example mission
In a new terminal
`cd Firmware`
`make px4_sitl gazebo-classic`

In another terminal, run QGroundControl
`./QGroundControl.AppImage`

In third terminal
`cd catkin_ws_2/src/MAVSDK-Python/examples/`
`python mission.py`

### Launching Gazebo classic with ROS
Edit the .bashrc file for configuring the px4 environment
`gedit .bashrc`
Add these lines to the end of the script
`px4_dir=~/Firmware`

>
source $px4_dir/Tools/simulation/gazebo-classic/setup_gazebo.bash $px4_dir $px4_dir/build/px4_sitl_default

`export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$px4_dir`

`export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$px4_dir/Tools/sitl_gazebo`
>
export GAZEBO_PLUGIN_PATH=$GAZEBO_PLUGIN_PATH:/usr/lib/x86_64-linux-gnu/gazebo-11/plugins

If it gives error related to some gazebo or ros plugin, then
`sudo apt-get install -y libgazebo11-dev`

Clone [this](https://github.com/ros-simulation/gazebo_ros_pkgs/tree/noetic-devel) repo in your catkin_ws, do catkin_make and then replace the gazebo plugin path in the last line with the path of the gazebo_plugins folder of this repo.

>
GAZEBO_PLUGIN_PATH=$GAZEBO_PLUGIN_PATH:~/catkin_ws/build/gazebo_ros_pkgs/gazebo_plugins

### Simulating custom drone in Gazebo

There are certain steps to follow to successfully simulate your custom drone.
You can refer [this](https://discuss.px4.io/t/create-custom-model-for-sitl/6700/3)

Steps -
* Go to `~/Firmware/Tools/simulation/gazebo-classic/sitl_gazebo-classic/models` and create a new folder of name "my_vehicle" where you give any name of your choice
* Add .sdf files and model.config files similar to whats there in iris model folder or any other model in that directory. If you have the urdf or xacro file of your vehicle then convert it into sdf file using the steps mentioned later in this doc [here](https://hackmd.io/VSUxe3WqR2uYY4R7AL8zRg#Spawning-turtlebot-in-px4-gazebo-environment). It is mostly recommended that you copy paste the iris.sdf from iris model and then make small small changes to your sdf file after simulating as at first instance your custom model can fail to load due to various unknown reasons(Please do change the name of your model in your sdf file after copy pasting). Please keep the model name, name of your sdf file and your model's folder name same.
* Create your own world file and save it in the worlds folder of the present directory. This file should also be similar to other world files in worlds folder.
* Next go to `~/Firmware/ROMFS/px4fmu_common/init.d-posix` and in the airframes folder, create your airframe file. You have to name it this way ""[Take any 4-6 digit number not used for any other file in that folder]_gazebo-classic_my_vehicle", for eg - 1014_gazebo-classic_iris_advanced. Note - after gazebo classic enter the name which you have used in the sdf file of your model previously. Again this airframe file should be based on the iris or iris_vision airframe files(so first copy paste and then add or remove or change the parameters in your file). Now open the CMakeLists.txt file in that airframes folder and add your airframe file name in the list. Also, go to `~/Firmware/build/px4_sitl_default/rootfs/etc/init.d-posix/airframes` and `~/Firmware/build/px4_sitl_default/etc/init.d-posix/airframes` these destinations and check whether your airframe file is there or not(generally it gets build there automatically). If not then paste your file there also.
* Next go to `~/Firmware/src/modules/simulation/simulator_mavlink` and open sitl_targets_gazebo-classic. Find the "set(models......)" command and add the model name of your vehicle in that list. **NOTE**: If at some point you are unable to find any specific folder mentioned in these steps then open file manager and in the Firmware search that folder in the search bar(PX4 keeps updating its repo so the location of some folders may change from the time this doc is written).
* Open a new terminal -
`cd Firmware`
`make clean`
`make px4_sitl gazebo-classic_my_vehicle`

### Attaching camera to the iris drone in px4 gazebo environment
You can refer to [this](https://discuss.px4.io/t/how-to-add-a-ros-camera-to-iris-for-gazebo-simulation/5118) or [this](https://discuss.px4.io/t/adding-a-camera-to-iris-base-model/17719) for better understanding

Or just follow these steps -

`cd ~/Firmware/Tools/simulation/gazebo-classic/sitl_gazebo-classic/models/iris`

Open iris.sdf.jinja file and above imu_link add these lines
>
<link name="down_camera_link">
<pose>0 0 -0.013633 1.5708 1.5708 0</pose>
<inertial>
<pose>0 0 0 0 0 0</pose>
<mass>0.050075</mass>
<inertia>
<ixx>3.2061e-05</ixx>
<ixy>-1.169e-08</ixy>
<ixz>-4.2601e-10</ixz>
<iyy>3.0815e-05</iyy>
<iyz>5.2182e-10</iyz>
<izz>5.1652e-06</izz>
</inertia>
</inertial>
<collision name='down_camera_collision'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<box>
<size>0.023813 0.027686 0.092682</size>
</box>
</geometry>
<surface>
<friction>
<ode>
<mu>1</mu>
<mu2>1</mu2>
</ode>
</friction>
<contact>
<ode>
<kp>1e+8</kp>
<kd>1</kd>
<max_vel>0.01</max_vel>
<min_depth>0.001</min_depth>
</ode>
</contact>
</surface>
</collision>
<visual name='down_camera_visual'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://iris/meshes/cam_link.STL</uri>
</mesh>
</geometry>
<material>
<script>
<name>Gazebo/DarkGrey</name>
<uri>file://media/materials/scripts/gazebo.material</uri>
</script>
</material>
</visual>
<gravity>1</gravity>
<velocity_decay/>
<self_collide>0</self_collide>
<sensor name="down_camera_imu" type="imu">
<always_on>1</always_on>
</sensor>
<sensor name="down_camera" type="camera"> <!-- name of sensor topic as well -->
<pose>0 0 0 -1.5708 0 0</pose>
<camera>
<horizontal_fov>2.0</horizontal_fov>
<image>
<format>R8G8B8</format>
<width>640</width>
<height>480</height>
</image>
<clip>
<near>0.05</near>
<far>15000</far>
</clip>
</camera>
<always_on>1</always_on>
<update_rate>10</update_rate>
<visualize>true</visualize>
<plugin filename="libgazebo_ros_camera.so" name="camera_en_ros">
<robotNamespace>iris</robotNamespace>
<cameraName>down_camera_link</cameraName>
<imageTopicName>down_raw_image</imageTopicName>
<cameraInfoTopicName>down_info_image</cameraInfoTopicName>
<FrameName>downCameraImage</FrameName>
<Cy>0</Cy>
<Cx>0</Cx>
<updateRate>30.0</updateRate>
</plugin>
<plugin name="GstCameraPlugin" filename="libgazebo_gst_camera_plugin.so">
<robotNamespace></robotNamespace>
<udpPort>5600</udpPort>
</plugin>
<plugin name="GeotaggedImagesPlugin" filename="libgazebo_geotagged_images_plugin.so">
<robotNamespace>iris</robotNamespace>
<interval>1</interval>
<width>3840</width>
<height>2160</height>
<maximum_zoom>8.0</maximum_zoom>
</plugin>
</sensor>
</link>
<joint name='down_camera_joint' type='fixed'>
<child>down_camera_link</child>
<parent>base_link</parent>
<pose>0 0 0 0 0 0</pose>
<axis>
<xyz>-1 0 0</xyz>
</axis>
</joint>

Save the file and then in a new terminal
`cd Firmware`
`DONT_RUN=1 make px4_sitl gazebo`
`roslaunch px4 mavros_posix_sitl.launch`

Then open rviz to see the image captured
`rosrun rviz rviz`

In rviz, Add the image display type and then in the image drop down, select image topic as iris/down_camera_link

If the image is not shown then it may be likely due to the transformation error. For that
>
rosrun tf static_transform_publisher 0 0 0 0 0 0 base_link /iris_1/camera_red_iris_link 50

### Spawning turtlebot in px4 gazebo environment
First go to turtlebot3_description in turtlebot3 package and find a file in urdf folder named "turtlebot3_burger.urdf.xacro". If not present then download from [here](https://github.com/ROBOTIS-GIT/turtlebot3/tree/master/turtlebot3_description/urdf)

We need to convert this xacro file to urdf file and then to sdf file
Open a terminal at that location and -
`xacro turtlebot3_burger.urdf.xacro > turtlebot3_burger.urdf`
`gz sdf -p turtlebot3_burger.urdf > turtlebot3_burger.sdf`

You will see the urdf and sdf files being created in the folder.

`roslaunch px4 mavros_posix_sitl.launch`

Now in a new terminal -

>
rosrun gazebo_ros spawn_model -file `rospack find turtlebot3_description`/urdf/turtlebot3_burger.urdf -urdf -x 0 -y 0 -z 0 -model turtlebot

You can also add this to your launch file to directly spawn your turtlebot with the drone.

>
<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-file $(find turtlebot3_description)/urdf/turtlebot3_burger.urdf -urdf -x 5 -y 5 -z 0 -model turtlebot" />

Make sure you have a turtlebot3_description package in your catkin_ws. If not there then clone [this](https://github.com/ROBOTIS-GIT/turtlebot3) repo in your catkin_ws and then do catkin_make.