ROS2 第三讲基本操作

2023-10-18 15:32:16

ROS2 第三讲基本操作

创建工作区

source /opt/ros/foxy/setup.bash

这句命令的目的是让ros2开头的命令可以在终端使用。

mkdir -p ~/dev_ws/src
cd ~/dev_ws/src

在工作区中创建工作包(package)， package的创建可以使用CMake或者是Python，后面我们主要使用python.

# ros2 pkg create --build-type ament_python <package_name>
ros2 pkg create --build-type ament_python --node-name my_node my_package

上面的命令在工作区中创建了名为my_pacakge的package，后面的参数--node-name 为我们创建了一个名为my_node的Hello World的测试文件。执行上面的命令时，终端会显示：

going to create a new package
package name: my_package
destination directory: /home/user/dev_ws/src
package format: 3
version: 0.0.0
description: TODO: Package description
maintainer: [‘<name> <email>‘]
licenses: [‘TODO: License declaration‘]
build type: ament_python
dependencies: []
node_name: my_node
creating folder ./my_package
creating ./my_package/package.xml
creating source folder
creating folder ./my_package/my_package
creating ./my_package/setup.py
creating ./my_package/setup.cfg
creating folder ./my_package/resource
creating ./my_package/resource/my_package
creating ./my_package/my_package/__init__.py
creating folder ./my_package/test
creating ./my_package/test/test_copyright.py
creating ./my_package/test/test_flake8.py
creating ./my_package/test/test_pep257.py
creating ./my_package/my_package/my_node.py

创建完成后，还需要使用colcon来构建package:

colcon build --package-select my_package

参数--package-select 告诉系统只需要构建my_package即可，不加此参数，则工作区内的所有package一起构建。

要使用你的package及其中的可执行文件，需要在新的终端上面，在工作区目录下(本例中，即为~/dev_ws),执行:

. install/setup.bash

然后，我们就可以在此终端下执行类似如下命令:

# ros2 run <package_name> <executable_file>
ros2 run my_package my_node

终端会显示这个测试文件的输出结果

Hi from my_package.

订阅与发布topic

创建一个package:

cd ~/dev_ws/src
ros2 pkg create --build-type ament_python py_pubsub
cd py_pubsub/py_pubsub

编写发布者节点

在其中创建名为publisher_member_function.py的文件:

import rclpy
from rclpy.node import Node

from std_msgs.msg import String


class MinimalPublisher(Node):

    def __init__(self):
        super().__init__(‘minimal_publisher‘)
        self.publisher_ = self.create_publisher(String, ‘hello_world‘, 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
        self.publisher_.publish(msg)
        self.get_logger().info(‘Publishing: "%s"‘ % msg.data)
        self.i += 1


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

    minimal_publisher = MinimalPublisher()

    rclpy.spin(minimal_publisher)

    # Destroy the node explicitly
    # (optional - otherwise it will be done automatically
    # when the garbage collector destroys the node object)
    minimal_publisher.destroy_node()
    rclpy.shutdown()


if __name__ == ‘__main__‘:
    main()

代码解释

import rclpy
from rclpy.node import Node

from std_msgs.msg import String

引入python中操作ROS2的功能包rclpy, 后面的一句是导入内置的字符串消息类型，节点使用此数据类型在topic上面进行数据传递。

 super().__init__(‘minimal_publisher‘)

调用父类Node的构造函数来为节点命名，本例中即为:minimal_publisher.

self.publisher_ = self.create_publisher(String, ‘hello_world‘, 10)

声明此节点将在名为hello_world的topic上面发布消息，消息类型是String，而且设定消息队列大小为10。

self.timer = self.create_timer(timer_period, self.timer_callback)

上句是调用父类Node的函数创建计时器，此计时器会每隔timer_period时间(本例为0.5秒)调用回调函数(本例为self.timer_callback).

...
msg = String()
msg.data = ‘Hello World: %d‘ % self.i
self.publisher_.publish(msg)
...

上面第一句是创建String消息类型，为其data赋值，最后将其发布出去。

在main函数中，首先初始化库rclpy,然后，构造发布者。rclpy.spin(minimal_publisher) 这句开使执行发布者，而且程序一直会"阻塞"在这一句，后面语句不会执行，直到关闭。后面的语句是显式地毁掉节节点, 并关闭rclpy

发布者的程序就写完了，但让它真正可又跑起来，还需要对几个文件进行处理。首先来到dev_ws/src/py_pubsub/目录下，打开package.xml文件, 并将下面几句加进去:

<exec_depend>rclpy</exec_depend><exec_depend>std_msgs</exec_depend>

这是在为程序执行增加依赖。再打开setup.py文件，在entry_points的console_scripts中加入:

‘talker = py_pubsub.publisher_member_function:main‘,

编写订阅者节点

也是在dev_ws/src/py_pubsub/py_pubsub目录下创建subscriber_member_function.py：

import rclpy
from rclpy.node import Node

from std_msgs.msg import String


class MinimalSubscriber(Node):

    def __init__(self):
        super().__init__(‘minimal_subscriber‘)
        self.subscription = self.create_subscription(
            String,
            ‘hello_world‘,
            self.listener_callback,
            10)
        self.subscription  # prevent unused variable warning

    def listener_callback(self, msg):
        self.get_logger().info(‘I heard: "%s"‘ % msg.data)


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

    minimal_subscriber = MinimalSubscriber()

    rclpy.spin(minimal_subscriber)

    # Destroy the node explicitly
    # (optional - otherwise it will be done automatically
    # when the garbage collector destroys the node object)
    minimal_subscriber.destroy_node()
    rclpy.shutdown()


if __name__ == ‘__main__‘:
    main()

只是要注意topic的名称(本例中为hello_world)一定要与发布者发布消息使用的topic名称相一致。

在构建之前，需要检查是否缺少依赖项:

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

开始构建:

colcon build --package-select py_pubsub

然后，新打开终端,到dev_ws的目录下:

. install/setup.bash
ros2 run py_pubsub talker

会显示类似如下信息:

[INFO] [1630456461.097123166] [minimal_publisher]: Publishing: "Hello World: 0"
[INFO] [1630456461.587039320] [minimal_publisher]: Publishing: "Hello World: 1"
[INFO] [1630456462.086940655] [minimal_publisher]: Publishing: "Hello World: 2"
[INFO] [1630456462.586954812] [minimal_publisher]: Publishing: "Hello World: 3"
[INFO] [1630456463.087072928] [minimal_publisher]: Publishing: "Hello World: 4"
[INFO] [1630456463.587015133] [minimal_publisher]: Publishing: "Hello World: 5"

再新打开一个终端，到dev_ws目录下:

. install/setup.bash
ros2 run py_pubsub listener

会显示类似如下信息:

[INFO] [1630456623.240335705] [minimal_subscriber]: I heard: "Hello World: 30"
[INFO] [1630456623.740374180] [minimal_subscriber]: I heard: "Hello World: 31"
[INFO] [1630456624.240430432] [minimal_subscriber]: I heard: "Hello World: 32"
[INFO] [1630456624.740421047] [minimal_subscriber]: I heard: "Hello World: 33"
[INFO] [1630456625.240359291] [minimal_subscriber]: I heard: "Hello World: 34"
[INFO] [1630456625.740324980] [minimal_subscriber]: I heard: "Hello World: 35"

响应与请求服务

进入到dev_ws/src目录中，创建新的package:

ros2 pkg create --build-type ament_python py_srvcli --dependencies rclpy example_interfaces

参数--dependencies自动将发要的依赖添加到package.xml，example_interfaces是包含AddTwoInts.srv的package。其数据结构:

int64 a
int64 b
---
int64 sum

前两行是请求参数，虚线下面的是响应参数。因为使用了--dependencies参数，也不就用手动地将依赖添加到package.xml.

进入到dev_ws/src/py_srvcli/py_srvcli目录，在其中创建名为service_member_function.py:

from example_interfaces.srv import AddTwoInts

import rclpy
from rclpy.node import Node


class MinimalService(Node):

    def __init__(self):
        super().__init__(‘minimal_service‘)
        self.srv = self.create_service(AddTwoInts, ‘add_two_ints‘,                                                   self.add_two_ints_callback)

    def add_two_ints_callback(self, request, response):
        response.sum = request.a + request.b
        self.get_logger().info(‘Incoming request\na: %d b: %d‘ % (request.a, request.b))

        return response


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

    minimal_service = MinimalService()

    rclpy.spin(minimal_service)

    rclpy.shutdown()


if __name__ == ‘__main__‘:
    main()

经过学习上面topic的发布与订阅，现在对这个服务的代码的也较容易上手，此文件创建了名为add_two_ints的服务，此服务接收两个整数，然后返回这两个整数的和。

为使用ros2 run ... 命令支行上面的代码，需要将下面的内容添加到dev_ws/src/py_srvcli/setup.py的console_scripts中:

‘service = py_srvcli.service_member_function:main‘,

然后再进入到dev_ws/src/py_srvcli/py_srvcli目录，编写client_member_function.py:

import sys

from example_interfaces.srv import AddTwoInts
import rclpy
from rclpy.node import Node


class MinimalClientAsync(Node):

    def __init__(self):
        super().__init__(‘minimal_client_async‘)
        self.cli = self.create_client(AddTwoInts, ‘add_two_ints‘)
        while not self.cli.wait_for_service(timeout_sec=1.0):
            self.get_logger().info(‘service not available, waiting again...‘)
        self.req = AddTwoInts.Request()

    def send_request(self):
        self.req.a = int(sys.argv[1])
        self.req.b = int(sys.argv[2])
        self.future = self.cli.call_async(self.req)


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

    minimal_client = MinimalClientAsync()
    minimal_client.send_request()

    while rclpy.ok():
        rclpy.spin_once(minimal_client)
        if minimal_client.future.done():
            try:
                response = minimal_client.future.result()
            except Exception as e:
                minimal_client.get_logger().info(
                    ‘Service call failed %r‘ % (e,))
            else:
                minimal_client.get_logger().info(
                    ‘Result of add_two_ints: for %d + %d = %d‘ %
                    (minimal_client.req.a, minimal_client.req.b, response.sum))
            break

    minimal_client.destroy_node()
    rclpy.shutdown()


if __name__ == ‘__main__‘:
    main()

代码解释:

while not self.cli.wait_for_service(timeout_sec=1.0):
    self.get_logger().info(‘service not available, waiting again...‘)

这两句是客户端每秒都会检查服务是否可用。

    while rclpy.ok():
    	...

这是在检查future是否有结果返回。

然后，同样地，将下面的内容添加到dev_ws/src/py_srvcli/setup.py的console_scripts中:

‘client = py_srvcli.client_member_function:main‘,

服务与客户端代码编写完成，后我们开始构建与运行，首先检查一下依赖:

‘client = py_srvcli.client_member_function:main‘,

到dev_ws目录下，构建package:

colcon build --packages-select py_srvcli

打开新的终端，并进入到dev_ws目录下:

. install/setup.bash
ros2 run py_srvcli service

再打开个终端:

. install/setup.bash
ros2 run py_srvcli client 2 3

在此终端会显示类似如下信息:

[INFO] [1630533285.392494354] [minimal_client_async]: Result of add_two_ints: for 2 + 3 = 5

同时，在上一个终端会显示类似:

[INFO] [1630533285.392494354] [minimal_client_async]: Result of add_two_ints: for 2 + 3 = 5

自定义ROS2 msg和srv文件

进入到目录dev_ws/src中，创建新的package:

ros2 pkg create --build-type ament_cmake tutorial_interfaces

这里没有使用ament_python来创建package，是因为目前版本ament_python无法生成.msg与.srv文件。不过即使在ament_python生成的package仍可使用python来编写代码。

进入到dev_ws/src/tutorial_interfaces来创建目录:

mkdir msg 
mkdir srv

进入到msg目录，创建一个新文件Num.msg:

int64 num

这个即为创建的自定义消息类型，其数据结构是一个64位的整数，名称为num.

类似地，进入到srv目录下，创建自定义服务文件AddThreeInts.srv:

int64 a
int64 b
int64 c
---
int64 sum

此服务需要在请求时提供三个整数，然后返回一个整数sum.

在CMakeLists.txt文件中加入:

find_package(rosidl_default_generators REQUIRED)

rosidl_generate_interfaces(${PROJECT_NAME}
  "msg/Num.msg"
  "srv/AddThreeInts.srv"
 )

在package.xml中加入:

<build_depend>rosidl_default_generators</build_depend>

<exec_depend>rosidl_default_runtime</exec_depend>

<member_of_group>rosidl_interface_packages</member_of_group>

然后进入到dev_ws目录下:

colcon build --packages-select tutorial_interfaces

构建package后，就可又查看刚刚自定义的消息与服务了:

. install/setup.bash
ros2 interface show tutorial_interfaces/msg/Num
ros2 interface show tutorial_interfaces/srv/AddThreeInts

为方便我们直接在上面编写的程序上面修改

import rclpy
from rclpy.node import Node

from tutorial_interfaces.msg import Num    # CHANGE


class MinimalPublisher(Node):

    def __init__(self):
        super().__init__(‘minimal_publisher‘)
        self.publisher_ = self.create_publisher(Num, ‘helloworld‘, 10)     # CHANGE
        timer_period = 0.5
        self.timer = self.create_timer(timer_period, self.timer_callback)
        self.i = 0

    def timer_callback(self):
        msg = Num()                                           # CHANGE
        msg.num = self.i                                      # CHANGE
        self.publisher_.publish(msg)
        self.get_logger().info(‘Publishing: "%d"‘ % msg.num)  # CHANGE
        self.i += 1


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

    minimal_publisher = MinimalPublisher()

    rclpy.spin(minimal_publisher)

    minimal_publisher.destroy_node()
    rclpy.shutdown()


if __name__ == ‘__main__‘:
    main()

import rclpy
from rclpy.node import Node

from tutorial_interfaces.msg import Num        # CHANGE


class MinimalSubscriber(Node):

    def __init__(self):
        super().__init__(‘minimal_subscriber‘)
        self.subscription = self.create_subscription(
            Num,                                              # CHANGE
            ‘helloworld‘,
            self.listener_callback,
            10)
        self.subscription

    def listener_callback(self, msg):
            self.get_logger().info(‘I heard: "%d"‘ % msg.num) # CHANGE


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

    minimal_subscriber = MinimalSubscriber()

    rclpy.spin(minimal_subscriber)

    minimal_subscriber.destroy_node()
    rclpy.shutdown()


if __name__ == ‘__main__‘:
    main()

接着在package.xml中加入:

<exec_depend>tutorial_interfaces</exec_depend>

然后编译:

colcon build --packages-select py_pubsub

我们就可在新窗口执行:

cd ~/dev_ws/
 . install/setup.bash
 ros2 run py_pubsub talker

cd ~/dev_ws/
 . install/setup.bash
 ros2 run py_pubsub listener

会打印类似如下信息:

[INFO] [1630716380.031157485] [minimal_publisher]: Publishing: "0"
[INFO] [1630716380.521359183] [minimal_publisher]: Publishing: "1"
[INFO] [1630716381.021380733] [minimal_publisher]: Publishing: "2"
[INFO] [1630716381.521212491] [minimal_publisher]: Publishing: "3"
[INFO] [1630716382.021220153] [minimal_publisher]: Publishing: "4"
[INFO] [1630716382.519948860] [minimal_publisher]: Publishing: "5"

同样地，我们修改一下服务与客户端请求脚本:

from tutorial_interfaces.srv import AddThreeInts     # CHANGE

import rclpy
from rclpy.node import Node


class MinimalService(Node):

    def __init__(self):
        super().__init__(‘minimal_service‘)
        self.srv = self.create_service(AddThreeInts, ‘add_three_ints‘, self.add_three_ints_callback)        # CHANGE

    def add_three_ints_callback(self, request, response):
        response.sum = request.a + request.b + request.c                                                  # CHANGE
        self.get_logger().info(‘Incoming request\na: %d b: %d c: %d‘ % (request.a, request.b, request.c)) # CHANGE

        return response

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

    minimal_service = MinimalService()

    rclpy.spin(minimal_service)

    rclpy.shutdown()

if __name__ == ‘__main__‘:
    main()

from tutorial_interfaces.srv import AddThreeInts       # CHANGE
import sys
import rclpy
from rclpy.node import Node


class MinimalClientAsync(Node):

    def __init__(self):
        super().__init__(‘minimal_client_async‘)
        self.cli = self.create_client(AddThreeInts, ‘add_three_ints‘)       # CHANGE
        while not self.cli.wait_for_service(timeout_sec=1.0):
            self.get_logger().info(‘service not available, waiting again...‘)
        self.req = AddThreeInts.Request()                                   # CHANGE

    def send_request(self):
        self.req.a = int(sys.argv[1])
        self.req.b = int(sys.argv[2])
        self.req.c = int(sys.argv[3])                  # CHANGE
        self.future = self.cli.call_async(self.req)


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

    minimal_client = MinimalClientAsync()
    minimal_client.send_request()

    while rclpy.ok():
        rclpy.spin_once(minimal_client)
        if minimal_client.future.done():
            try:
                response = minimal_client.future.result()
            except Exception as e:
                minimal_client.get_logger().info(
                    ‘Service call failed %r‘ % (e,))
            else:
                minimal_client.get_logger().info(
                    ‘Result of add_three_ints: for %d + %d + %d = %d‘ %                               # CHANGE
                    (minimal_client.req.a, minimal_client.req.b, minimal_client.req.c, response.sum)) # CHANGE
            break

    minimal_client.destroy_node()
    rclpy.shutdown()


if __name__ == ‘__main__‘:
    main()

在package.xml中加入:

<exec_depend>tutorial_interfaces</exec_depend>

编译:

colcon build --packages-select py_srvcli

cd ~/dev_ws/
 . install/setup.bash
 ros2 run py_srvcli service

cd ~/dev_ws/
 . install/setup.bash
 ros2 run py_srvcli client 2 3 4

会分类显示类似:

# service terminal
[INFO] [1630717079.441505424] [minimal_service]: Incoming request
a: 2 b: 3 c: 4

# client terminal
[INFO] [1630717079.448639918] [minimal_client_async]: Result of add_three_ints: for 2 + 3 + 4 = 9

节点参数

进入到dev_ws/src目录下,创建工作包:

ros2 pkg create --build-type ament_python python_parameters --dependencies rclpy

进入到dev_ws/src/python_parameters/python_parameters中，编写python_parameters_node.py:

import rclpy
import rclpy.node
from rcl_interfaces.msg import ParameterDescriptor
from rclpy.exceptions import ParameterNotDeclaredException
from rcl_interfaces.msg import ParameterType

class MinimalParam(rclpy.node.Node):
    def __init__(self):
        super().__init__(‘minimal_param_node‘)
        timer_period = 2  # seconds
        self.timer = self.create_timer(timer_period, self.timer_callback)
		my_parameter_descriptor = ParameterDescriptor(description=‘This parameter is mine!‘)
        self.declare_parameter(‘my_parameter‘, ‘world‘,my_parameter_descriptor)

    def timer_callback(self):
        my_param = self.get_parameter(‘my_parameter‘).get_parameter_value().string_value

        self.get_logger().info(‘Hello %s!‘ % my_param)

        my_new_param = rclpy.parameter.Parameter(
            ‘my_parameter‘,
            rclpy.Parameter.Type.STRING,
            ‘world‘
        )
        all_new_parameters = [my_new_param]
        self.set_parameters(all_new_parameters)

def main():
    rclpy.init()
    node = MinimalParam()
    rclpy.spin(node)

if __name__ == ‘__main__‘:
    main()

代码解释:

timer_period = 2  # seconds
self.timer = self.create_timer(timer_period, self.timer_callback)

为观察参数变化，我们创建了一个简单地定时函数来打印参数的值。

 my_parameter_descriptor = ParameterDescriptor(description=‘This parameter is mine!‘)
 self.declare_parameter(‘my_parameter‘, ‘world‘,my_parameter_descriptor)

声明了一个名为my_parameter的参数，默认值为world.

def timer_callback(self):
    my_param = self.get_parameter(‘my_parameter‘).get_parameter_value().string_value

    self.get_logger().info(‘Hello %s!‘ % my_param)

    my_new_param = rclpy.parameter.Parameter(
        ‘my_parameter‘,
        rclpy.Parameter.Type.STRING,
        ‘world‘
    )
    all_new_parameters = [my_new_param]
    self.set_parameters(all_new_parameters)

此回调函数先是获取参数my_parameter当前值进行消费，此处只是简单地将此参数值在log中打印出来。然后，又将此参数的值设置回原默认参数，此处为world.

然后，同样地，在setup.py文件中的console_scripts中加入:

 ‘param_talker = python_parameters.python_parameters_node:main‘,

然后检查依赖、构建、进行:

cd ~/dev_ws
rosdep install -i --from-path src --rosdistro foxy -y
colcon build --packages-select python_parameters
. install/setup.bash
ros2 run python_parameters param_talker

然后会看到终端会显示类似如下信息:

[INFO] [1630969878.922440651] [minimal_param_node]: Hello world!
[INFO] [1630969880.924136402] [minimal_param_node]: Hello world!
[INFO] [1630969882.923843790] [minimal_param_node]: Hello world!
...

在新的终端输入:

ros2 param list

会显示:

/minimal_param_node:
  my_parameter
  use_sim_time

也可以在终端重设参数:

ros2 param set /minimal_param_node my_parameter earth

然后会显示:

Set parameter successful

同时在前一个终端中会显示类似:

[INFO] [1630970016.917325178] [minimal_param_node]: Hello earth!

我们也可以在launch文件中设置参数。先在dev_ws/src/python_parameters/中创建目录launch并在其中编写python_parameters_launch.py：

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        Node(
            package=‘python_parameters‘,
            executable=‘param_talker‘,
            name=‘custom_parameter_node‘,
            output=‘screen‘,
            emulate_tty=True,
            parameters=[
                {‘my_parameter‘: ‘earth‘}
            ]
        )
    ])

在setup.py中加入:

import os
from glob import glob
# ...

setup(
  # ...
  data_files=[
      # ...
      (os.path.join(‘share‘, package_name), glob(‘launch/*_launch.py‘)),
    ]
  )

然后:

colcon build --packages-select python_parameters
. install/setup.bash
ros2 launch python_parameters python_parameters_launch.py

会显示类似如何信息:

[INFO] [launch]: All log files can be found below /home/[username]/.ros/log/2021-09-07-07-46-33-861137-[user-pc-name]-14461
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [param_talker-1]: process started with pid [14463]
[param_talker-1] [INFO] [1630971996.143561574] [custom_parameter_node]: Hello earth!
[param_talker-1] [INFO] [1630971998.134791036] [custom_parameter_node]: Hello world!
...

ROS2 第三讲基本操作

码农公寓

ROS2 第三讲 基本操作

创建工作区

订阅与发布topic

编写发布者节点

代码解释

编写订阅者节点

响应与请求服务

自定义ROS2 msg和srv文件

节点参数

代码解释:

相关文章

ROS2 第三讲基本操作