Criar um script Python no ROS2

Neste tutorial, veremos como criar e executar scripts Python no ROS2. Isto permitir-lhe-á criar os seus próprios nós e começar a desenvolver com o ROS.

Criar um espaço de trabalho

Uma boa prática para desenvolver sob o ROS2 é criar espaços de trabalho nos quais instalar os pacotes desejados, separados da instalação principal.

Instalação do Colcon

sudo apt install python3-colcon-common-extensions

Criar pasta

mkdir -p ~/tuto_ws/src
cd ~/tuto_ws

Em seguida, copie um projeto de exemplo, como o tutorial que contém o turtlesim

git clone https://github.com/ros/ros_tutorials.git -b iron

Em seguida, construa o projeto utilizando o comando

colcon build

Para carregar o seu espaço de trabalho num novo terminal

source /opt/ros/iron/setup.bash
cd ~/ros2_ws
source install/local_setup.bash

N.B.: pode colocar os seguintes comandos num ros_profile para a fonte num único comando “source ros_profile”

Criar um pacote python

O ROS2 oferece uma ferramenta simples para criar a arquitetura de arquivos de um pacote Python ou C++. É necessário especificar pelo menos o nome do pacote (my_package). Também é possível dar o nome do nó.

No diretório ~/ros2_sw/src, introduza o seguinte comando

ros2 pkg create --build-type ament_python --license Apache-2.0 --node-name my_node my_package --license Apache-2.0

o script python está agora no diretório ~/ros2_ws/src/my_package/my_package

Uma vez criado o pacote, pode adicionar tantos nós quantos quiser a esta pasta.

Terá de atualizar os ficheiros setup.py e package.xml com as dependências e os pontos de entrada

Instalação de dependências

Antes de compilar o projeto, é aconselhável resolver as dependências.

rosdep install -i --from-path src --rosdistro iron -y

Compilar o projeto

É possível compilar todo o projeto ou selecionar um pacote específico

colcon build --packages_select my_package --symlink-install

N.B.: symlink-install significa que não tens de recompilar sempre que alteras o script Python.

Lançar o nó

Depois de o pacote ter sido compilado, pode iniciar o nó com o comando

ros2 run my_package my_node

O código básico para criar um nó

def main():
    print('Hi from my_package.')

if __name__ == '__main__':
    main()

Quando iniciado, este código apresenta simplesmente o texto da impressão

Criar um editor e um subscritor para o Turtlesim

Neste exemplo, veremos como criar um pacote com dois nós, um controlando a tartaruga e o outro lendo a posição.

Criação de pacotes

ros2 pkg create --build-type ament_python --license Apache-2.0 --node-name projects control_turtle --license Apache-2.0

Aqui está o ficheiro de arquitetura desejado

.
├── LICENSE
├── package.xml
├── projects
│   ├── control_turtle.py
│   └── read_turtle.py
├── setup.cfg
└── setup.py

Adicionar scripts Python

• Editor controlo_tartaruga.py

Importamos o tipo de mensagem a publicar Torcer para controlar a velocidade da tartaruga

Ligamo-nos ao tópico especificando o nome (‘/turtle1/cmd_vel’) e o tipo de dados (Twist)

Criamos um temporizador que nos permitirá publicar regularmente sobre o tema (create_timer)

Finalmente, publicamos a linha e a velocidade angular

#!/usr/bin/env python
# -*- coding: utf-8 -*-
#
#  control_turtle.py


import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist

import os

# Node should have the same name as the node file
nodename= os.path.splitext(os.path.basename(__file__))[0]

class TurtleController(Node):

	def __init__(self):
		super().__init__(nodename)
		self.get_logger().info(nodename+" started")
		self.publisher_ = self.create_publisher(
			Twist,
			'/turtle1/cmd_vel',
			10)
		timer_period = 0.5  # seconds
		self.timer = self.create_timer(timer_period, self.timer_callback)
		self.i = 0

	def timer_callback(self):
		#msg = String()
		#msg.data = 'Hello World: %d' % self.i
		msg = Twist()
		msg.linear.x = 1.0
		msg.angular.z = 0.5
		self.publisher_.publish(msg)
		self.get_logger().info('Publishing: "%s"' % msg)
		self.i += 1


def main(args=None):
	rclpy.init(args=args)

	turtle_controller = TurtleController()
	try:
		rclpy.spin(turtle_controller)
		turtle_controller.destroy_node()
		rclpy.shutdown()
	except:
		turtle_controller.get_logger().info(nodename+" stopped")


if __name__ == '__main__':
	main()

• Assinante read_turtle.py

Importar o tipo de mensagem a publicar Pose

Ligamo-nos ao tópico especificando o nome (‘/turtle1/pose’) e o tipo de dados (Pose)

Escrevemos a função de escuta que é executada quando estão disponíveis novos dados sobre o tema

Criamos um temporizador que nos permitirá apresentar os valores com a frequência pretendida

Finalmente, publicamos a linha e a velocidade angular

#!/usr/bin/env python
# -*- coding: utf-8 -*-
#
#  control_turtle.py


import rclpy #rospy
from rclpy.node import Node
from turtlesim.msg import Pose


import os

# Node should have the same name as the node file
nodename= os.path.splitext(os.path.basename(__file__))[0]

class TurtleReader(Node):

	def __init__(self):
		super().__init__(nodename)
		self.get_logger().info(nodename+" started")
		self.subscription = self.create_subscription(
			Pose,
			'/turtle1/pose',
			self.listener_callback,
			10)
		self.subscription  # prevent unused variable warning
		self.timer = self.create_timer(2, self.timer_callback)
		self.msg = None
		
	def listener_callback(self, msg):
		self.msg = msg
		
	def timer_callback(self):
		self.get_logger().info("I read %s" % self.msg )
		


def main(args=None):
	rclpy.init(args=args)

	turtle_reader = TurtleReader()
	try:
		rclpy.spin(turtle_reader)
		turtle_reader.destroy_node()
		rclpy.shutdown()
	except:
		turtle_reader.get_logger().info(nodename+" stopped")





if __name__ == '__main__':
	main()

Modificar setup.py

Nos pontos de entrada do ficheiro setup.py, é necessário especificar os nomes dos nós e a função a executar (main)

entry_points={
        'console_scripts': [
                'control_turtle = projects.control_turtle:main',
                'read_turtle = projects.read_turtle:main',
        ],
},

Compilação do pacote

colcon build --packages_select projects --symlink-install

Lançamento de nós

Para lançar um nó num terminal, é necessário obter a fonte do projeto

source /opt/ros/iron/setup.bash
cd ~/ros2_ws
source install/local_setup.bash

Em três terminais diferentes, execute os seguintes comandos

ros2 run turtlesim turtlesim_node #terminal1
ros2 run projects read_turtle #terminal2
ros2 run projects control_turtle #terminal3

Resultados

Vê-se que a tartaruga descreve a forma de um círculo e a mudança de posição no centro do assinante.

Lançar um nó com argumentos

Os nós podem ser configurados em tempo de execução. Neste exemplo, vamos definir parâmetros de entrada para a velocidade linear e angular. Por isso, vamos modificar o código no editor acima

Na inicialização do nó, declaramos os parâmetros com os valores por defeito

    self.declare_parameter('x', 1.0)
    self.declare_parameter('z', 0.5)

Podemos então obter o valor destes parâmetros e armazená-los em variáveis que utilizamos na função de retorno de chamada

    self.linx = self.get_parameter('linx').get_parameter_value().double_value
    self.angz = self.get_parameter('angz').get_parameter_value().double_value

Agora é possível iniciar um nó com e sem o

ros2 run projects circle_turtle #default param
ros2 run projects circle_turtle --ros-args -p linx:=0.5 -p angz:=1.0

Código completo

#!/usr/bin/env python
# -*- coding: utf-8 -*-
#
#  control_turtle.py
import rclpy #rospy
from rclpy.node import Node
from std_msgs.msg import String

from turtlesim.msg import Pose
from geometry_msgs.msg import Twist

import os

nodename= os.path.splitext(os.path.basename(__file__))[0]

class TurtleController(Node):

	def __init__(self):
		super().__init__(nodename)
		self.get_logger().info(nodename+" started")
		self.publisher_ = self.create_publisher(
			Twist,
			'/turtle1/cmd_vel',
			10)
			
		self.declare_parameter('linx', 1.0)
		self.declare_parameter('angz', 0.5)
		self.linx = self.get_parameter('linx').get_parameter_value().double_value
		self.angz = self.get_parameter('angz').get_parameter_value().double_value
			
		timer_period = 0.5  # seconds
		self.timer = self.create_timer(timer_period, self.timer_callback)
		self.i = 0

	def timer_callback(self):
		#msg = String()
		#msg.data = 'Hello World: %d' % self.i
		msg = Twist()
		msg.linear.x = self.linx
		msg.angular.z = self.angz
		self.publisher_.publish(msg)
		self.get_logger().info('Publishing: "%s"' % msg)
		self.i += 1


def main(args=None):
	#os.system('ros2 service call /reset std_srvs/srv/Empty') #reset pos
	rclpy.init(args=args)

	turtle_controller = TurtleController()
	try:
		rclpy.spin(turtle_controller)
		turtle_controller.destroy_node()
		rclpy.shutdown()
	except:
		turtle_controller.get_logger().info(nodename+" stopped")


if __name__ == '__main__':
	main()

Abrir os vários terminais com um script

Para poupar tempo durante o desenvolvimento, pode abrir terminais e lançar nós a partir de um único script bash

#open terminal and run turtlesim
gnome-terminal -- $SHELL -c "source ros_profile && ros2 run turtlesim turtlesim_node;exec $SHELL"
gnome-terminal -- $SHELL -c "source ros_profile && ros2 run projects control_turtle;exec $SHELL"

Resolução de problemas

• Descontinuação do SetupTools

Verificar a versão do setuptools

python3
import setuptools
setuptools.__version__

‘59.6.0’

Instalar o pip

sudo apt install python3-pip

depois instale a versão 58.2.0 do setuptools

python3 -m pip install setuptools==58.2.0

Fontes

• Instalar o ROS2
• Usando pacotes Python no ROS2
• Controlador PID TurtleSim