SITL + ArduPilot + Gazebo + ROS Camera Plugin (Software In Loop Simulation Interfaces, Models)
Finally an all in one tutorial for setting up your virtual drone using Ardupilot (Arducopter) + SITL in an complete 3D virtual enviroment provided by Gazebo.
At last but not least (Actually the most complicated to find examples of...) added the camera plugin from ROS to publish the images so you can take them outside Gazebo and do something with them...
Following are the requirements, setup steps and finally how to of each part..
- Ubuntu (18.04 LTS) Full 3D graphics hight recommended.
- Gazebo version 8.x or greater (Recomended 9.6)
- ROS melodic (Required to work with Gazebo)
- MAVROS
sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list'
wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add -
sudo apt-get update
sudo apt-get install gazebo9
sudo apt-get install libgazebo9-dev
gazebo --verbose
NOTE: When running in VM mode this might be required for gazebo to run
export SVGA_VGPU10=0
To make the change permanent use:
$ echo "export SVGA_VGPU10=0" >> ~/.profile
Install ROS with sudo apt install ros-melodic-desktop-full (follow instruction here http://wiki.ros.org/melodic/Installation/Ubuntu).
sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
sudo apt-key adv --keyserver hkp://ha.pool.sks-keyservers.net:80 --recv-key 421C365BD9FF1F717815A3895523BAEEB01FA116
sudo apt update
sudo apt install ros-melodic-desktop-full
sudo rosdep init
rosdep update
echo "source /opt/ros/melodic/setup.bash" >> ~/.bashrc
source ~/.bashrc
sudo apt install python-rosinstall python-rosinstall-generator python-wstool build-essential
MAVLink extendable communication node for ROS with proxy for Ground Control Station (See original instructions here http://ardupilot.org/dev/docs/ros-install.html#installing-mavros).
sudo apt-get install ros-melodic-mavros ros-melodic-mavros-extras
cd ~/
wget https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh
chmod a+x install_geographiclib_datasets.sh
./install_geographiclib_datasets.sh
sudo apt-get install ros-melodic-rqt ros-melodic-rqt-common-plugins ros-melodic-rqt-robot-plugins
sudo apt-get install python-catkin-tools
Now that we have everything correctly installed we can begin our system configuration
- STEP 1 - SITL Ardupilot
- STEP 2 - Ardupilot gazebo plugin (Original khancyr version)
- STEP 3 - Gazebo ROS plugin (roscam)
- STEP 4 - Connect ArduPilot to ROS
Instructions taken from ardupilot.org (See original instructions here http://ardupilot.org/dev/docs/setting-up-sitl-on-linux.html).
cd ~/
git clone https://github.com/r0ch1n/ardupilot
cd ardupilot
git submodule update --init --recursive
If you are on a debian based system (such as Ubuntu or Mint), we provide a script that will do it for you. From ardupilot directory :
Tools/scripts/install-prereqs-ubuntu.sh -y
Reload the path (log-out and log-in to make permanent):
. ~/.profile
To start the simulator first change directory to the vehicle directory. For example, for the multicopter code change to ardupilot/ArduCopter:
cd ~/ardupilot/ArduCopter
Then start the simulator using sim_vehicle.py. The first time you run it you should use the -w option to wipe the virtual EEPROM and load the right default parameters for your vehicle.
sim_vehicle.py --console --map
New versions of MAVProxy and pymavlink are released quite regularly. If you are a regular SITL user you should update every now and again using this command
sudo pip install --upgrade pymavlink MAVProxy
This concludes the first step SITL Ardupilot installation.
(See original instructions here https://github.com/khancyr/ardupilot_gazebo).
cd ~/
git clone https://github.com/r0ch1n/ardupilot_gazebo
cd ardupilot_gazebo
mkdir build
cd build
cmake ..
# use make without any parameter if running in a VM
make -j4
sudo make install
echo 'source /usr/share/gazebo/setup.sh' >> ~/.bashrc
Set Path of Gazebo Models (Adapt the path to where to clone the repo)
echo 'export GAZEBO_MODEL_PATH=~/ardupilot_gazebo/models' >> ~/.bashrc
Set Path of Gazebo Worlds (Adapt the path to where to clone the repo)
echo 'export GAZEBO_RESOURCE_PATH=~/ardupilot_gazebo/worlds:${GAZEBO_RESOURCE_PATH}' >> ~/.bashrc
Reload the path (log-out and log-in to make permanent):
source ~/.bashrc
Open one Terminal and launch SITL Ardupilot
sim_vehicle.py -v ArduCopter -f gazebo-iris --map --console
Open a second Terminal and launch Gazebo running ardupilot_gazebo plugin
gazebo --verbose worlds/iris_arducopter_runway.world
You should see a gazebo world with a small quadcopter right at the center
This contains the ROS integrated custom models and .world files for Gazebo
# Source ROS
source /opt/ros/melodic/setup.bash
# Clone custom Gazebo ROS package
cd ~/
git clone https://github.com/r0ch1n/ardupilot_gazebo_roscam
cd ardupilot_gazebo_roscam
catkin init
cd src
catkin_create_pkg ardupilot_gazebo
cd ..
catkin build
# Add Custom models and plugin to Gazebo
export GAZEBO_MODEL_PATH=~/ardupilot_gazebo/models:$GAZEBO_MODEL_PATH
export GAZEBO_MODEL_PATH=~/ardupilot_gazebo_roscam/src/ardupilot_gazebo/models:$GAZEBO_MODEL_PATH
export GAZEBO_PLUGIN_PATH=/usr/lib/x86_64-linux-gnu/gazebo-9/plugins:$GAZEBO_PLUGIN_PATH
export GAZEBO_PLUGIN_PATH=/opt/ros/melodic/lib:$GAZEBO_PLUGIN_PATH
# Test installation
source ~/ardupilot_gazebo_roscam/devel/setup.bash
roslaunch ardupilot_gazebo iris_with_roscam.launch
Connect to Ardupilot from ROS (Ardupilot <–> MAVLink <–> ROS ) Original information taken from here http://ardupilot.org/dev/docs/ros-sitl.html Note - Gazebo is not included in MAVROS so you cannot connect or access any of the Gazebo's Environments.
New versions of MAVProxy and pymavlink are released quite regularly. If you are a regular SITL user you should update every now and again using this command
cd ~/
mkdir -p ardupilot_ws/src
cd ardupilot_ws
catkin init
cd src
mkdir launch
cd launch
roscp mavros apm.launch apm.launch
sudo gedit apm.launch
To connect to SITL we just need to modify the first line to <arg name="fcu_url" default="udp://127.0.0.1:14551@14555" />. save you file and launch it with
cd ~/ardupilot_ws/src/launch
roslaunch apm.launch
Open one Terminal and launch ROS integrated Gazebo
#Make sure you have all the right environment, if you are not sure run the following first
source /opt/ros/melodic/setup.bash
export GAZEBO_MODEL_PATH=~/ardupilot_gazebo/models:$GAZEBO_MODEL_PATH
export GAZEBO_MODEL_PATH=~/ardupilot_gazebo_roscam/src/ardupilot_gazebo/models:$GAZEBO_MODEL_PATH
export GAZEBO_PLUGIN_PATH=/usr/lib/x86_64-linux-gnu/gazebo-9/plugins:$GAZEBO_PLUGIN_PATH
export GAZEBO_PLUGIN_PATH=/opt/ros/melodic/lib:$GAZEBO_PLUGIN_PATH
#Launch ROS integrated Gazebo
source ~/ardupilot_gazebo_roscam/devel/setup.bash
roslaunch ardupilot_gazebo iris_with_roscam.launch
Open a second Terminal and launch SITL Ardupilot
cd ~/ardupilot/ArduCopter
sim_vehicle.py -f gazebo-iris --console --map
Open a third Terminal and RTL
rqt
Select Plugins -> Visualization -> Image View
Then choose /roscam/cam/image_raw
You should see the live feed from inside gazebo
You can use any GCS Adrupilot software to control the UAV.
I hope you can use this basic template for your own projects