From 29c3d84f471f3476287fbd1e362fc7939fa2a8f3 Mon Sep 17 00:00:00 2001 From: Kentaro Tanaka Date: Wed, 25 Dec 2019 22:11:55 +0900 Subject: [PATCH] imprement python library for getting parameters of consai2r2_description --- consai2r2_description/CMakeLists.txt | 13 ++- .../consai2r2_description/__init__.py | 30 ++++++ .../consai2r2_description/parameter.py | 96 +++++++++++++++++++ consai2r2_description/launch/config.launch.py | 2 +- .../launch/description.launch.py | 48 ++++++++++ consai2r2_description/package.xml | 1 + 6 files changed, 187 insertions(+), 3 deletions(-) create mode 100755 consai2r2_description/consai2r2_description/__init__.py create mode 100755 consai2r2_description/consai2r2_description/parameter.py create mode 100644 consai2r2_description/launch/description.launch.py diff --git a/consai2r2_description/CMakeLists.txt b/consai2r2_description/CMakeLists.txt index d082746..36a78e1 100644 --- a/consai2r2_description/CMakeLists.txt +++ b/consai2r2_description/CMakeLists.txt @@ -13,6 +13,15 @@ endif() # find dependencies 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}) include_directories(include ${CMAKE_CURRENT_BINARY_DIR}) add_executable(${PROJECT_NAME}_node @@ -39,8 +48,6 @@ install(DIRECTORY DESTINATION share/${PROJECT_NAME}/ ) - - if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights @@ -52,4 +59,6 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() +set(PYTHON_EXECUTABLE "${_PYTHON_EXECUTABLE}") + ament_package() diff --git a/consai2r2_description/consai2r2_description/__init__.py b/consai2r2_description/consai2r2_description/__init__.py new file mode 100755 index 0000000..5fdfdf3 --- /dev/null +++ b/consai2r2_description/consai2r2_description/__init__.py @@ -0,0 +1,30 @@ +# Copyright (c) 2019 SSL-Roots +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. + +import consai2r2_description.parameter + + +class consai2r2_parameters(): + + def __init__(self, node): + param_names = consai2r2_description.parameter.list_parameters(node) + params = consai2r2_description.parameter.get_parameters(node, param_names) + for param_name, param_value in params.items(): + setattr(self, param_name, param_value) diff --git a/consai2r2_description/consai2r2_description/parameter.py b/consai2r2_description/consai2r2_description/parameter.py new file mode 100755 index 0000000..fef2bfc --- /dev/null +++ b/consai2r2_description/consai2r2_description/parameter.py @@ -0,0 +1,96 @@ +# 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 +from rcl_interfaces.srv import ListParameters + +import rclpy + + +# https://github.com/ros2/ros2cli/blob/master/ros2param/ros2param/api/__init__.py#L174 +def list_parameters(node): + # create client + client = node.create_client( + ListParameters, + 'consai2r2_description/list_parameters') + + # 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 = ListParameters.Request() + future = client.call_async(request) + rclpy.spin_until_future_complete(node, future) + + # handle response + response = future.result() + if response is None: + raise RuntimeError("Failed to get the list of parameters'") + + return response.result.names + + +# https://github.com/ros2/ros2cli/blob/master/ros2param/ros2param/api/__init__.py#L54 +def get_parameters(node, parameter_names): + # create client + client = node.create_client( + GetParameters, + 'consai2r2_description/get_parameters') + + # 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(node, future) + + # handle response + response = future.result() + if response is None: + e = future.exception() + raise RuntimeError("Failed to get parameters form node 'consai2r2_description'") + + return_values = {} + + for i, pvalue in enumerate(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[parameter_names[i]] = value + + return return_values diff --git a/consai2r2_description/launch/config.launch.py b/consai2r2_description/launch/config.launch.py index ed9dc17..f44c60e 100644 --- a/consai2r2_description/launch/config.launch.py +++ b/consai2r2_description/launch/config.launch.py @@ -20,9 +20,9 @@ import os +from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch_ros.actions import Node -from ament_index_python.packages import get_package_share_directory def generate_launch_description(): diff --git a/consai2r2_description/launch/description.launch.py b/consai2r2_description/launch/description.launch.py new file mode 100644 index 0000000..0e35c31 --- /dev/null +++ b/consai2r2_description/launch/description.launch.py @@ -0,0 +1,48 @@ +# Copyright (c) 2019 SSL-Roots +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + config_path = os.path.join( + get_package_share_directory('consai2r2_description'), 'config', 'config.yaml') + + return LaunchDescription([ + Node( + package='consai2r2_description', node_executable='consai2r2_description_node', + output='screen', parameters=[config_path]), + Node( + package='consai2r2_receiver', node_executable='referee_receiver', + output='screen', parameters=[config_path]), + Node( + package='consai2r2_receiver', node_executable='vision_receiver', + output='screen', parameters=[config_path]), + Node( + package='consai2r2_vision_wrapper', node_executable='vision_wrapper', + output='screen', parameters=[config_path]), + Node( + package='consai2r2_referee_wrapper', node_executable='referee_wrapper', + output='screen', parameters=[config_path]), + ]) diff --git a/consai2r2_description/package.xml b/consai2r2_description/package.xml index 8227096..dd71773 100644 --- a/consai2r2_description/package.xml +++ b/consai2r2_description/package.xml @@ -8,6 +8,7 @@ MIT ament_cmake + python_cmake_module rclcpp launch_ros