Simulating a drone in Gazebo

I wanted to test a certain thing in simulation and so I thought I'd prepare a drone model in Gazebo along with a simple autopilot. There is of course multiple existing frameworks (for example PixHawk already has Gazebo simulation: https://dev.px4.io/v1.9.0/en/simulation/gazebo.html), but I wanted something lightweight, and pretty much enjoy creating something from scratch. I'll describe the project in a series of posts. Today, we will build a simple URDF model of a hexacopter (see Fig. 1) and prepare the ROS package for further development.

Fig. 1. Simple drone model.

I've used the following software versions:

• Ubuntu 16.04.3 LTS
• ROS Kinetic 1.12.14
• Gazebo 7.16.0
• Python 2.7.12

... but it will probably work on the newer versions as well (let me know if anything needs updating!).

You can find the code for the whole tutorial on Gitlab HERE

1. Creating the package

Let's start with the package structure. Assuming you already have your catkin workspace configured (furthermore assuming it's located in ~/catkin_ws), let's create the package:

cd ~/catkin_ws/src
catkin_create_pkg kair_drone roscpp rospy std_msgs geometry_msgs sensor_msgs tf message_generation

This takes care of several dependencies we may use. We will add more of those later. Next, let's create a few directories:

cd drone
mkdir urdf launch rviz world

2. Creating the RVIZ launch file

When building an URDF model in a ROS package, it's usually a good idea to start with a launch file that will run RVIZ and upload the model to be visualized for you. Let's create the following rviz.launch file in the launch directory:

<launch>
  <!-- this defines the path to the URDF model -->
  <arg name="model" default="$(find kair_drone)/urdf/drone.urdf.xacro"/>
  <!-- this is the path to the rviz configuration file -->
  <arg name="rvizconfig" default="$(find kair_drone)/rviz/drone.rviz" />

  <!-- load the drone description onto the ROS parameter server -->
  <param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)"/>


  <!-- launch joint_state_publisher to publish fake joint states -->
  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher"/>


  <!-- launch robot_state_publisher to publish link positions -->
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher"/>
   <!-- launch rviz -->
  <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true"/>
</launch>



The model file and the rviz configuration file don't yet exist, so we will have to create those before launching RVIZ.

3. Building the hexacopter model

I opted for building the hexacopter model out of geometric primitives rather than molding the whole mesh in something like Blender. Since the hexacopter is highly symmetrical and has several repeating parts (arms, propellers), it's advisable to use XACRO to build the model. The listings below are a part of the drone.urdf.xacro file, which should be placed in the urdf directory of the package.

First, let us define some properties. You can change these if you wish to modify the drone overall shape.

<?xml version="1.0"?>
<robot xmlns:xacro="http://wiki.ros.org/xacro" name="drone">
 
  <!-- properties -->
  <xacro:property name="frame_radius" value="0.1"/>
  <xacro:property name="frame_height" value="0.05"/>
  <xacro:property name="frame_mass" value="0.88"/>
  <xacro:property name="arm_radius" value="0.01"/>
  <xacro:property name="arm_length" value="0.15"/>
  <xacro:property name="arm_mass" value="0.01"/>
  <xacro:property name="propeller_radius" value="0.1"/>
  <xacro:property name="propeller_height" value="0.01"/>
  <xacro:property name="propeller_height_offset" value="0.025"/>
  <xacro:property name="propeller_mass" value="0.01"/>

Next, we define a macro for calculating the inertia matrix for cylinder shapes (the whole model will be built out of cylinders).

   <xacro:macro name="cylinder_inertial" params="radius height mass *origin">
    <inertial>
      <mass value="${mass}"/>
      <xacro:insert_block name="origin"/>
      <inertia
        ixx="${0.0833333 * mass * (3 * radius * radius + height * height)}"
        ixy="0.0"
        ixz="0.0"
        iyy="${0.0833333 * mass * (3 * radius * radius + height * height)}"
        iyz="0.0"
        izz="${0.5 * mass * radius * radius}"
      />
    </inertial>
  </xacro:macro>

Since the model contains six arms, let us also create a macro for each of the arms. Note that we use expressions to calculate the positions of the arms around the drone frame. If you'd like your model to have fewer or more arms, you would have to modify these. The macro defines the arm link and the joint linking it to the frame. It also contains a gazebo reference tag.

  <xacro:macro name="arm" params="i">
    <link name="arm${i}">
      <visual>
        <origin xyz="${cos((i-1)*pi/3+pi/6)*(frame_radius+arm_length/2)} ${sin((i-1)*pi/3+pi/6)*(frame_radius+arm_length/2)} ${frame_height/2-arm_radius}" rpy="0 ${pi/2} ${(i-1)*pi/3+pi/6}"/>
        <geometry>
          <cylinder radius="${arm_radius}" length="${arm_length}"/>
        </geometry>
        <material name="arm_material"/>
      </visual>
      <collision>
        <origin xyz="${cos((i-1)*pi/3+pi/6)*(frame_radius+arm_length/2)} ${sin((i-1)*pi/3+pi/6)*(frame_radius+arm_length/2)} ${frame_height/2-arm_radius}" rpy="0 ${pi/2} ${(i-1)*pi/3+pi/6}"/>
        <geometry>
          <cylinder radius="${arm_radius}" length="${arm_length}"/>
        </geometry>
      </collision>
      <xacro:cylinder_inertial radius="${arm_radius}" height="${arm_length}" mass="${arm_mass}">
        <origin xyz="${cos((i-1)*pi/3+pi/6)*(frame_radius+arm_length/2)} ${sin((i-1)*pi/3+pi/6)*(frame_radius+arm_length/2)} ${frame_height/2-arm_radius}" rpy="0 ${pi/2} ${(i-1)*pi/3+pi/6}"/>
      </xacro:cylinder_inertial>
    </link>
   
    <joint name="frame_arm${i}" type="fixed">
      <parent link="frame"/>
      <child link="arm${i}"/>
    </joint>
   
    <gazebo reference="arm${i}">
      <material>Gazebo/Grey</material>
    </gazebo>
  </xacro:macro>

Next, we will define a similar macro for each of the six propellers. Here, the macro takes, in addition to the propeller number, a mat parameter, such that each of the propellers can be shown in different color. We will use that to indicate the back and front propellers (that will only be shown in Gazebo simulation, but you can do something similar in the visual->material tag for Rviz). Note that the propellers are modelled using revolute joints below. This is not strictly necessary and fixed joints could be used instead. I did that in order to make the control plugin a little bit easier to write.

  <xacro:macro name="propeller" params="i mat">
    <link name="propeller${i}">
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <cylinder radius="${propeller_radius}" length="${propeller_height}"/>
        </geometry>
        <material name="propeller_material"/>
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <cylinder radius="${propeller_radius}" length="${propeller_height}"/>
        </geometry>
      </collision>
      <xacro:cylinder_inertial radius="${propeller_radius}" height="${propeller_height}" mass="${propeller_mass}">
        <origin xyz="0 0 0" rpy="0 0 0"/>
      </xacro:cylinder_inertial>
    </link>
   
    <joint name="arm${i}_propeller${i}" type="revolute">
      <parent link="arm${i}"/>
      <child link="propeller${i}"/>
      <origin xyz="${cos((i-1)*pi/3+pi/6)*(frame_radius+arm_length)} ${sin((i-1)*pi/3+pi/6)*(frame_radius+arm_length)} ${frame_height/2-arm_radius+propeller_height_offset}" rpy="0 0 0"/>
      <axis xyz="0 0 1"/>
      <limit lower="0" upper="0" effort="0" velocity="0"/>
    </joint>
   
    <gazebo reference="propeller${i}">
      <material>${mat}</material>
    </gazebo>
  </xacro:macro>

Next comes the material definitions for Rviz:

  <material name="frame_material">
    <color rgba="1 0.2 0.2 1"/>
  </material>
 
  <material name="arm_material">
    <color rgba="0.8 0.8 0.8 1"/>
  </material>
 
  <material name="propeller_material">
    <color rgba="0 0 0 0.6"/>
  </material>

Then we can define the frame link:

  <link name="frame">
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <cylinder radius="${frame_radius}" length="${frame_height}"/>
      </geometry>
      <material name="frame_material">
        <color rgba="0.8 0.8 0.8 1.0"/>
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <cylinder radius="${frame_radius}" length="${frame_height}"/>
      </geometry>
    </collision>
    <xacro:cylinder_inertial radius="${frame_radius}" height="${frame_height}" mass="${frame_mass}">
      <origin xyz="0 0 0" rpy="0 0 0" />
    </xacro:cylinder_inertial>
  </link>

  <gazebo reference="frame">
    <material>Gazebo/Orange</material>
  </gazebo>

... and we use the macros we have defined earlier to add the arms and the propellers. Since the arms go clockwise, the #1 and #6 are for the front propellers (red), and #3 and #4 are for the back propellers (blue).

  <xacro:arm i="1"/>
  <xacro:arm i="2"/>
  <xacro:arm i="3"/>
  <xacro:arm i="4"/>
  <xacro:arm i="5"/>
  <xacro:arm i="6"/>
 
  <xacro:propeller i="1" mat="Gazebo/RedTransparent"/>
  <xacro:propeller i="2" mat="Gazebo/BlackTransparent"/>
  <xacro:propeller i="3" mat="Gazebo/BlueTransparent"/>
  <xacro:propeller i="4" mat="Gazebo/BlueTransparent"/>
  <xacro:propeller i="5" mat="Gazebo/BlackTransparent"/>
  <xacro:propeller i="6" mat="Gazebo/RedTransparent"/>

</robot>

4. Checking the model out in Rviz

You could convert the Xacro file above into URDF by hand:

roscd kair_drone/urdf
xacro --inorder drone.urdf.xacro > drone.urdf

but it is all done by the launch file created in section 2 already. It is a good idea to check whether there are any mistakes in the file though, so using xacro and check_urdf first is not a bad idea.

We can now check the model out in Rviz:

roslaunch kair_drone rviz.launch

You should see a model similar to the one in Fig. 1 now. If you wish, you can now add the Axes elements to show the positions of links. If you now save the Rviz configuration to rviz/drone.rviz (check the launch file), it will be automatically used whenever you use the launch file again.

5. Running the model in Gazebo

First, we will need a world file, which defines the environment that the drone will be put in. This is easy enough: simply launch Gazebo and save the empty world it starts with using File->Save World As as the default.world file in the world directory.

Next, prepare the following gazebo.launch file in the launch directory:

<launch>

  <!-- these are the arguments you can pass this launch file, for example paused:=true -->
  <arg name="paused" default="false"/>
  <arg name="use_sim_time" default="true"/>
  <arg name="gui" default="true"/>
  <arg name="headless" default="false"/>
  <arg name="debug" default="false"/>

  <!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="$(find kair_drone)/worlds/default.world"/>
    <arg name="debug" value="$(arg debug)" />
    <arg name="gui" value="$(arg gui)" />
    <arg name="paused" value="$(arg paused)"/>
    <arg name="use_sim_time" value="$(arg use_sim_time)"/>
    <arg name="headless" value="$(arg headless)"/>
  </include>

  <!-- Load the URDF into the ROS Parameter Server -->
  <param name="robot_description"
    command="$(find xacro)/xacro --inorder '$(find kair_drone)/urdf/drone.urdf.xacro'" />

  <!-- Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
  <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
    args="-urdf -model drone -param robot_description -z 0.1"/>
   
</launch>

Running the file as:

roslaunch kair_drone gazebo.launch

should spawn the model in Gazebo (see Fig. 2).

Fig. 2. Hexacopter in Gazebo.

Next time, we will create a Gazebo plugin to control the model and make it fly.