Creación de un script Python en ROS2

En este tutorial, veremos cómo crear y ejecutar scripts Python bajo ROS2. Esto te permitirá crear tus propios nodos y empezar a desarrollar bajo ROS.

Crear un espacio de trabajo

Una buena práctica para desarrollar bajo ROS2 es crear espacios de trabajo en los que instalar los paquetes deseados, separados de la instalación principal.

Instalación Colcon

sudo apt install python3-colcon-common-extensions

Crear carpeta

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

A continuación, copie un proyecto de ejemplo, como el tutorial que contiene turtlesim

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

A continuación, compile el proyecto mediante el comando

colcon build

Para cargar su espacio de trabajo en un nuevo terminal

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

N.B.: puede poner los siguientes comandos en un ros_profile para la fuente en un solo comando «source ros_profile»

Creación de un paquete python

ROS2 ofrece una sencilla herramienta para crear la arquitectura de ficheros de un paquete Python o C++. Debe especificar al menos el nombre del paquete (mi_paquete). También puede dar el nombre del nodo.

En el directorio ~/ros2_sw/src, introduzca el siguiente comando

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

el script python está ahora en el directorio ~/ros2_ws/src/my_package/my_package

Una vez creado el paquete, puede añadir tantos nodos como desee a esta carpeta.

Tendrás que actualizar los archivos setup.py y package.xml con las dependencias y puntos de entrada

Instalación de dependencias

Antes de compilar el proyecto, es aconsejable resolver las dependencias.

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

Compilar el proyecto

Puede compilar todo el proyecto o seleccionar un paquete concreto

colcon build --packages_select my_package --symlink-install

N.B.: symlink-install significa que no tienes que recompilar cada vez que cambias el script Python.

Lanzar el nudo

Una vez compilado el paquete, puede lanzar el nodo con el comando

ros2 run my_package my_node

El código básico al crear un nodo

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

if __name__ == '__main__':
    main()

Cuando se ejecute, este código simplemente mostrará el texto de la impresión

Creación de un editor y un suscriptor para Turtlesim

En este ejemplo, veremos cómo crear un paquete con dos nodos, uno controlando la tortuga y el otro leyendo la posición.

Creación de paquetes

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

He aquí la arquitectura de archivos deseada

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

Añadir scripts Python

• Editor control_turtle.py

Importamos el tipo de mensaje a publicar Twist para controlar la velocidad de la tortuga

Nos conectamos al tema especificando el nombre (‘/turtle1/cmd_vel’) y el tipo de datos (Twist)

Creamos un temporizador que nos permitirá publicar regularmente sobre el tema (create_timer)

Por último, publicamos la línea y la velocidad 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()

• Suscriptor read_turtle.py

Importar el tipo de mensaje a publicar Pose

Nos conectamos al tema especificando el nombre (‘/turtle1/pose’) y el tipo de datos (Pose)

Escribimos la función de escucha que se ejecuta cuando hay nuevos datos disponibles sobre el tema

Creamos un temporizador que nos permitirá visualizar los valores a la frecuencia deseada

Por último, publicamos la línea y la velocidad 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

En los puntos de entrada del archivo setup.py, debes especificar los nombres de los nodos y la función a ejecutar (main)

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

Compilación del paquete

colcon build --packages_select projects --symlink-install

Nodos de lanzamiento

Para lanzar un nodo en un terminal, es necesario el código fuente del proyecto

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

En tres terminales diferentes, ejecute los siguientes comandos

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

Resultados

Puedes ver cómo la tortuga describe la forma de un círculo y el cambio de posición en el centro del abonado.

Lanzar un nodo con argumentos

Los nodos pueden configurarse en tiempo de ejecución. En este ejemplo, vamos a definir los parámetros de entrada para la velocidad lineal y angular. Así que vamos a modificar el código en el Editor de arriba

En la inicialización del nodo, declaramos los parámetros con los valores por defecto

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

A continuación, podemos recuperar el valor de estos parámetros y almacenarlos en variables que utilizamos en la función de devolución de llamada

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

Ahora puede lanzar un nodo con y sin la etiqueta

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 los distintos terminales con un script

Para ahorrar tiempo durante el desarrollo, puede abrir terminales y lanzar nodos desde un ú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"

Solución de problemas

• Caducidad de SetupTools

Compruebe la versión de setuptools

python3
import setuptools
setuptools.__version__

‘59.6.0’

Instalar pip

sudo apt install python3-pip

a continuación, instale la versión 58.2.0 de setuptools

python3 -m pip install setuptools==58.2.0

Fuentes

• Instalación de ROS2
• Uso de paquetes Python en ROS2
• Controlador PID TurtleSim