# Integração Matlab-ROS

Tutorial adaptado do excelente:
[RotorS Simulator](https://github.com/ethz-asl/rotors_simulator/wiki/Interfacing-RotorS-through-Matlab)

OS: Ubuntu 20.04
ROS: Noetic (ROS 1)
Python: 3.9
Matlab: 2023b

Autor: Lucca Gandra ([e-mail])

## Instalação do Python correto

Para inserirmos ROS dentro do Matlab, precisamos ter instalado uma versão do Python compatível com o Matlab.

Verifique no link abaixo as versões de Python compatíveis com o MATLAB, por lançamento:
[Compatibilidade Matlab-Python](https://www.mathworks.com/support/requirements/python-compatibility.html). E, caso queira, instale um [Matlab pirata](https://downloadlynet.ir/2020/26/9256/03/matlab-2/16/?#/9256-matlab-r-222448060405.html). (Baixei o 2023b Linux sem problemas)

No caso em específico analisado, temos que a última versão do Python que o Ubuntu 20.04 suporta é a 3.9. A versão do Matlab que aceita o 3.9 é a 2023b. Portanto, para instalá-lo, usamos:

sudo apt install python3.9 python3.9-venv python3.9-dev

Você pode verificar os caminhos do seu python instalado com o comando:

whereis python3.9

## Setup ROS-Toolbox

Insira o caminho do python3.9 instalado em seu sistema. No meu caso é "/usr/bin/python3.9".



## Execução do script

É recomendado enviar esses comandos no terminal antes do script:

clear classes

rehash toolboxcache

Se tudo der certo, no momento que executar um arquivo Matlab com a estrutura:

    %% ROS: New connection with host
    rosshutdown

    IP_ROS_Master = 'http://localhost:11311/';
    rosinit(IP_ROS_Master)
    node = ros.Node('/ros_matlab');

A conexão com a master deverá ter sido iniciada e uma mensagem como essa aparecerá no terminal:


## Criação de mensagens customizadas

No terminal do Matlab, utilize o comando rosgenmsg para construir as mensagens customizadas. Em meu caso, utilizei as mensagens próprias do wiki do RotorS, que estão disponíveis neste [link](https://github.com/gsilano/CrazyS/wiki/files/customMessages.rar).

O argumento deve ser o caminho da pasta que contém as mensagens customizadas.

rosgenmsg('PATH-TO-CUSTOM_MSGS')

Agora é só copiar os comandos gerados pelo serviço.

addpath('CAMINHO-GERADO-PELO-ROSGENMSG')
savepath
clear classes
rehash toolboxcache

## Exemplo de Subscriber e Publisher

    %% ROS: Publishers and Subscribers
    
    %% Setup
    ros_sub_odometry = rossubscriber('/M600/odometry'); % contains the drone state
    %ros_sub_motor_speed = rossubscriber('/kopterworx/motor_speed'); 
    
    [ros_pub_motor_speed, ros_actuator_msg] = rospublisher('/M600/gazebo/command/motor_speed','mav_msgs/Actuators');
    
    %% ------------------------------------------------------------
        
    %% Loop
    ros_odom = receive(ros_sub_odometry);

    %GazeboMessage{i,1} = ros_odom;

    % Position
    x_GAZEBO = ros_odom.Pose.Pose.Position.X;
    y_GAZEBO = ros_odom.Pose.Pose.Position.Y;
    z_GAZEBO = ros_odom.Pose.Pose.Position.Z;

    p = [x_GAZEBO y_GAZEBO z_GAZEBO]';


    %Linear velocity
    u_GAZEBO = ros_odom.Twist.Twist.Linear.X;
    v_GAZEBO = ros_odom.Twist.Twist.Linear.Y;
    w_GAZEBO = ros_odom.Twist.Twist.Linear.Z;

    v = [u_GAZEBO v_GAZEBO w_GAZEBO]';

    % Quaternion

    q0_GAZEBO = ros_odom.Pose.Pose.Orientation.W;
    q1_GAZEBO = ros_odom.Pose.Pose.Orientation.X;
    q2_GAZEBO = ros_odom.Pose.Pose.Orientation.Y;
    q3_GAZEBO = ros_odom.Pose.Pose.Orientation.Z;

    q = [q0_GAZEBO q1_GAZEBO q2_GAZEBO q3_GAZEBO]';

    %Angular velocities in ABC frame (ABC Frame?)

    p_GAZEBO = ros_odom.Twist.Twist.Angular.X;
    q_GAZEBO = ros_odom.Twist.Twist.Angular.Y;
    r_GAZEBO = ros_odom.Twist.Twist.Angular.Z;

    Omega = [p_GAZEBO q_GAZEBO r_GAZEBO]';

## Exemplo:

Drone com controle SSC, implementado no Matlab e simulado no Gazebo. A planta no Gazebo usou as mesmas constantes da simulada em Matlab, mas com a interface do [RotorS Simulator](https://github.com/ethz-asl/rotors_simulator/wiki/Interfacing-RotorS-through-Matlab).

O setpoint de posição foi [0,0,5].

## À fazer:

Verificar passo de simulação e tempo do ROS. Como sincronizar?