Interfacing CrazyS through MATLAB
This wiki section explains how to to use the MathWorks Robotics System Toolbox (RST) to build-up a Software-in-the-loop (SITL) architecture in which Simulink schemes of control loops (defined in earlier designing phase) are reused and interfaced to Gazebo in order to simulate the detailed aircraft physical model.
The SITL infrastructure allows in a few steps to compare the results obtained from the interaction between Simulink schemes (controller) and Gazebo (physics) with the outcomes of the system completely implemented in Matlab/Simulink (both physical model and controller). In this way, implementation details like controller discretization, concurrency, timing issues, can be isolated when looking at the Matlab/Simulink platform only, while their effects can be investigated by considering the Simulink and Gazebo simulations.
Therefore, the RST allows in an easy way to test and verify the behavior of the flight control system, by comparing and evaluating different control strategies, making it possible to come back easily to the control design phase (whose output is usually the Simulink schemes) before implementing the ROS code. Such an approach saves time in the development of possible problematic code and fulfills requirements of modern embedded systems development based on the well-known V-cycle.
The entire process (described in this page) has been tested with the 2017b release of Matlab, but it is compatible with any Matlab release successive to 2015a. The code (available in Section Example Controller is specific for CrazyS, but it can be easily and quickly customized to work with any aircraft in the simulation framework.
As described in [1], the MathWorks Robotics System Toolbox™ provides algorithms and hardware connectivity for developing autonomous robotics applications for aerial and ground vehicles, manipulators, and humanoid robots.
The system toolbox provides an interface between MATLAB® and Simulink® and the Robot Operating System (ROS) that enables us to test and verify applications on ROS-enabled robots and robot simulators such as Gazebo. It includes examples showing how to work with virtual robots in Gazebo and with actual ROS-enabled robots.
Therefore, the Simulink schemes employed during the simulation can be easily transformed making them compatible with RTS blocks. Starting from that schemes (the download link is reported in the Section Example Controller, the feedback loops are replaced by RST blocks implementing the publish/subscribe paradigm dealing with ROS topics, as depicted in Figure 1 and described in [2]. Instead, the robot dynamics (the Crazyflie in the case of study) is provided by Gazebo (when simulating the robots in a virtual reality environment) or the real platform.
In CrazyS, the drone dynamics come from the sensors data, while the controller outputs (actuators commands) are sent by the modified Simulink scheme to the detailed physical model in the virtual scenario (Gazebo).
Figure 1: The Simulink control scheme obtained using RST blocks is depicted. The red box highlights the block implementing the ROS topic subscription to the sensors values, while the green box indicates the block in charge to publish the propellers angular velocity.
Therefore, the physical model, that was present in the Simulink scheme when simulating the controlled drone dynamics in Matlab, can be removed. Although now the simulation is based on the physical engine of Gazebo and runs through the ROS middleware, any change or modification of the control law is limited to standard Simulink blocks at a higher abstraction level.
Robotics System Toolbox supports C++ code generation, enabling to generate a ROS node from a Simulink scheme and automatically deploy it to a ROS network. Furthermore, the support for Simulink external mode lets view signals and change parameters while the deployed model is running.
Although RST is available from the 2015a release of Matlab, it does not support all types of ROS messages, such as mav msgs/Actuators (employed in CrazyS to send the propellers angular velocities to the Gazebo physics engine). The issue can be partially overcome by installing a suitable add-on roboticsAddons, hosted on the
MathWorks add-ons explore, and by creating the custom messages starting from the properly ROS package. Indeed, the toolbox supports the forwarding of custom messages only via Matlab scripts. Therefore, the Simulink schemes have to be adapted and integrated with Matlab scripts for exchanging data and commands with ROS and Gazebo, as shown in the files described in Section Examples Controller.
As shown in this video, the communication between Simulink and Gazebo needs to be synchronized via Gazebo services (unpause and pause physics) that run and stop the simulation so to avoid data losses and system instabilities. When the scheme runs in synchronization mode, the client (Matlab) is in charge of deciding when the next step should be triggered by making the server (Gazebo) advance the simulation. In this way, it is avoided any possible synchronization/communication issue arising from a real implementation of a cyber-physical system. When the control strategy is sufficiently investigated and verified, all implementation issues can be modeled and/or taken into account thus removing the artificial synchronization and proceeding with the coding for implementing the control strategy on middleware like ROS or even on a real-time operating system (OS).
Below the launch code (specifically the crazyflie_without_controller.launch) employed to link Matlab/Simulink with ROS and Gazebo, is reported. Such code starts the server (Gazebo) that simulates the Crazyflie dynamics and sensors. Then, Gazebo goes on standby waiting for the Simulink scheme implementing the controller (the files reported in the Section Examples Controller). It will be in charge to run and pause the physical engine computations in order to simulate the controlled scenario.
<launch>
<arg name="mav_name" default="crazyflie2"/>
<arg name="world_name" default="basic"/>
<arg name="enable_logging" default="false" />
<arg name="enable_ground_truth" default="true" />
<arg name="enable_state_estimator" default="false" />
<arg name="log_file" default="$(arg mav_name)" />
<arg name="paused" value="true"/>
<arg name="debug" default="false"/>
<arg name="gui" default="true"/>
<arg name="verbose" default="false"/>
<env name="GAZEBO_MODEL_PATH" value="${GAZEBO_MODEL_PATH}:$(find rotors_gazebo)/models"/>
<env name="GAZEBO_RESOURCE_PATH" value="${GAZEBO_RESOURCE_PATH}:$(find rotors_gazebo)/models"/>
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(find rotors_gazebo)/worlds/$(arg world_name)_crazyflie.world" />
<arg name="debug" value="$(arg debug)" />
<arg name="paused" value="$(arg paused)"/>
<arg name="gui" value="$(arg gui)" />
<arg name="verbose" value="$(arg verbose)"/>
</include>
</launch>
Here there is an example file showing how to use the Robotics System Toolbox to control the propellers angular velocity of a generic aircraft described in RotorS. Instead, here the scheme employed to interface Matlab/Simulink with Gazebo and to control the drone dynamics in the hovering example.
Note: as suggested in #19, the above file has some bugs and typos fixed in this version.
A MATLAB script implementing Lee's Geometric Controller has been also implemented. Thanks to Stefano Grossi for this effort. I would also thank Lorenzo Sabattini, Federico Pratissoli, and Beatrice Capelli for their support in Stefano's master thesis.
An unofficial procedure to install ROS custom messages without using the suitable add-ons. First, download the messages from the link and link and extract the archive content in the Matlab workspace. Later, move into the folder:
C\Users\$PCUserName\AppData\Roaming\MathWorks\MATLAB\$releaseSoftware,
where $PCUserName and $releaseSoftware are the pc username and the release software of MATLAB, respectively. In the case that more than one release of Matlab is installed on the pc, you should choose which one would like to enable.
Into the folder, create a txt file and rename it as javaclasspath.txt. Later, copy and paste the following lines:
C:\Users\$PCUserName\Documents\MATLAB\customMessages\matlab_gen\jar\geometry_msgs-1.12.5.jar
C:\Users\$PCUserName\Documents\MATLAB\customMessages\matlab_gen\jar\sensor_msgs-1.12.5.jar
C:\Users\$PCUserName\Documents\MATLAB\customMessages\matlab_gen\jar\trajectory_msgs-1.12.5.jar
C:\Users\$PCUserName\Documents\MATLAB\customMessages\matlab_gen\jar\mav_msgs-3.2.0.jar
Now, to enable the ROS messages is enough to run in the command window the commands:
addpath('C:\Users\$PCUserName\Documents\MATLAB\customMessages\matlab_gen\msggen');
savepath
To complete the procedure, Matlab has to be restarted.
To enable the communication between Matlab and Gazebo, the hosts file in C:\Windows\System32\drivers\etc has to be modified by adding the line:
IP_OF_THE_LINUX_MACHINE (where Gazebo is hosted) NAME_OF_THE_LINUX_MACHINE
- Giuseppe Silano
- Antonio Bono
- Stefano Grossi
- Lorenzo Sabattini
- Federico Pratissoli
- Beatrice Capelli