Lab: Introduction to ROS2 tools


Today's Lesson

Practice :

RVIZ

rviz is a tool to simulate, publish and visualize ROS 2 sensor data.

Let's simulate a point cloud sensor : pointCloudPublisher.py 

        import numpy as np

        import rclpy
        from rclpy.node import Node
        from sensor_msgs.msg import PointCloud2
        from sensor_msgs.msg import PointField
        from sensor_msgs_py import point_cloud2
        from std_msgs.msg import Header


        class PointCloudPublisher(Node):

            rate = 30
            moving = True
            width = 100
            height = 100

            header = Header()
            header.frame_id = 'map'

            dtype = PointField.FLOAT32
            point_step = 16
            fields = [PointField(name='x', offset=0, datatype=dtype, count=1),
                      PointField(name='y', offset=4, datatype=dtype, count=1),
                      PointField(name='z', offset=8, datatype=dtype, count=1),
                      PointField(name='intensity', offset=12, datatype=dtype, count=1)]

            def __init__(self):
                super().__init__('pc_publisher')
                self.publisher_ = self.create_publisher(PointCloud2, 'test_cloud', 10)
                timer_period = 1 / self.rate
                self.timer = self.create_timer(timer_period, self.timer_callback)
                self.counter = 0

            def timer_callback(self):
                self.header.stamp = self.get_clock().now().to_msg()
                x, y = np.meshgrid(np.linspace(-2, 2, self.width), np.linspace(-2, 2, self.height))
                z = 0.5 * np.sin(2*x-self.counter/10.0) * np.sin(2*y)
                points = np.array([x, y, z, z]).reshape(4, -1).T
                pc2_msg = point_cloud2.create_cloud(self.header, self.fields, points)
                self.publisher_.publish(pc2_msg)

                if self.moving:
                    self.counter += 1


        def main(args=None):
            rclpy.init(args=args)
            pc_publisher = PointCloudPublisher()
            rclpy.spin(pc_publisher)
            pc_publisher.destroy_node()
            rclpy.shutdown()


        if __name__ == '__main__':
            main()
Add rclpy, sensor_msgs_py, python3-numpy, sensor_msgs, std_msgs dependency in package.xml <build_type>ament_python</build_type> ... <depend>rclpy<</depend> <depend>sensor_msgs_py</depend> <depend>python3-numpy</depend> <depend>sensor_msgs</depend> <depend>std_msgs</depend> Build only the iml_buche_test_2 package : 

    $ colcon build --packages-select iml_buche_test_2
While running : 

          $ rviz2
Add "pointcloud2"


PointCloundRviz


URDF

URDF is used to describe kinematic and dynamic properties of a robot. It is written in XML and commonly used in ROS applications.
Let's try to code the following structure : IMG
Create a directory for urdf files (inside the package): 

	mkdir urdf
Create my_robot.urdf and place it in ros2_ws/src/iml_buche_test_2/urdf/ <robot name="test_robot"> <link name="link1" /> <link name="link2" /> <link name="link3" /> <link name="link4" /> <joint name="joint1" type="continuous"> <parent link="link1"/> <child link="link2"/> </joint> <joint name="joint2" type="continuous"> <parent link="link1"/> <child link="link3"/> </joint> <joint name="joint3" type="continuous"> <parent link="link3"/> <child link="link4"/> </joint> </robot> Check if the file is correct 

        $ check_urdf my_robot.urdf
Add the dimensions <robot name="test_robot"> <link name="link1" /> <link name="link2" /> <link name="link3" /> <link name="link4" /> <joint name="joint1" type="continuous"> <parent link="link1"/> <child link="link2"/> <origin xyz="5 3 0" rpy="0 0 0" /> </joint> <joint name="joint2" type="continuous"> <parent link="link1"/> <child link="link3"/> <origin xyz="-2 5 0" rpy="0 0 1.57" /> </joint> <joint name="joint3" type="continuous"> <parent link="link3"/> <child link="link4"/> <origin xyz="5 0 0" rpy="0 0 -1.57" /> </joint> </robot> Completing the Kinematics : which axis the joints rotate <robot name="test_robot"> <link name="link1" /> <link name="link2" /> <link name="link3" /> <link name="link4" /> <joint name="joint1" type="continuous"> <parent link="link1"/> <child link="link2"/> <origin xyz="5 3 0" rpy="0 0 0" /> <axis xyz="-0.9 0.15 0" /> </joint> <joint name="joint2" type="continuous"> <parent link="link1"/> <child link="link3"/> <origin xyz="-2 5 0" rpy="0 0 1.57" /> <axis xyz="-0.707 0.707 0" /> </joint> <joint name="joint3" type="continuous"> <parent link="link3"/> <child link="link4"/> <origin xyz="5 0 0" rpy="0 0 -1.57" /> <axis xyz="0.707 -0.707 0" /> </joint> </robot> Visualize the URDF using graphiz 

          $ urdf_to_graphviz my_robot.urdf
          $ evince test_robot.pdf


LAUNCH

A launch file helps to run a complex ROS 2 system.

Modify setup.py to add path to launch files (+ udrf files + world files) 

from setuptools import setup
import os
from glob import glob

package_name = 'iml_buche_test_2'

setup(
    name=package_name,
    version='0.0.0',
    packages=[package_name],
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
        (os.path.join('share', package_name), glob('launch/*.py')),
        (os.path.join('share', package_name), glob('urdf/*')),
        (os.path.join('share', package_name), glob('worlds/*')),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='buche',
    maintainer_email='buche@todo.todo',
    description='TODO: Package description',
    license='TODO: License declaration',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'test_python_receiver_node = iml_buche_test_2.receiver:main',
            'test_python_publisher_node = iml_buche_test_2.publisher:main',
            'test_python_pointCloud_node = iml_buche_test_2.pointCloudPublisher:main'
        ],
    },
)

Launch file - Nodes

A launch file enables you to launch multiple ROS 2 nodes at the same time. 

    ros2 launch package_name launch_file_name
Create a directory for launch files (inside the package): 

	mkdir launch
Create a new file, iml_test.launch.py (inside the launch directory) with the following skeleton: 

import launch
import launch_ros

def generate_launch_description():
    return launch.LaunchDescription([
        # launch actions here...
    ])
Try to make a launch file starting Publisher and Receiver 

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, ExecuteProcess
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch.launch_description_sources import PythonLaunchDescriptionSource

def generate_launch_description():
  package_dir=get_package_share_directory('pkgname')
  urdf = os.path.join(package_dir,'urdf_file.urdf')

  return LaunchDescription([
    #   publishes TF for links of the robot without joints
        Node(
            package='iml_buche_test_2',
            executable='test_python_receiver_node',
            output="screen"
        ),
        Node(
            package='iml_buche_test_2',
            executable='test_python_publisher_node',
            output="screen"
        )
  ])
After launching the launch.py file, the output should be similar to the following:
PointCloundRviz
© Cedric Buche