---
# System prepended metadata

title: Capítulo 1 - Artigo 5 - Subscritor
tags: [Apostila de Introdução Geral no LEAD - 2024]

---

# Capítulo 1 - Artigo 5 - Subscritor

> Gazebo + Joy | Keyboard

## 5.0 - Introdução 

Introduzindo o artigo 5

## 5.0.1 - Sumário Geral do capitulo 1
1. Introdução do Capítulo
2. Instalando o ROS
3. Nó
4. Publicador
5. Subscritor       < Estamos aqui
6. Raspberry Pi
7. Nó Raspberry
8. Launch

## 5.0.2 Sumário Local
[TOC]

## 5.1 Criando um novo pacote no mesmo workspace: Gazebo_car_pkg 

> crie o espaço para poder criar o nó

Esse pacote será criado no mesmo Workspace do tutorial anterior.

Criando o pacote:

[https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Creating-Your-First-ROS2-Package.html](https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Creating-Your-First-ROS2-Package.html)

```bash
user@humbleV1:/home/USER$ cd ~/ros2_ws/src  
user@humbleV1:~/ros2_ws/src$ source /opt/ros/humble/setup.bash  
user@humbleV1:~/ros2_ws/src$ ros2 pkg create --build-type ament_python --license Apache-2.0 Gazebo_car_pkg 
user@humbleV1:~/ros2_ws/src$ cd Gazebo_car_pkg/Gazebo_car_pkg/
```

- Feito o pacote. Precisamos então criar nosso nó.

## 5.2 - Criando o nó Gazebo_Car_Key_node

Criando um nó:

```bash
user@humbleV1:~/ros2_ws/src/Gazebo_car_pkg/Gazebo_car_pkg$ touch Gazebo_Car_Key_node.py 
```

Este código é um exemplo de um nó em ROS escrito em Python. Ele permite controlar um robô simulado em ambiente Gazebo utilizando teclas 'a','s','d','w' do teclado.


```python!=1
import numpy, rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from std_msgs.msg import String

pub = None
matrixAtual = numpy.array([0,0,0,0])


def readOutputKeyForGazeboCar(outputKey):
  global matrixAtual
  nW = nA = nS = nD = -1
  for element in outputKey:
    match element:
      case 'w':
        nW=1
      case 'a':
        nA=1
      case 's':
        nS=1
      case 'd':
        nD=1
  matrixAtual = numpy.maximum(matrixAtual+numpy.array([nW,nS,nA,nD]), 0)
  Lx = matrixAtual[0] - matrixAtual[1]
  Az = matrixAtual[2] - matrixAtual[3]
  return Lx, Az

def limparStringsVazias(listaStrings):
    return [string for string in listaStrings if string.strip()]

def keyCallback(data):
   listaKeyboard = limparStringsVazias(str(data.data).split(" "))
   Lx, Az = readOutputKeyForGazeboCar(listaKeyboard)
   publish(Lx,Az)

def publish(Lx,Az):
    global pub
    msg = Twist()
    msg.linear.x  = float(Lx)
    msg.angular.z = float(Az)
    if not ((pub is None)):
        pub.publish(msg)

def main(args=None):
    global pub
    
    rclpy.init(args=args)    
    node = Node("key_node")

    pub = node.create_publisher(Twist, '/demo/cmd_demo', 1)
    node.create_subscription(String,'/key',keyCallback,1)
    
    rclpy.spin(node)

main()
```
- Esse código será explicado no 5.4

## 5.3 - Instalando e Executando o Nó

```bash
user@humbleV1:~/ros2_ws/src/Gazebo_car_pkg/Gazebo_car_pkg$ cd ~/ros2_ws
```

- Depois de feito isso você terá que mudar o arquivo “package.xml”,adicionando as dependencias do seu código.
 
Ele estará assim:
```xml=7
...
<license>Apache-2.0</license>

<test_depend>ament_copyright</test_depend>
...
```
- terá que adicionar 3 linhas entre essas duas linhas, ficando assim:

```xml=7
...
<license>Apache-2.0</license>

<exec_depend>rclpy</exec_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>sensor_msgs</exec_depend>

<test_depend>ament_copyright</test_depend>
<test_depend>ament_flake8</test_depend>
...
```


- E, por fim, você terá que modificar o setup.py:


```python!=1
...

tests_require=['pytest'],
  entry_points={
    'console_scripts': [
        'key_node = key_pkg.node_pub_key:main',
    ],
  },
)
```

- O setup.py é quem diz como o colcon irá construir seu nó.

- Será necessário instalar as dependencias do seu programa agora. Também será necessário especificar uma biblioteca, pelo uso padrão do Humble.

```bash
user@humbleV1:~/ros2_ws$ pip3 install pynput
user@humbleV1:~/ros2_ws$ rosdep update 
user@humbleV1:~/ros2_ws$ rosdep install -i --from-path src --rosdistro humble -y
```
Agora você já pode buildar seu nó. Para isso rode:

```bash
user@humbleV1:~/ros2_ws$ colcon build -symlink-install 
```

## 5.4 - Explicando o Nó gazebo Car Key

```python!=1
import numpy, rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from std_msgs.msg import String

pub = None
matrixAtual = numpy.array([0,0,0,0])
```

São importadas algumas bibliotecas e classes para o funcionamento do código. 

Importamos as bibliotecas numpy para operações matriciais e rclpy para programação em ROS.

Importamos a classe Node do rclpy.node, que será usada para criar o nó ROS.

Importamos o tipo de mensagem Twist do pacote geometry_msgs. Essa mensagem é frequentemente usada para enviar comandos de velocidade para robôs em ROS.

Importamos o tipo de mensagem String do pacote std_msgs, essa mensagem é usada para enviar strings em ROS.

Definimos uma variável global pub como None, ela será usada para publicar mensagens Twist.

Definimos uma matriz matrixAtual como uma matriz NumPy de tamanho 4x1 preenchida com zeros, essa matriz será usada para acompanhar as teclas pressionadas.

```python!=10
def readOutputKeyForGazeboCar(outputKey):
  global matrixAtual
  nW = nA = nS = nD = -1
  for element in outputKey:
    match element:
      case 'w':
        nW=1
      case 'a':
        nA=1
      case 's':
        nS=1
      case 'd':
        nD=1
  matrixAtual = numpy.maximum(matrixAtual+numpy.array([nW,nS,nA,nD]), 0)
  Lx = matrixAtual[0] - matrixAtual[1]
  Az = matrixAtual[2] - matrixAtual[3]
  return Lx, Az
```
Essa função verifica quais teclas estão sendo pressionadas, atualiza a matriz de estado matrixAtual baseada nessas teclas e então calcula as velocidades linear e angular com base nesse estado atualizado. 

> seria legal colocar uma equação para demostrar isso ai

Linha 24: Calcula a velocidade linear Lx como a diferença entre o primeiro e o segundo elemento de matrixAtual.

Linha 25: Calcula a velocidade angular Az como a diferença entre o terceiro e o quarto elemento de matrixAtual.


```python!=28
def limparStringsVazias(listaStrings):
    return [string for string in listaStrings if string.strip()]
```

Esta função é projetada para limpar strings vazias de uma lista de strings, removendo qualquer entrada de espaço adicional. Assim, a função limparStringsVazias retorna uma lista que contém apenas as strings da lista original listaStrings que não estão vazias após a remoção de espaços em branco extras. Isso garante que quaisquer entradas de espaço adicionais sejam removidas, deixando apenas as teclas pressionadas como elementos válidos na lista resultante. Isso é útil para garantir que o processamento subsequente das teclas seja feito corretamente, sem interferência de espaços extras.

A sintaxe utilizada nessa função é conhecida como compreensão de lista em Python, que é uma maneira concisa de criar listas a partir de outras estruturas como listas, tuplas, ou conjuntos. Você pode estudar mais sobre essa sintaxe aqui: [link](https://www.w3schools.com/python/python_lists_comprehension.asp)


```python!=31
def keyCallback(data):
   listaKeyboard = limparStringsVazias(str(data.data).split(" "))
   Lx, Az = readOutputKeyForGazeboCar(listaKeyboard)
   publish(Lx,Az)

```

Esta função é chamada quando uma mensagem é recebida no tópico 'key'. Ela processa essa mensagem, extrai as teclas pressionadas e calcula as velocidades linear e angular correspondentes.

Linha 33: Chama a função readOutputKeyForGazeboCar, passando a lista de teclas pressionadas como argumento. Esta função processa as teclas e retorna as velocidades linear (Lx) e angular (Az).

Linha 34: publish(Lx,Az): Chama a função publish, passando as velocidades linear e angular calculadas como argumentos. Esta função publica essas velocidades no tópico /demo/cmd_demo, controlando assim o movimento do robô.

```python!=36
def publish(Lx,Az):
    global pub
    msg = Twist()
    msg.linear.x = float(Lx)
    msg.angular.z = float(Az)
    if not ((pub is None)):
        pub.publish(msg)
```

Essa função tem como objetivo publicar mensagens Twist contendo velocidades linear e angular em um tópico específico no ROS.


Linha 38: Cria uma nova mensagem Twist vazia.

Linha 39 e 40: msg.linear.x = float(Lx): Define a velocidade linear da mensagem como o valor contido na variável Lx e Az, convertendo-a para um número de ponto flutuante.

Linha 41: Verifica se pub não é None, ou seja, se o publicador foi inicializado corretamente. Isso é uma verificação de segurança para garantir que a publicação seja feita apenas se o publicador estiver disponível.

Linha 42: Publica a mensagem msg no tópico associado ao publicador pub. Esta linha é a responsável por enviar a mensagem Twist com as velocidades linear e angular especificadas para outros nós no sistema ROS.


```python!=44
def main(args=None):
    global pub
    
    rclpy.init(args=args)    
    node = Node("key_node")

    pub = node.create_publisher(Twist, '/demo/cmd_demo', 1)

    node.create_subscription(String,'/key',keyCallback,1)
    
    rclpy.spin(node)

main()
```

A função main é responsável por inicializar o ambiente ROS, criar e configurar um nó ROS, criar publicadores e assinantes necessários, e iniciar o loop principal do ROS para o nó criado. 

Linha 47: Inicializa o ROS. Isso configura o ambiente ROS e prepara o sistema para usar o ROS.

Linha 48: Cria um novo nó ROS com o nome "key_node". Este nó será responsável por receber comandos do teclado e publicar mensagens Twist correspondentes.

Linha 50: Cria um publicador (publisher) ROS que publica mensagens Twist no tópico /demo/cmd_demo. Este tópico é usado para enviar comandos de velocidade para o robô simulado.

Linha 52: Cria um assinante (subscriber) ROS que assina o tópico /key para receber mensagens do tipo String. Quando uma mensagem é recebida neste tópico, a função keyCallback é chamada para processar a mensagem.

Linha 54: Inicia o loop principal do ROS para o nó node. Este loop permite que o nó responda a eventos ROS, como mensagens recebidas ou ações programadas.

Finalmete a função main é chamada na linha 56.


## 5.5 Criando um novo nó no mesmo pacote (joy)
AQUIIII


----
outro artigo:

Criando um nó:

```bash
user@humbleV1:~/ros2_ws/src/Gazebo_car_pkg/Gazebo_car_pkg$ touch Joy.py 
```

Este código é um exemplo de um nó em ROS escrito em Python. Sera o nó joy



```bash
user@humbleV1:~/ros2_ws/src/Gazebo_car_pkg/Gazebo_car_pkg$ cd ~/ros2_ws
```

- Depois de feito isso você terá que mudar o arquivo “package.xml”,adicionando as dependencias do seu código.
 
Ele estará assim:
```xml=7
...
<license>Apache-2.0</license>

<test_depend>ament_copyright</test_depend>
...
```
- terá que adicionar 3 linhas entre essas duas linhas, ficando assim:

```xml=7
...
<license>Apache-2.0</license>

<exec_depend>rclpy</exec_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>sensor_msgs</exec_depend>

<test_depend>ament_copyright</test_depend>
<test_depend>ament_flake8</test_depend>
...
```


- E, por fim, você terá que modificar o setup.py:

```python!=1
...

tests_require=['pytest'],
  entry_points={
    'console_scripts': [
        'key_node = key_pkg.node_pub_key:main',
    ],
  },
)
```

- O setup.py é quem diz como o colcon irá construir seu nó.

- Será necessário instalar as dependencias do seu programa agora. Também será necessário especificar uma biblioteca, pelo uso padrão do Humble.

```bash
user@humbleV1:~/ros2_ws$ pip3 install pynput
user@humbleV1:~/ros2_ws$ rosdep update 
user@humbleV1:~/ros2_ws$ rosdep install -i --from-path src --rosdistro humble -y
```
Agora você já pode buildar seu nó. Para isso rode:

```bash
user@humbleV1:~/ros2_ws$ colcon build -symlink-install ```








## 5.5 instalando e executando



