Skip to content

Commit 9f516bc

Browse files
sloretzsloretz
committed
Set parameters at node construction
1 parent f9a78df commit 9f516bc

File tree

5 files changed

+121
-0
lines changed

5 files changed

+121
-0
lines changed

‎rclcpp/CMakeLists.txt

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,7 @@ find_package(ament_cmake_ros REQUIRED)
66
find_package(builtin_interfaces REQUIRED)
77
find_package(rcl REQUIRED)
88
find_package(rcl_interfaces REQUIRED)
9+
find_package(rcl_yaml_param_parser REQUIRED)
910
find_package(rmw REQUIRED)
1011
find_package(rmw_implementation REQUIRED)
1112
find_package(rosidl_generator_cpp REQUIRED)
@@ -98,6 +99,7 @@ add_library(${PROJECT_NAME}
9899
# specific order: dependents before dependencies
99100
ament_target_dependencies(${PROJECT_NAME}
100101
"rcl"
102+
"rcl_yaml_param_parser"
101103
"builtin_interfaces"
102104
"rosidl_typesupport_cpp"
103105
"rosidl_generator_cpp")
@@ -120,6 +122,8 @@ ament_export_libraries(${PROJECT_NAME})
120122

121123
ament_export_dependencies(ament_cmake)
122124
ament_export_dependencies(rcl)
125+
# TODO(sloretz) Delete this if it doesn't end up being used in an installed header
126+
ament_export_dependencies(rcl_yaml_param_parser)
123127
ament_export_dependencies(builtin_interfaces)
124128
ament_export_dependencies(rosidl_typesupport_cpp)
125129
ament_export_dependencies(rosidl_typesupport_c)

‎rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -46,6 +46,7 @@ class NodeParameters : public NodeParametersInterface
4646

4747
RCLCPP_PUBLIC
4848
NodeParameters(
49+
rclcpp::node_interfaces::NodeBaseInterface * node_base,
4950
rclcpp::node_interfaces::NodeTopicsInterface * node_topics,
5051
bool use_intra_process);
5152

‎rclcpp/package.xml

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,7 @@
2323
<build_export_depend>rosidl_typesupport_cpp</build_export_depend>
2424

2525
<depend>rcl</depend>
26+
<depend>rcl_yaml_param_parser</depend>
2627
<depend>rmw_implementation</depend>
2728

2829
<exec_depend>ament_cmake</exec_depend>

‎rclcpp/src/rclcpp/node.cpp

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -63,6 +63,7 @@ Node::Node(
6363
node_topics_(new rclcpp::node_interfaces::NodeTopics(node_base_.get())),
6464
node_services_(new rclcpp::node_interfaces::NodeServices(node_base_.get())),
6565
node_parameters_(new rclcpp::node_interfaces::NodeParameters(
66+
node_base_.get(),
6667
node_topics_.get(),
6768
use_intra_process_comms
6869
)),

‎rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp

Lines changed: 114 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -21,13 +21,15 @@
2121
#include <vector>
2222

2323
#include "rcl_interfaces/srv/list_parameters.hpp"
24+
#include "rcl_yaml_param_parser/parser.h"
2425
#include "rclcpp/create_publisher.hpp"
2526
#include "rcutils/logging_macros.h"
2627
#include "rmw/qos_profiles.h"
2728

2829
using rclcpp::node_interfaces::NodeParameters;
2930

3031
NodeParameters::NodeParameters(
32+
rclcpp::node_interfaces::NodeBaseInterface * node_base,
3133
rclcpp::node_interfaces::NodeTopicsInterface * node_topics,
3234
bool use_intra_process)
3335
: node_topics_(node_topics)
@@ -44,6 +46,118 @@ NodeParameters::NodeParameters(
4446
rmw_qos_profile_parameter_events,
4547
use_intra_process,
4648
allocator);
49+
50+
// Get the node's allocator
51+
const rcl_node_t * node = node_base->get_rcl_node_handle();
52+
if (NULL == node) {
53+
throw std::runtime_error("Need valid node handle in NodeParameters");
54+
}
55+
const rcl_node_options_t * options = rcl_node_get_options(node);
56+
if (NULL == options) {
57+
throw std::runtime_error("Need valid node options NodeParameters");
58+
}
59+
60+
// TODO(sloretz) remove when circular dependency between rcl and rcl_yaml_param_parser is solved
61+
// TODO(sloretz) Somehow need to get yaml parameters from RCL
62+
const std::string yaml_path("/tmp/sloretz-test-params.yaml");
63+
rcl_params_t * yaml_params = rcl_yaml_node_struct_init(options->allocator);
64+
if (NULL == yaml_params) {
65+
throw std::runtime_error("Failed to initialize yaml params struct");
66+
}
67+
if (!rcl_parse_yaml_file(yaml_path.c_str(), yaml_params, options->allocator)) {
68+
throw std::runtime_error("Failed to parse parameters " + yaml_path);
69+
}
70+
71+
// Get namespace + node name
72+
const std::string node_name = node_base->get_name();
73+
const std::string node_namespace = node_base->get_namespace();
74+
if (0u == node_namespace.size() || 0u == node_name.size()) {
75+
// Should never happen
76+
throw std::runtime_error("Node name and namespace were not set");
77+
}
78+
std::string combined_name;
79+
if ('/' == node_namespace.at(node_namespace.size()-1)) {
80+
combined_name = node_namespace + node_name;
81+
} else {
82+
combined_name = node_namespace + '/' + node_name;
83+
}
84+
85+
// TODO(sloretz) should yaml param parser provide node names with a leading slash?
86+
// Strip leading slash
87+
combined_name.erase(combined_name.begin());
88+
89+
// Convert c structs into a list of parameters to set
90+
std::vector<rclcpp::parameter::ParameterVariant> parameters;
91+
for (size_t n = 0; n < yaml_params->num_nodes; ++n) {
92+
if (combined_name != yaml_params->node_names[n]) {
93+
continue;
94+
}
95+
const rcl_node_params_t * const params_node = &(yaml_params->params[n]);
96+
for (size_t p = 0; p < params_node->num_params; ++p) {
97+
const std::string param_name = params_node->parameter_names[p];
98+
const rcl_variant_t * const param_value = &(params_node->parameter_values[p]);
99+
100+
if (param_value->bool_value) {
101+
parameters.emplace_back(param_name, *(param_value->bool_value));
102+
} else if (param_value->integer_value) {
103+
parameters.emplace_back(param_name, *(param_value->integer_value));
104+
} else if (param_value->double_value) {
105+
parameters.emplace_back(param_name, *(param_value->double_value));
106+
} else if (param_value->string_value) {
107+
parameters.emplace_back(param_name, std::string(param_value->string_value));
108+
} else if (param_value->byte_array_value) {
109+
const rcl_byte_array_t * const byte_array = param_value->byte_array_value;
110+
std::vector<uint8_t> bytes;
111+
bytes.reserve(byte_array->size);
112+
for (size_t v = 0; v < byte_array->size; ++v) {
113+
bytes.push_back(byte_array->values[v]);
114+
}
115+
parameters.emplace_back(param_name, bytes);
116+
} else if (param_value->bool_array_value) {
117+
const rcl_bool_array_t * const bool_array = param_value->bool_array_value;
118+
std::vector<bool> bools;
119+
bools.reserve(bool_array->size);
120+
for (size_t v = 0; v < bool_array->size; ++v) {
121+
bools.push_back(bool_array->values[v]);
122+
}
123+
parameters.emplace_back(param_name, bools);
124+
} else if (param_value->integer_array_value) {
125+
const rcl_int64_array_t * const int_array = param_value->integer_array_value;
126+
std::vector<int64_t> integers;
127+
integers.reserve(int_array->size);
128+
for (size_t v = 0; v < int_array->size; ++v) {
129+
integers.push_back(int_array->values[v]);
130+
}
131+
parameters.emplace_back(param_name, integers);
132+
} else if (param_value->double_array_value) {
133+
const rcl_double_array_t * const double_array = param_value->double_array_value;
134+
std::vector<double> doubles;
135+
doubles.reserve(double_array->size);
136+
for (size_t v = 0; v < double_array->size; ++v) {
137+
doubles.push_back(double_array->values[v]);
138+
}
139+
parameters.emplace_back(param_name, doubles);
140+
} else if (param_value->string_array_value) {
141+
const rcutils_string_array_t * const string_array = param_value->string_array_value;
142+
std::vector<std::string> strings;
143+
strings.reserve(string_array->size);
144+
for (size_t v = 0; v < string_array->size; ++v) {
145+
strings.emplace_back(string_array->data[v]);
146+
}
147+
parameters.emplace_back(param_name, strings);
148+
} else {
149+
rcl_yaml_node_struct_fini(yaml_params, options->allocator);
150+
throw std::runtime_error("Invalid parameter from parser");
151+
}
152+
}
153+
}
154+
rcl_yaml_node_struct_fini(yaml_params, options->allocator);
155+
156+
// Set the parameters
157+
rcl_interfaces::msg::SetParametersResult result = set_parameters_atomically(parameters);
158+
if (!result.successful) {
159+
throw std::runtime_error("Failed to set initial parameters");
160+
}
47161
}
48162

49163
NodeParameters::~NodeParameters()

0 commit comments

Comments
 (0)