Utiliser URDF avec `robot_state_publisher`

Objectif : Simulez un robot marcheur modélisé en URDF et visualisez-le dans Rviz.

Niveau du didacticiel : Intermédiaire

Durée : 15 minutes

Contenu

Arrière-plan
Conditions préalables
Tâches
Résumé

Arrière-plan 

Ce didacticiel vous montrera comment modéliser un robot marcheur, publier l’état en tant que message tf2 et afficher la simulation dans Rviz. Tout d’abord, nous créons le modèle URDF décrivant l’assemblage du robot. Ensuite, nous écrivons un nœud qui simule le mouvement et publie le JointState et les transformations. Nous utilisons ensuite robot_state_publisher pour publier l’état complet du robot dans /tf2.

Conditions préalables 

refze <http://index.rus.org/b/refzie/> a

Comme toujours, n’oubliez pas de sourcer ROS 2 dans chaque nouveau terminal que vous ouvrez.

Tâches 

1 Créer un package 

mkdir -p ~/second_ros2_ws/src  # change as needed
cd ~/second_ros2_ws/src
ros2 pkg create urdf_tutorial_r2d2 --build-type ament_python --dependencies rclpy
cd urdf_tutorial_r2d2

Vous devriez maintenant voir un dossier urdf_tutorial_r2d2. Ensuite, vous y apporterez plusieurs modifications.

2 Créer le fichier URDF 

Créez le répertoire où nous allons stocker certains actifs :

mkdir -p urdf

Téléchargez le fichier URDF et enregistrez-le sous ~/second_ros2_ws/src/urdf_tutorial_r2d2/urdf/r2d2.urdf.xml. Téléchargez le fichier de configuration Rviz et enregistrez-le sous ~/second_ros2_ws/src/urdf_tutorial_r2d2/urdf/r2d2.rviz.

3 Publier l’état 

Nous avons maintenant besoin d’une méthode pour spécifier dans quel état se trouve le robot. Pour ce faire, nous devons spécifier les trois articulations et l’odométrie globale.

Lancez votre éditeur préféré et collez le code suivant dans ~/second_ros2_ws/src/urdf_tutorial_r2d2/urdf_tutorial_r2d2/state_publisher.py

from math import sin, cos, pi
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile
from geometry_msgs.msg import Quaternion
from sensor_msgs.msg import JointState
from tf2_ros import TransformBroadcaster, TransformStamped

class StatePublisher(Node):

    def __init__(self):
        rclpy.init()
        super().__init__('state_publisher')

        qos_profile = QoSProfile(depth=10)
        self.joint_pub = self.create_publisher(JointState, 'joint_states', qos_profile)
        self.broadcaster = TransformBroadcaster(self, qos=qos_profile)
        self.nodeName = self.get_name()
        self.get_logger().info("{0} started".format(self.nodeName))

        degree = pi / 180.0
        loop_rate = self.create_rate(30)

        # robot state
        tilt = 0.
        tinc = degree
        swivel = 0.
        angle = 0.
        height = 0.
        hinc = 0.005

        # message declarations
        odom_trans = TransformStamped()
        odom_trans.header.frame_id = 'odom'
        odom_trans.child_frame_id = 'axis'
        joint_state = JointState()

        try:
            while rclpy.ok():
                rclpy.spin_once(self)

                # update joint_state
                now = self.get_clock().now()
                joint_state.header.stamp = now.to_msg()
                joint_state.name = ['swivel', 'tilt', 'periscope']
                joint_state.position = [swivel, tilt, height]

                # update transform
                # (moving in a circle with radius=2)
                odom_trans.header.stamp = now.to_msg()
                odom_trans.transform.translation.x = cos(angle)*2
                odom_trans.transform.translation.y = sin(angle)*2
                odom_trans.transform.translation.z = 0.7
                odom_trans.transform.rotation = \
                    euler_to_quaternion(0, 0, angle + pi/2) # roll,pitch,yaw

                # send the joint state and transform
                self.joint_pub.publish(joint_state)
                self.broadcaster.sendTransform(odom_trans)

                # Create new robot state
                tilt += tinc
                if tilt < -0.5 or tilt > 0.0:
                    tinc *= -1
                height += hinc
                if height > 0.2 or height < 0.0:
                    hinc *= -1
                swivel += degree
                angle += degree/4

                # This will adjust as needed per iteration
                loop_rate.sleep()

        except KeyboardInterrupt:
            pass

def euler_to_quaternion(roll, pitch, yaw):
    qx = sin(roll/2) * cos(pitch/2) * cos(yaw/2) - cos(roll/2) * sin(pitch/2) * sin(yaw/2)
    qy = cos(roll/2) * sin(pitch/2) * cos(yaw/2) + sin(roll/2) * cos(pitch/2) * sin(yaw/2)
    qz = cos(roll/2) * cos(pitch/2) * sin(yaw/2) - sin(roll/2) * sin(pitch/2) * cos(yaw/2)
    qw = cos(roll/2) * cos(pitch/2) * cos(yaw/2) + sin(roll/2) * sin(pitch/2) * sin(yaw/2)
    return Quaternion(x=qx, y=qy, z=qz, w=qw)

def main():
    node = StatePublisher()

if __name__ == '__main__':
    main()

4 Créer un fichier de lancement 

Créez un nouveau dossier ~/second_ros2_ws/src/urdf_tutorial_r2d2/launch. Ouvrez votre éditeur et collez le code suivant, en l’enregistrant sous ~/second_ros2_ws/src/urdf_tutorial_r2d2/launch/demo.launch.py

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node

def generate_launch_description():

    use_sim_time = LaunchConfiguration('use_sim_time', default='false')

    urdf_file_name = 'r2d2.urdf.xml'
    urdf = os.path.join(
        get_package_share_directory('urdf_tutorial_r2d2'),
        urdf_file_name)
    with open(urdf, 'r') as infp:
        robot_desc = infp.read()

    return LaunchDescription([
        DeclareLaunchArgument(
            'use_sim_time',
            default_value='false',
            description='Use simulation (Gazebo) clock if true'),
        Node(
            package='robot_state_publisher',
            executable='robot_state_publisher',
            name='robot_state_publisher',
            output='screen',
            parameters=[{'use_sim_time': use_sim_time, 'robot_description': robot_desc}],
            arguments=[urdf]),
        Node(
            package='urdf_tutorial_r2d2',
            executable='state_publisher',
            name='state_publisher',
            output='screen'),
    ])

5 Modifiez le fichier setup.py 

Vous devez indiquer à l’outil de construction colcon comment installer votre package Python. Modifiez le fichier ~/second_ros2_ws/src/urdf_tutorial_r2d2/setup.py comme suit :

inclure ces déclarations d’importation

import os
from glob import glob
from setuptools import setup
from setuptools import find_packages

ajoutez ces 2 lignes à l’intérieur de data_files

data_files=[
  ...
  (os.path.join('share', package_name), glob('launch/*.py')),
  (os.path.join('share', package_name), glob('urdf/*'))
],

modifiez la table entry_points afin de pouvoir exécuter ultérieurement “state_publisher” à partir d’une console

'console_scripts': [
    'state_publisher = urdf_tutorial_r2d2.state_publisher:main'
],

Enregistrez le fichier setup.py avec vos modifications.

6 Installez le paquet 

cd ~/second_ros2_ws
colcon build --symlink-install --packages-select urdf_tutorial_r2d2
source install/setup.bash

7 Visualiser les résultats 

Lancer le package

ros2 launch urdf_tutorial_r2d2 demo.launch.py

Ouvrez un nouveau terminal, exécutez Rviz en utilisant

rviz2 -d ~/second_ros2_ws/install/urdf_tutorial_r2d2/share/urdf_tutorial_r2d2/r2d2.rviz

Voir le Guide de l’utilisateur pour plus de détails sur l’utilisation de Rviz.

Résumé 

Vous avez créé un nœud d’éditeur JointState et l’avez couplé avec robot_state_publisher pour simuler un robot marchant. Le code utilisé dans ces exemples peut être trouvé ici.

Le crédit est donné aux auteurs de ce tutoriel ROS 1 dont une partie du contenu a été réutilisé.

Utiliser URDF avec robot_state_publisher

Utiliser URDF avec `robot_state_publisher`