Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add hook to generate node options in ComponentManager #1702

Merged
merged 6 commits into from
Jul 7, 2021
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -131,6 +131,15 @@ class ComponentManager : public rclcpp::Node
create_component_factory(const ComponentResource & resource);

protected:
/// Create node options for loaded component
/*
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Also, this should be /**, but this whole file is wrong...

* \param request information with the node to load
* \return node options
*/
RCLCPP_COMPONENTS_PUBLIC
virtual rclcpp::NodeOptions
CreateNodeOptions(const std::shared_ptr<LoadNode::Request> request);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I missed this when skimming before, but can we please change this to be create_node_options? We don't use CamelCase for methods in core ROS 2 packages like this one.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Well, I just saw that more of these methods (protected ones only) are CamelCase... so I guess this can be left as-is for now... we really need to change them though.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I've opened #1716 to address these style issues.


/// Service callback to load a new node in the component
/*
* This function allows to add parameters, remap rules, a specific node, name a namespace
Expand Down
85 changes: 46 additions & 39 deletions rclcpp_components/src/component_manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -121,6 +121,51 @@ ComponentManager::create_component_factory(const ComponentResource & resource)
return {};
}

rclcpp::NodeOptions
ComponentManager::CreateNodeOptions(const std::shared_ptr<LoadNode::Request> request)
{
std::vector<rclcpp::Parameter> parameters;
for (const auto & p : request->parameters) {
parameters.push_back(rclcpp::Parameter::from_parameter_msg(p));
}

std::vector<std::string> remap_rules;
remap_rules.reserve(request->remap_rules.size() * 2 + 1);
remap_rules.push_back("--ros-args");
for (const std::string & rule : request->remap_rules) {
remap_rules.push_back("-r");
remap_rules.push_back(rule);
}

if (!request->node_name.empty()) {
remap_rules.push_back("-r");
remap_rules.push_back("__node:=" + request->node_name);
}

if (!request->node_namespace.empty()) {
remap_rules.push_back("-r");
remap_rules.push_back("__ns:=" + request->node_namespace);
}

auto options = rclcpp::NodeOptions()
.use_global_arguments(false)
.parameter_overrides(parameters)
.arguments(remap_rules);

for (const auto & a : request->extra_arguments) {
const rclcpp::Parameter extra_argument = rclcpp::Parameter::from_parameter_msg(a);
if (extra_argument.get_name() == "use_intra_process_comms") {
if (extra_argument.get_type() != rclcpp::ParameterType::PARAMETER_BOOL) {
throw ComponentManagerException(
"Extra component argument 'use_intra_process_comms' must be a boolean");
}
options.use_intra_process_comms(extra_argument.get_value<bool>());
}
}

return options;
}

void
ComponentManager::OnLoadNode(
const std::shared_ptr<rmw_request_id_t> request_header,
Expand All @@ -142,45 +187,7 @@ ComponentManager::OnLoadNode(
continue;
}

std::vector<rclcpp::Parameter> parameters;
for (const auto & p : request->parameters) {
parameters.push_back(rclcpp::Parameter::from_parameter_msg(p));
}

std::vector<std::string> remap_rules;
remap_rules.reserve(request->remap_rules.size() * 2 + 1);
remap_rules.push_back("--ros-args");
for (const std::string & rule : request->remap_rules) {
remap_rules.push_back("-r");
remap_rules.push_back(rule);
}

if (!request->node_name.empty()) {
remap_rules.push_back("-r");
remap_rules.push_back("__node:=" + request->node_name);
}

if (!request->node_namespace.empty()) {
remap_rules.push_back("-r");
remap_rules.push_back("__ns:=" + request->node_namespace);
}

auto options = rclcpp::NodeOptions()
.use_global_arguments(false)
.parameter_overrides(parameters)
.arguments(remap_rules);

for (const auto & a : request->extra_arguments) {
const rclcpp::Parameter extra_argument = rclcpp::Parameter::from_parameter_msg(a);
if (extra_argument.get_name() == "use_intra_process_comms") {
if (extra_argument.get_type() != rclcpp::ParameterType::PARAMETER_BOOL) {
throw ComponentManagerException(
"Extra component argument 'use_intra_process_comms' must be a boolean");
}
options.use_intra_process_comms(extra_argument.get_value<bool>());
}
}

auto options = CreateNodeOptions(request);
auto node_id = unique_id_++;

if (0 == node_id) {
Expand Down