MQTT to Gazebo - Drone Simulator

[Helena_Moyen]

Introduction

Hi! I am currently developing a game project with my group in my university that will integrate a joystick, ESP8266, an FPGA board and will have a Gazebo as a graphical interface! I am responsible for the ESP8266 and PC integration via MQTT and the motion of a virtual drone in Gazebo with ROS 1. The idea is to allow movement of the drone with the joystick, which will be connected to the FPGA board. The board will use the the ESP to communicate with the PC. I would like to document the progress here

The game is based on avoiding obstacles on the map in Gazebo. Gazebo will detect collisions and send the signal back to the FPGA. The FPGA will handle these signals, have the game state machine and transform analog signals to digital. We are using FPGA because it is a mandatory project requirement for the subject.

First part: MQTT to ROS

The first part of the project was to allow the movement of the drone via MQTT. I started using Mosquitto to have a local broker in my PC and allow communication with the ESP8266. Then, I used the Paho Client library to get the messages from the ESP and resend them to a MAVROS topic, all that in a Python script. I used the Ardupilot simulation (SITL) to control my virtual drone. I was able to send
PoseStamped messages to the topic /mavros/setpoint_position/local when the ESP sent a message in an /out topic via MQTT. Basically, I was only reading the state of the BUILT IN LED and if it was off, the drone moved, otherwise it did nothing. Later, I will read the actual PINS of the ESP, instead of the LED (only did that because I had nothing connected to the ESP). I did the same for the topic /mavros/setpoint_velocity/cmd_vel . It worked okay, but now I need to do that for velocity in other axis too!

Second part: Detecting collisions in Gazebo

To detect collisions in Gazebo, I started following this tutorial that teaches how to use the Contact Sensor. I changed the iris_base model of the copter to have a contact sensor and a collision the size of a box (may change in the future). I am a beginner, so I will take any suggestions.

<collision name="drone_collision">
          <geometry>
            <box>
              <size>0.4 0.4 0.4</size>
            </box>
          </geometry>
        </collision>
<sensor name='my_contact' type='contact'>
        <plugin name="my_plugin" filename="libcontact.so"/>
          <contact>
            <collision>drone_collision</collision>
          </contact> 
        </sensor>

I thought about every obstacle having a sensor, but I decided that it was simpler to just add a sensor to the drone.

Then I did a test. I modified a world and added a static box. After that I made the drone move towards the box, looking for a collision. I monitored it through the topic /gazebo/default/iris/drone_with_camera/iris/base_link/my_contact and got the expected results when the drone touched the box.

Next steps

I need to find a way to turn the contact message into a signal of 1 or 0 to send back to the ESP and also try to control other axis with the pins of the ESP (and in the future, using the joytstick). The scripts, worlds and models used are here.

[Helena_Moyen]

Second part: change of plans

Hey! I came back with some updates on the collisions part and also on the Python code structure. First of all, I discovered the bumper plugin on Gazebo that posted messages to a ROS thread and I thought it fit the bill better. So, my drone model .sdf file had this change:

<collision name="drone_collision">
          <geometry>
            <box>
              <size>0.4 0.4 0.4</size>
            </box>
          </geometry>
        </collision>
      <sensor name='my_contact' type='contact'>
          <contact>
            <collision>drone_collision</collision>
          </contact> 
        <plugin name="drone_gazebo_ros_bumper_controller" filename="libgazebo_ros_bumper.so">
        <alwaysOn>true</alwaysOn>
        <updateRate>${update_rate}</updateRate>
        <bumperTopicName>drone_bumper</bumperTopicName>
        <frameName>world</frameName>
      </plugin>
  </sensor>

Now, I could subscribe to the topic /drone_bumper and get the collisions through the msg.states information. The problem I encountered was with the communication with the ESP. My ESP and MQTT communication could not handle the number of messages in the collision topic and ended up being very delayed compared to what was happening in the simulation (the pins only went to the HIGH state after a while when there was a collision). That’s why I had to create some filter for the messages that were published on the MQTT topic at a frequency of 10Hz. It would be interesting to do some type of mean or mode of the last messages to filter isolated collisions (I don’t want the sensor to be so sensitive, the game would be too hard!).

if current_time - self.last_publish_time >= self.publish_interval:
      self.last_publish_time = current_time
      client.publish_collision("1")  # MQTT client publishes 1 in topic of collision to ESP

Third part: putting it all together

And this is where I ran into some problems. Basically I have several topics on both MQTT and ROS active. I started by placing callbacks for each MQTT Subscriber and creating Publishers in MQTT. Then I did the same for ROS, and it caused confusion. This is because I had two loops: one for the MQTT client and one for ROS. I had problems with only being able to access one MQTT topic and then also not being able to post messages in ROS, as the loops were different and the program was getting messy. I think I could make it work, but then I discovered that the [mqtt_client](https://I think I could make it work, but then I discovered that the mqtt_client - ROS Wiki package existed) package existed (my mistake for not looking it up sooner!). This made everything easier. Basically now I would only need to access ROS topics in my Python code and the package would do all the rest of the work. I just needed to define the MQTT topics that I wanted to be posted to ROS and vice versa. I’m also always using the primitive = True argument, as all my messages with ESP will be digital and so far in string: “1” or “0”.

broker:
  host: lena-Inspiron-5402
  port: 1883
bridge:
  ros2mqtt:
    - ros_topic: /broker/collision
      mqtt_topic: /drone_collision
      primitive: true
  mqtt2ros:
    - mqtt_topic: /broker/set_vel
      ros_topic: /ros/set_vel
      primitive: true

For example, for my collision data, I need to subscribe to the topic /drone_bumper as before, read the msg.states and send an output message to the topic /broker/collision . The mqtt_client package then remaps the message to a MQTT topic /drone_collision, which my ESP has access to and reads the data as “1” or “0”.

The same logic applies to my velocity data. My ESP posts “1” or “0” for a certain axis of velocity (the format can change in the future) in a topic such as /broker/set_vel, the package remaps the message to the ROS topic /ros/set_vel and in my code I reinterpret the message and post a message in the topic /mavros/setpoint_velocity/cmd_vel.

Next steps
I will try to add movement to more than one axis in my code and have all the data in the correct format for the MQTT topics. Then, I will start creating different worlds, based on the static maps my friend provided me (16x4 matrix with obstacles).

[Helena_Moyen]

Hey! I am back for more updates!

Third part: putting it all together

The simulator now has movement in both x and y-axis, has the collision detector and a world selector. I also implemented a topic to initialize and end the current world (so I don’t need to re-open and re-start the whole simulation in case I just want to change my world). It is working well but I had to use a lower frequency to avoid an overflow of the ESP. The script is getting quite robust, with so many different signals and states, but it is working! To add these features, I am using the same logic and resources as explained in previous updates I made.

About the FPGA board, it is working as a simplified digital twin of the simulation: it has its own map in a 16x4 matrix and calculates the position of the drone with the signals I send from Gazebo of movement. Since I multiplied the matrix by 10 in Gazebo, if my drone moves 1 meter forward, it does not mean it changed position in my 16x4 matrix. So, basically, I need to check every cycle if my drone moved enough to change “squares” of my matrix. Then, if it changed “squares”, I send the FPGA a signal that it has moved one square left, right, forwards or backwards.

Forth part: new worlds

I spent the last few days changing the way my worlds are constructed and searching for models to pretend the simulation occurs in a small city, with people, buildings, trees and walls. My code has different matrices that the user can choose from to build the map. When the matrix is chosen through the topic /ros/world, the script spawn the obstacles (now buildings) in the 1 marked positions of the matrix. It also builds a wall around the proportional matrix in Gazebo, so my drone may not escape from there. Here is a sample of how it is looking:

I noticed my SITL simulation was getting quite slow to arm the drone, not sure if this is an en effect of heavier simulation (many models, messages, topics and services being requested).

I am using models I found in this link and this one. They are perfect for what I want to build!

Next steps
Tomorrow we are testing the whole game together to see if the synchronization with Gazebo, ESP and FPGA works! If it does, I will clean the code, do the documentation, maybe a docker image and finish the project