ROS2チートシート

現状ではROS2にまともなドキュメントがないので、チートシートを作成しておきます。
開発中に困ったことをその場で書き足していくので内容は薄いです。

rclpy

logger

launchしたらprint周りが表示されなくなってしまうから、ちゃんとloggerで出力しましょう。

rclpy.logging._root_logger.info('message_info')
rclpy.logging._root_logger.warning('message_warn')
rclpy.logging._root_logger.error('message_error')
rclpy.logging._root_logger.fatal('message_fatal')

時刻の計算

地獄

ROS1の.msgファイルでは時刻の型はtimeでしたが、ROS2ではbuiltin_interfaces/Timeとなります。

import rclpy
from rclpy.clock import Clock, ClockType
from rclpy.time import Time

diff_time_stamp = Clock(clock_type=ClockType.ROS_TIME).now() - \
    Time.from_msg(some_msg.stamp)
diff_time_secs = diff_time_stamp.nanoseconds / 1000000000

 

他ノードのparameterを取得

ROS2ではroscoreが廃止され、各ノード毎にパラメータサーバを保有しています。
ですので、他ノードのパラメータを取得するには少々面倒な処理が必要となります。

他ノードとのparameterまわりの通信を行う方法は、ros2cliのros2 paramコマンド周りの実装を参考にすれば分かります。

他ノードからparameterの値を取得する場合、具体的な処理は

• ros2cli/ros2param/ros2param/verb/get.py
• ros2cli/ros2param/ros2param/api/__init__.pyのcall_get_parametersメソッド

に記載されています。

• GetParameters.Request()したことによるresponseはrcl_interfaces.srv.GetParameters_Response
• response.valuesはrcl_interfaces.msg.ParameterValue型のarrayが格納されています。

ParameterValueには型のtype(bool, integer等)及びその型に応じたvalueが用意されています。
(以下、printしたときの例)

rcl_interfaces.msg.ParameterValue(type=4, bool_value=False, integer_value=0,
    double_value=0.0, string_value='blue', byte_array_value=[], bool_array_value=[],
    integer_array_value=[], double_array_value=[], string_array_value=[])

自分で型のtypeを確認し、型に応じてinteger_valueやらdouble_value等から取得しないといけません。めんどくさい。

get_parameters_from_another_nodeメソッドを用意しておけば便利。多分。
(本来の型と違うparameter値でoverwriteされた時のことは考慮していないので注意)

# Copyright 2018 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

from rcl_interfaces.msg import ParameterType
from rcl_interfaces.srv import GetParameters
import rclpy
from rclpy.node import Node

class SomeNode(Node):
    def __init__(self):
        super().__init__('some_node')
        values = self.get_parameters_from_another_node(self, 'node_name', ['param_name'])
        
    def get_parameters_from_another_node(self, node_name, parameter_names):
        # create client
        client = self.create_client(
            GetParameters,
            '{node_name}/get_parameters'.format_map(locals()))

        # call as soon as ready
        ready = client.wait_for_service(timeout_sec=5.0)
        if not ready:
            raise RuntimeError('Wait for service timed out')

        request = GetParameters.Request()
        request.names = parameter_names
        future = client.call_async(request)
        rclpy.spin_until_future_complete(self, future)

        # handle response
        response = future.result()
        if response is None:
            e = future.exception()
            raise RuntimeError(
                'Exception while calling service of node '
                "'{args.node_name}': {e}".format_map(locals()))
        
        return_values = []

        for pvalue in response.values:
            if pvalue.type == ParameterType.PARAMETER_BOOL:
                value = pvalue.bool_value
            elif pvalue.type == ParameterType.PARAMETER_INTEGER:
                value = pvalue.integer_value
            elif pvalue.type == ParameterType.PARAMETER_DOUBLE:
                value = pvalue.double_value
            elif pvalue.type == ParameterType.PARAMETER_STRING:
                value = pvalue.string_value
            elif pvalue.type == ParameterType.PARAMETER_BYTE_ARRAY:
                value = pvalue.byte_array_value
            elif pvalue.type == ParameterType.PARAMETER_BOOL_ARRAY:
                value = pvalue.bool_array_value
            elif pvalue.type == ParameterType.PARAMETER_INTEGER_ARRAY:
                value = pvalue.integer_array_value
            elif pvalue.type == ParameterType.PARAMETER_DOUBLE_ARRAY:
                value = pvalue.double_array_value
            elif pvalue.type == ParameterType.PARAMETER_STRING_ARRAY:
                value = pvalue.string_array_value
            elif pvalue.type == ParameterType.PARAMETER_NOT_SET:
                value = None
            else:
                raise RuntimeError("Unknown parameter type '{pvalue.type}'" \
                    .format_map(locals()))
            return_values.append(value)
        
        return return_values

リストの取得やsetの方法についてはros2cliを参照してください。

cmake周り

同一パッケージ内でC++とPythonスクリプトを共存させる

Pythonスクリプトをモジュールとしてインストールはできますが、ノードとして登録する方法は謎。(検索中)
とりあえずament_python_install_packageしたらモジュールのinstallはされます。

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(python_cmake_module REQUIRED)

set(_PYTHON_EXECUTABLE "${PYTHON_EXECUTABLE}")

if(WIN32 AND CMAKE_BUILD_TYPE STREQUAL "Debug")
  set(PYTHON_EXECUTABLE "${PYTHON_EXECUTABLE_DEBUG}")
endif()

ament_python_install_package(${PROJECT_NAME})

(参考) rclpyのCMakeLists.txt