imprement python library for getting parameters of consai2r2_description

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()

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)

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

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():

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]),
+    ])

consai2r2_description/package.xml

@@ -8,6 +8,7 @@
   <license>MIT</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>python_cmake_module</buildtool_depend>
 
   <depend>rclcpp</depend>
   <depend>launch_ros</depend>