Convert rcl_params_t to ParameterMap

rclcpp/CMakeLists.txt

@@ -6,6 +6,7 @@ find_package(ament_cmake_ros REQUIRED)
 find_package(builtin_interfaces REQUIRED)
 find_package(rcl REQUIRED)
 find_package(rcl_interfaces REQUIRED)
+find_package(rcl_yaml_param_parser REQUIRED)
 find_package(rmw REQUIRED)
 find_package(rmw_implementation REQUIRED)
 find_package(rosgraph_msgs REQUIRED)
@@ -62,6 +63,7 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/parameter_value.cpp
   src/rclcpp/parameter_client.cpp
   src/rclcpp/parameter_events_filter.cpp
+  src/rclcpp/parameter_map.cpp
   src/rclcpp/parameter_service.cpp
   src/rclcpp/publisher.cpp
   src/rclcpp/service.cpp
@@ -100,6 +102,7 @@ add_library(${PROJECT_NAME}
 # specific order: dependents before dependencies
 ament_target_dependencies(${PROJECT_NAME}
   "rcl"
+  "rcl_yaml_param_parser"
   "builtin_interfaces"
   "rosgraph_msgs"
   "rosidl_typesupport_cpp"
@@ -128,6 +131,7 @@ ament_export_dependencies(rosgraph_msgs)
 ament_export_dependencies(rosidl_typesupport_cpp)
 ament_export_dependencies(rosidl_typesupport_c)
 ament_export_dependencies(rosidl_generator_cpp)
+ament_export_dependencies(rcl_yaml_param_parser)
 
 if(BUILD_TESTING)
   find_package(ament_cmake_gtest REQUIRED)
@@ -225,6 +229,10 @@ if(BUILD_TESTING)
     )
     target_link_libraries(test_parameter ${PROJECT_NAME})
   endif()
+  ament_add_gtest(test_parameter_map test/test_parameter_map.cpp)
+  if(TARGET test_parameter_map)
+    target_link_libraries(test_parameter_map ${PROJECT_NAME})
+  endif()
   ament_add_gtest(test_publisher test/test_publisher.cpp)
   if(TARGET test_publisher)
     target_include_directories(test_publisher PUBLIC

rclcpp/include/rclcpp/exceptions.hpp

@@ -181,6 +181,21 @@ class EventNotRegisteredError : public std::runtime_error
   : std::runtime_error("event already registered") {}
 };
 
+/// Thrown if passed parameters are inconsistent or invalid
+class InvalidParametersException : public std::runtime_error
+{
+public:
+  // Inherit constructors from runtime_error;
+  using std::runtime_error::runtime_error;
+};
+
+/// Throwing if passed parameter value is invalid.
+class InvalidParameterValueException : public std::runtime_error
+{
+  // Inherit constructors from runtime_error;
+  using std::runtime_error::runtime_error;
+};
+
 }  // namespace exceptions
 }  // namespace rclcpp
 

rclcpp/include/rclcpp/parameter_map.hpp

@@ -0,0 +1,53 @@
+// 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.
+
+#ifndef RCLCPP__PARAMETER_MAP_HPP_
+#define RCLCPP__PARAMETER_MAP_HPP_
+
+#include <rcl_yaml_param_parser/types.h>
+
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/parameter.hpp"
+#include "rclcpp/parameter_value.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+
+/// A map of fully qualified node names to a list of parameters
+using ParameterMap = std::unordered_map<std::string, std::vector<Parameter>>;
+
+/// Convert parameters from rcl_yaml_param_parser into C++ class instances.
+/// \param[in] c_params C structures containing parameters for multiple nodes.
+/// \returns a map where the keys are fully qualified node names and values a list of parameters.
+/// \throws InvalidParametersException if the `rcl_params_t` is inconsistent or invalid.
+RCLCPP_PUBLIC
+ParameterMap
+parameter_map_from(const rcl_params_t * const c_params);
+
+/// Convert parameter value from rcl_yaml_param_parser into a C++ class instance.
+/// \param[in] c_value C structure containing a value of a parameter.
+/// \returns an instance of a parameter value
+/// \throws InvalidParameterValueException if the `rcl_variant_t` is inconsistent or invalid.
+RCLCPP_PUBLIC
+ParameterValue
+parameter_value_from(const rcl_variant_t * const c_value);
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__PARAMETER_MAP_HPP_

rclcpp/package.xml

@@ -25,6 +25,7 @@
   <build_export_depend>rosidl_typesupport_cpp</build_export_depend>
 
   <depend>rcl</depend>
+  <depend>rcl_yaml_param_parser</depend>
   <depend>rmw_implementation</depend>
 
   <exec_depend>ament_cmake</exec_depend>

rclcpp/src/rclcpp/parameter_map.cpp

@@ -0,0 +1,128 @@
+// 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.
+
+#include <string>
+#include <vector>
+
+#include "rclcpp/parameter_map.hpp"
+
+using rclcpp::exceptions::InvalidParametersException;
+using rclcpp::exceptions::InvalidParameterValueException;
+using rclcpp::ParameterMap;
+using rclcpp::ParameterValue;
+
+ParameterMap
+rclcpp::parameter_map_from(const rcl_params_t * const c_params)
+{
+  if (NULL == c_params) {
+    throw InvalidParametersException("parameters struct is NULL");
+  } else if (NULL == c_params->node_names) {
+    throw InvalidParametersException("node names array is NULL");
+  } else if (NULL == c_params->params) {
+    throw InvalidParametersException("node params array is NULL");
+  }
+
+  // Convert c structs into a list of parameters to set
+  ParameterMap parameters;
+  for (size_t n = 0; n < c_params->num_nodes; ++n) {
+    const char * c_node_name = c_params->node_names[n];
+    if (NULL == c_node_name) {
+      throw InvalidParametersException("Node name at index " + std::to_string(n) + " is NULL");
+    }
+
+    /// make sure there is a leading slash on the fully qualified node name
+    std::string node_name("/");
+    if ('/' != c_node_name[0]) {
+      node_name += c_node_name;
+    } else {
+      node_name = c_node_name;
+    }
+
+    const rcl_node_params_t * const c_params_node = &(c_params->params[n]);
+
+    std::vector<Parameter> & params_node = parameters[node_name];
+    params_node.reserve(c_params_node->num_params);
+
+    for (size_t p = 0; p < c_params_node->num_params; ++p) {
+      const char * const c_param_name = c_params_node->parameter_names[p];
+      if (NULL == c_param_name) {
+        std::string message(
+          "At node " + std::to_string(n) + " parameter " + std::to_string(p) + " name is NULL");
+        throw InvalidParametersException(message);
+      }
+      const rcl_variant_t * const c_param_value = &(c_params_node->parameter_values[p]);
+      params_node.emplace_back(c_param_name, parameter_value_from(c_param_value));
+    }
+  }
+  return parameters;
+}
+
+ParameterValue
+rclcpp::parameter_value_from(const rcl_variant_t * const c_param_value)
+{
+  if (NULL == c_param_value) {
+    throw InvalidParameterValueException("Passed argument is NULL");
+  }
+  if (c_param_value->bool_value) {
+    return ParameterValue(*(c_param_value->bool_value));
+  } else if (c_param_value->integer_value) {
+    return ParameterValue(*(c_param_value->integer_value));
+  } else if (c_param_value->double_value) {
+    return ParameterValue(*(c_param_value->double_value));
+  } else if (c_param_value->string_value) {
+    return ParameterValue(std::string(c_param_value->string_value));
+  } else if (c_param_value->byte_array_value) {
+    const rcl_byte_array_t * const byte_array = c_param_value->byte_array_value;
+    std::vector<uint8_t> bytes;
+    bytes.reserve(byte_array->size);
+    for (size_t v = 0; v < byte_array->size; ++v) {
+      bytes.push_back(byte_array->values[v]);
+    }
+    return ParameterValue(bytes);
+  } else if (c_param_value->bool_array_value) {
+    const rcl_bool_array_t * const bool_array = c_param_value->bool_array_value;
+    std::vector<bool> bools;
+    bools.reserve(bool_array->size);
+    for (size_t v = 0; v < bool_array->size; ++v) {
+      bools.push_back(bool_array->values[v]);
+    }
+    return ParameterValue(bools);
+  } else if (c_param_value->integer_array_value) {
+    const rcl_int64_array_t * const int_array = c_param_value->integer_array_value;
+    std::vector<int64_t> integers;
+    integers.reserve(int_array->size);
+    for (size_t v = 0; v < int_array->size; ++v) {
+      integers.push_back(int_array->values[v]);
+    }
+    return ParameterValue(integers);
+  } else if (c_param_value->double_array_value) {
+    const rcl_double_array_t * const double_array = c_param_value->double_array_value;
+    std::vector<double> doubles;
+    doubles.reserve(double_array->size);
+    for (size_t v = 0; v < double_array->size; ++v) {
+      doubles.push_back(double_array->values[v]);
+    }
+    return ParameterValue(doubles);
+  } else if (c_param_value->string_array_value) {
+    const rcutils_string_array_t * const string_array = c_param_value->string_array_value;
+    std::vector<std::string> strings;
+    strings.reserve(string_array->size);
+    for (size_t v = 0; v < string_array->size; ++v) {
+      strings.emplace_back(string_array->data[v]);
+    }
+    return ParameterValue(strings);
+  }
+
+  throw InvalidParameterValueException("No parameter value set");
+}