Simulating a drone in Gazebo part 2 - Gazebo plugin

Last time we have built a URDF model of a drone and spawned it in Gazebo. Today we will make a Gazebo plugin to control the drone.

Our goal is to create a Gazebo Model Plugin that will interface with ROS and add forces to the model based on the desired motor speeds, such that the drone will be able to fly. This is the very basic level of control, which we need to implement.

The plugin ROS interface will be as follows:

• Drone pose in the world frame of reference will be published on the /pose topic. We will create a kair_drone/Pose message type for easy orientation representation using RPY angles.
• The plugin will also publish the frame pose using a TF broadcaster on the /tf topic.
• The plugin will listen for the kair_drone/MotorSpeed message on the /motor_speeds_cmd topic and apply the simulated forces to the model.
• The plugin parameters will be defined as the SDF elements in the xacro model file.

As usual, the code is all available in the repository.

1. Creating the messages

First, we shall create the two message types necessary. The Pose message shall carry the complete position and orientation information about the model in simulation and the MotorSpeed will define the desired speeds for the six motors of the model.

The Pose message is defined in the msg/Pose.msg file as follows:

float32 x
float32 y
float32 z
float32 roll
float32 pitch
float32 yaw
 

There are several message types defined in the standard geometry_msgs package that we could use instead, e.g. Pose or Transform. These involve the conventional ROS quaternion representation though and it will be easier for us to represent the orientation as RPY angles explicitly.

The MotorSpeed message is defined in the msg/MotorSpeed.msg file:

string[] name
float32[] velocity

Next, we have to modify the CMakeLists.txt and the package.xml to enable message building. Make sure that the package has the following dependencies:

• geometry_msgs
• sensor_msgs
• std_msgs
• tf
• message_generation

Next, make sure that the message definition and message generation sections in the CMakeLists.txt look like this:

## Generate messages in the 'msg' folder
add_message_files(
  FILES
  Pose.msg
  MotorSpeed.msg
)

## Generate added messages and services with any dependencies listed here
generate_messages(
  DEPENDENCIES
  geometry_msgs
  sensor_msgs
  std_msgs
)

Rebuild the project now and re-source the setup files:

cd ~/catkin_ws

catkin build

source devel/setup.bash

2. Preparing the build system for the Gazebo plugin

 The following changes need to be made to the CMakeLists.txt in order to build the Gazebo plugin:

• in the system dependencies section:

## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
find_package(gazebo REQUIRED)

• in the Build section:

## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
# include
  ${catkin_INCLUDE_DIRS}
  ${GAZEBO_INCLUDE_DIRS}
)

link_directories(
  ${GAZEBO_LIBRARY_DIRS}
)
list(APPEND CMAKE_CXX_FLAGS "${GAZEBO_CXX_FLAGS}")

• and finally, we add the plugin target:

add_library(drone_plugin SHARED src/drone_plugin.cpp)
target_link_libraries(drone_plugin ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES})

3. Creating the plugin

Let's begin by creating the boilerplate Model plugin file src/drone_plugin.cpp:

#include <iostream>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>

class DronePlugin : public gazebo::ModelPlugin {
public:
  DronePlugin() : gazebo::ModelPlugin() {
    std::cout << "Starting drone_plugin" << std::endl;
  }
 
  virtual ~DronePlugin() {
    std::cout << "Closing drone_plugin" << std::endl;
  }

  void Load(gazebo::physics::ModelPtr parent, sdf::ElementPtr sdf) {
    _model = parent;
  }
 
private:
  gazebo::physics::ModelPtr _model;

};
 
GZ_REGISTER_MODEL_PLUGIN(DronePlugin)

This is the very basic plugin file structure that we will gradually expand. It is a good idea at this point to check whether the build system is set up properly:

cd ~/catkin_ws

catkin build

4. Reading the plugin parameters from SDF

If we want to specify the plugin parameters in the model SDF (and we do), we can then read them inside the plugin's Load() function. We will read the updateRate parameter (how often the pose messages shall be published), publishTf parameter (whether to publish the pose on /tf as well), rotorThrustCoeff (a coefficient that relates the rotor lift to its rotational speed) and rotorTorqueCoeff (which relates the rotor torque to its speed). This can be done by modifying the Load() function as:

  void Load(gazebo::physics::ModelPtr parent, sdf::ElementPtr sdf) {
    _model = parent;
   
    if (sdf->HasElement("updateRate")) {
      _rate = sdf->GetElement("updateRate")->Get<double>();
    } else {
      _rate = 100.0;
    }
   
    if (sdf->HasElement("publishTf")) {
      _publish_tf = sdf->GetElement("publishTf")->Get<bool>();
    } else {
      _publish_tf = true;
    }
   
    if (sdf->HasElement("rotorThrustCoeff")) {
      _rotor_thrust_coeff =

         sdf->GetElement("rotorThrustCoeff")->Get<double>();
    } else {
      _rotor_thrust_coeff = 0.00025;
    }
   
    if (sdf->HasElement("rotorTorqueCoeff")) {
      _rotor_torque_coeff =

        sdf->GetElement("rotorTorqueCoeff")->Get<double>();
    } else {
      _rotor_torque_coeff = 0.0000074;
    }

  }

Of course, the new variables also have to be added to the private section of the DronePlugin class:

private:
  double _rate;
  bool _publish_tf;
  double _rotor_thrust_coeff;
  double _rotor_torque_coeff;

5. Creating the ROS node

In order to enable the ROS interface, we need to create a ROS node handle during the plugin initialization. This is also done in the Load() function:

void Load(gazebo::physics::ModelPtr parent, sdf::ElementPtr sdf) {
  ...
 
  if (!ros::isInitialized()) {
    int argc = 0;
    char** argv = NULL;
    ros::init(argc, argv, "kair_drone",

       ros::init_options::NoSigintHandler);
  }

  _nh = new ros::NodeHandle("");
  _pose_pub = _nh->advertise<kair_drone::Pose>("pose", 100);
  _cmd_sub = _nh->subscribe("motor_speed_cmd", 100,

      &DronePlugin::onMotorSpeedsMsg, this);
  _ros_thread = 

      std::thread(std::bind(&DronePlugin::rosThread, this));
 
}

It is necessary to check whether the ROS was already initialized. Since we are going to launch Gazebo through gazebo_ros, it most likely was. We create a new ROS handle _nh and the /pose publisher and the /motor_speed_cmd subscriber as well. Furthermore, we also create a separate thread for ROS to spin in.

The onMotorSpeedsMsg() callback looks simple enough:

void onMotorSpeedsMsg(const kair_drone::MotorSpeed::ConstPtr& msg) {
  _cmd_mtx.lock();
  _motor_speed_msg = *msg;
  _cmd_mtx.unlock();
}

The rosThread() is the place where the ROS gets updated and where we will publish the model pose from:

void rosThread() {
  ros::Rate rate(_rate);
  while (ros::ok()) {
    ros::spinOnce();
    publishDronePose();
    rate.sleep();
  }
}

We might as well define the publishDronePose() method now:

void publishDronePose() {
  _pose_mtx.lock();
  gazebo::math::Pose pose = _pose;
  _pose_mtx.unlock();
 
  gazebo::math::Vector3 rpy = pose.rot.GetAsEuler();
  tf::Quaternion q(pose.rot.x, pose.rot.y, pose.rot.z, pose.rot.w);
  tf::Matrix3x3 m(q);
  double roll, pitch, yaw;
  m.getRPY(roll, pitch, yaw);
 
  kair_drone::Pose pose_msg;
  pose_msg.x = pose.pos.x;
  pose_msg.y = pose.pos.y;
  pose_msg.z = pose.pos.z;
  pose_msg.roll = roll;
  pose_msg.pitch = -pitch;
  pose_msg.yaw = yaw;
  _pose_pub.publish(pose_msg);
 
  if (_publish_tf) {
    tf::Transform T;
    T.setOrigin(tf::Vector3(pose.pos.x, pose.pos.y, pose.pos.z));
    T.setRotation(

      tf::Quaternion(pose.rot.x, pose.rot.y, 

        pose.rot.z, pose.rot.w));
    _tf.sendTransform(

        tf::StampedTransform(T, ros::Time::now(),

        "world", "drone"));
  }
}

Here you can see how the conversion from the conventional quaternion orientation to RPY is done. Notice also that we multiply pitch by -1. This is done in order to match the frame orientation in simulation with the common aircraft convention where the Z axis is expected to be directed downwards.

Please consult the complete code in the repository to see which headers and what data fields need to be added to the source at this point.

6. Adding the forces

The idea behind simulating the rotor equipped drone model is that at each step of the simulation we add an array of forces and torques to the drone body that correspond to the forces that would be exerted by the propellers (see Fig. 1). 

Fig. 1. Adding forces and torques to the model.

The forces for each of the propellers are calculated as:

The sgn() function is there to make it possible to define the direction of the torque by specifying the sign of the angular speed of the rotor.

Let us begin by adding yet another callback in the Load() function. This will make the onUpdate() method called at each step of the simulation:

void Load(gazebo::physics::ModelPtr parent, sdf::ElementPtr sdf) {
  ...
 
  _updateConnection =

      gazebo::event::Events::ConnectWorldUpdateBegin(

          std::bind(&DronePlugin::onUpdate, this));
}

The onUpdate() function updates the _pose (so only now will the publishDronePose work properly), and calls the updateThrust() method:

void onUpdate() {
  _pose_mtx.lock();
  _pose = _model->GetWorldPose();
  _pose_mtx.unlock();
 
  updateThrust();
}

The updateThrust() method looks up the child links of the model (so that is why we have modelled them as separate links attached by the revolute joints!) according to the name field in the MotorSpeed message, calculates the force and torque for each, and applies those to the link:

void updateThrust() {
  _cmd_mtx.lock();
  kair_drone::MotorSpeed cmd = _motor_speed_msg;
  _cmd_mtx.unlock();
 
  int n = cmd.name.size();
  for (int i = 0; i < n; ++i) {
    double thrust = calculateThrust(cmd.velocity[i]);
    double torque = calculateTorque(cmd.velocity[i]);
    gazebo::physics::LinkPtr link = _model->GetLink(cmd.name[i]);
    if (link != NULL) {
      link->AddLinkForce(gazebo::math::Vector3(0, 0, thrust));
      link->AddRelativeTorque(gazebo::math::Vector3(0, 0, torque));
    }
  }
}
 

AddLinkForce() and AddRelativeTorque() are the appropriate Gazebo methods to use here. The Gazebo documentation is sometimes quite insufficient in this regard, but these methods apply the force and torque respectively as an impulse; just in that one step of the simulation. What remains now is to calculate the force and torque for each of the propellers:

double calculateThrust(double w) {
  double thrust = _rotor_thrust_coeff * w * w;
  return thrust;

}

double calculateTorque(double w) {
  double torque = copysign(_rotor_torque_coeff * w * w, w);
  return torque;

}

7. Adding the plugin to the drone model

We can build the plugin now and add it to the drone model defined in urdf/drone.urdf.xacro. It goes right towards the end of the file:

...

<gazebo>
    <plugin name="drone_plugin" filename="libdrone_plugin.so">
      <updateRate>100</updateRate>
      <publishTf>true</publishTf>
      <rotorThrustCoeff>0.00025</rotorThrustCoeff>
      <rotorTorqueCoeff>0.0000074</rotorTorqueCoeff>
    </plugin>
  </gazebo>

</robot>

When we launch the program now:

roslaunch kair_drone gazebo.launch

we should see the /pose and /motor_speed_cmd visible in rostopic list. And we can now watch the drone fly (uncontrollably) up (Fig. 2)!

rostopic pub /motor_speed_cmd kair_drone/MotorSpeed "name: ['propeller1', 'propeller2', 'propeller3', 'propeller4', 'propeller5', 'propeller6']
velocity: [125, -125, 125, -125, 125, -125]" -1

Fig. 2. Drone flying up!

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.