Créer un script Python sous ROS2

Étiquettes : Linux, Python, Raspberry PI, ROS

Dans ce tutoriel, nous allons voir comment créer et lancer des script Python sous ROS2. Vous pourrez ainsi créer vos propres noeuds et commencer à développer sous ROS.

Créer un espace de travail

Une bonne pratique pour développer sous ROS2 est de créer des workspaces dans lesquels on va installer les paquets désirés, séparés de l’installation principale.

Installation de colcon

sudo apt install python3-colcon-common-extensions

Créer le dossier

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

Copier ensuite un projet type, comme tutoriel contenant turtlesim

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

Puis construisez le projet à l’aide de la commande

colcon build

Pour charger votre workspace dans un nouveau terminal

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

N.B.: vous pouvez mettre les commandes suivantes dans un fichier ros_profile pour le source en une seule commande « source ros_profile »

Créer un Package python

ROS2 offre un outil simple pour créer l’architecture de fichiers d’un paquets Python ou C++. Vous devez alors spécifier au moins le nom du paquet (my_paquet). Il est possible également de donner le nom du nœud.

Dans le répertoire ~/ros2_sw/src, entrez la commande suivante

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

le script python se trouve alors dans le répertoire ~/ros2_ws/src/my_package/my_package

Une fois le paquet créé, vous pouvez ajouter autant de nœud que vous le souhaitez dans ce dossier.

Il faudra penser à mettre à jour les fichiers setup.py et package.xml avec les dépendances et points d’entrée

Installer les dépendances

Avant de compiler le projet, il est conseillé de résoudre les dépendances.

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

Compiler le projet

Il est possible de compiler le projet entier ou de sélectionner un paquet en particulier

colcon build --packages_select my_package --symlink-install

N.B.: symlink-install permet de ne pas recompiler à chaque changement du script Python

Lancer le nœud

Une fois le paquet compilé, vous pouvez lancer le nœud à partir de la commande

ros2 run my_package my_node

Le code de base lors de la création d’un nœud

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

if __name__ == '__main__':
    main()

Au lancement ce code affichera simplement le text du print

Création d’un Publisher et d’un Subscriber pour Turtlesim

Nous allons voir dans cet exemple, comment créer un paquet avec deux nœuds, l’un venant contrôler la tortue, l’autre, venant lire la position.

Création du paquet

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

Voici l’architecture de fichiers désiré

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

Ajouter les scripts Python

Publisher control_turtle.py

On importe le type de message à publier Twist pour le contrôle de la vitesse de la tortue

On se connecte au topic en précisant le nom (‘/turtle1/cmd_vel’) et le type de donnée (Twist)

On crée un timer qui nous permettra de publier régulièrement sur le topic (create_timer)

Enfin on publie la vitesse linéaire et angulaire

#!/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()

Subscriber read_turtle.py

On importe le type de message à publier Pose

On se connecte au topic en précisant le nom (‘/turtle1/pose’) et le type de donnée (Pose)

On écrit la fonction d’écoute qui s’exécute lorsqu’une une nouvelle donnée est disponible sur le topic

On crée un timer qui nous permettra d’afficher les valeurs à la fréquence désirée

Enfin on publie la vitesse linéaire et angulaire

#!/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()

Modifier le fichier setup.py

Dans les points d’entrée du fichier setup.py, vous devez spécifier les noms des noeuds ainsi que la fonction à lancer (main)

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

Compiler le paquet

colcon build --packages_select projects --symlink-install

Lancer les noeuds

Pour lancer un noeud dans un terminal, vous devez sourcer le projet

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

Dans trois terminaux différents, lancez les commandes suivantes

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

Résultat

Vous pouvez voir la tortue décrire la forme d’un cercle et la position évoluer dans la fenêtre du subscriber

Lancer un noeud avec des arguments

Il est possible de configurer les noeuds lors de leur exécution. Dans cet exemple, nous allons définir des paramètre d’entrée pour la vitesse linéaire et angulaire. Nous allons donc modifier le code du Publieur ci-dessus

Dans l’initialisation du noeud, nous déclarons les paramètres avec les valeurs par défaut

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

Nous pouvons ensuite récupérer la valeur de ces paramètres pour les stocker dans des variables que nous utilisons dans la fonction callback

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

Vous pouvez désormais lancer un noeud avec et sans paramètre

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

Code complet

#!/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()

Ouvrir les différents terminaux avec un script

Pour gagner du temps lors de votre développement, vous pouvez ouvrir les terminaux et lancer les noeuds à partir d’un seul 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"

Troubleshoot

SetupTools deprecation

Vérifier la version setuptools

python3
import setuptools
setuptools.__version__

‘59.6.0’

Installer pip

sudo apt install python3-pip

puis installer la version 58.2.0 de setuptools

python3 -m pip install setuptools==58.2.0