diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index a36d26ed11..4df91ea732 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -191,6 +191,16 @@ if(BUILD_TESTING) ) target_link_libraries(test_node ${PROJECT_NAME}) endif() + ament_add_gtest(test_node_global_args test/test_node_global_args.cpp) + if(TARGET test_node_global_args) + target_include_directories(test_node_global_args PUBLIC + ${rcl_interfaces_INCLUDE_DIRS} + ${rmw_INCLUDE_DIRS} + ${rosidl_generator_cpp_INCLUDE_DIRS} + ${rosidl_typesupport_cpp_INCLUDE_DIRS} + ) + target_link_libraries(test_node_global_args ${PROJECT_NAME}) + endif() ament_add_gtest(test_parameter_events_filter test/test_parameter_events_filter.cpp) if(TARGET test_parameter_events_filter) target_include_directories(test_parameter_events_filter PUBLIC diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index e90a04e0fd..c44d87948f 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -84,6 +84,9 @@ class Node : public std::enable_shared_from_this * \param[in] node_name Name of the node. * \param[in] namespace_ Namespace of the node. * \param[in] context The context for the node (usually represents the state of a process). + * \param[in] arguments Command line arguments that should apply only to this node. + * This can be used to provide remapping rules that only affect one instance. + * \param[in] use_global_arguments False to prevent node using arguments passed to the process. * \param[in] use_intra_process_comms True to use the optimized intra-process communication * pipeline to pass messages between nodes in the same process using shared memory. */ @@ -92,6 +95,8 @@ class Node : public std::enable_shared_from_this const std::string & node_name, const std::string & namespace_, rclcpp::Context::SharedPtr context, + const std::vector & arguments, + bool use_global_arguments = true, bool use_intra_process_comms = false); RCLCPP_PUBLIC diff --git a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp b/rclcpp/include/rclcpp/node_interfaces/node_base.hpp index b0754cd69a..0e07e36317 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_base.hpp @@ -39,7 +39,9 @@ class NodeBase : public NodeBaseInterface NodeBase( const std::string & node_name, const std::string & namespace_, - rclcpp::Context::SharedPtr context); + rclcpp::Context::SharedPtr context, + const std::vector & arguments, + bool use_global_arguments); RCLCPP_PUBLIC virtual diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index 4d55972749..ae82aa8b30 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -43,6 +43,8 @@ Node::Node( node_name, namespace_, rclcpp::contexts::default_context::get_global_default_context(), + {}, + true, use_intra_process_comms) {} @@ -50,8 +52,11 @@ Node::Node( const std::string & node_name, const std::string & namespace_, rclcpp::Context::SharedPtr context, + const std::vector & arguments, + bool use_global_arguments, bool use_intra_process_comms) -: node_base_(new rclcpp::node_interfaces::NodeBase(node_name, namespace_, context)), +: node_base_(new rclcpp::node_interfaces::NodeBase( + node_name, namespace_, context, arguments, use_global_arguments)), node_graph_(new rclcpp::node_interfaces::NodeGraph(node_base_.get())), node_logging_(new rclcpp::node_interfaces::NodeLogging(node_base_.get())), node_timers_(new rclcpp::node_interfaces::NodeTimers(node_base_.get())), diff --git a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp index 26418c109d..46aaf4be7b 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp @@ -19,6 +19,7 @@ #include "rclcpp/node_interfaces/node_base.hpp" +#include "rcl/arguments.h" #include "rclcpp/exceptions.hpp" #include "rcutils/logging_macros.h" #include "rmw/validate_node_name.h" @@ -31,7 +32,9 @@ using rclcpp::node_interfaces::NodeBase; NodeBase::NodeBase( const std::string & node_name, const std::string & namespace_, - rclcpp::Context::SharedPtr context) + rclcpp::Context::SharedPtr context, + const std::vector & arguments, + bool use_global_arguments) : context_(context), node_handle_(nullptr), default_callback_group_(nullptr), @@ -84,18 +87,37 @@ NodeBase::NodeBase( } // Create the rcl node and store it in a shared_ptr with a custom destructor. - rcl_node_t * rcl_node = new rcl_node_t(rcl_get_zero_initialized_node()); + std::unique_ptr rcl_node(new rcl_node_t(rcl_get_zero_initialized_node())); rcl_node_options_t options = rcl_node_get_default_options(); + std::unique_ptr c_args; + if (!arguments.empty()) { + c_args.reset(new const char *[arguments.size()]); + for (std::size_t i = 0; i < arguments.size(); ++i) { + c_args[i] = arguments[i].c_str(); + } + } + // TODO(sloretz) Pass an allocator to argument parsing + if (arguments.size() > std::numeric_limits::max()) { + throw_from_rcl_error(RCL_RET_INVALID_ARGUMENT, "Too many args"); + } + ret = rcl_parse_arguments( + static_cast(arguments.size()), c_args.get(), rcl_get_default_allocator(), + &(options.arguments)); + if (RCL_RET_OK != ret) { + finalize_notify_guard_condition(); + throw_from_rcl_error(ret, "failed to parse arguments"); + } + + options.use_global_arguments = use_global_arguments; // TODO(wjwwood): pass the Allocator to the options options.domain_id = domain_id; - ret = rcl_node_init(rcl_node, node_name.c_str(), namespace_.c_str(), &options); + + ret = rcl_node_init(rcl_node.get(), node_name.c_str(), namespace_.c_str(), &options); if (ret != RCL_RET_OK) { // Finalize the interrupt guard condition. finalize_notify_guard_condition(); - delete rcl_node; - if (ret == RCL_RET_NODE_INVALID_NAME) { rcl_reset_error(); // discard rcl_node_init error int validation_result; @@ -145,7 +167,7 @@ NodeBase::NodeBase( } node_handle_.reset( - rcl_node, + rcl_node.release(), [](rcl_node_t * node) -> void { if (rcl_node_fini(node) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( diff --git a/rclcpp/test/test_node.cpp b/rclcpp/test/test_node.cpp index 656c35b331..7c39cb5d41 100644 --- a/rclcpp/test/test_node.cpp +++ b/rclcpp/test/test_node.cpp @@ -16,6 +16,7 @@ #include #include +#include #include "rclcpp/exceptions.hpp" #include "rclcpp/node.hpp" diff --git a/rclcpp/test/test_node_global_args.cpp b/rclcpp/test/test_node_global_args.cpp new file mode 100644 index 0000000000..cff72d4245 --- /dev/null +++ b/rclcpp/test/test_node_global_args.cpp @@ -0,0 +1,62 @@ +// 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 + +#include +#include +#include + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/rclcpp.hpp" + +class TestNodeWithGlobalArgs : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + const char * const args[] = {"proc", "__node:=global_node_name"}; + rclcpp::init(2, args); + } +}; + +TEST_F(TestNodeWithGlobalArgs, local_arguments_before_global) { + auto context = rclcpp::contexts::default_context::get_global_default_context(); + const std::vector arguments = {"__node:=local_arguments_test"}; + const bool use_global_arguments = true; + const bool use_intra_process = false; + auto node = rclcpp::Node::make_shared( + "orig_name", "", context, arguments, use_global_arguments, use_intra_process); + EXPECT_STREQ("local_arguments_test", node->get_name()); +} + +TEST_F(TestNodeWithGlobalArgs, use_or_ignore_global_arguments) { + auto context = rclcpp::contexts::default_context::get_global_default_context(); + const std::vector arguments = {}; + const bool use_intra_process = false; + + { // Don't use global args + const bool use_global_arguments = false; + auto node = rclcpp::Node::make_shared( + "orig_name", "", context, arguments, use_global_arguments, use_intra_process); + EXPECT_STREQ("orig_name", node->get_name()); + } + { // Do use global args + const bool use_global_arguments = true; + auto node = rclcpp::Node::make_shared( + "orig_name", "", context, arguments, use_global_arguments, use_intra_process); + EXPECT_STREQ("global_node_name", node->get_name()); + } +} diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 12137e5d7b..a9343ba540 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -88,6 +88,9 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, * \param[in] node_name Name of the node. * \param[in] node_name Namespace of the node. * \param[in] context The context for the node (usually represents the state of a process). + * \param[in] arguments Command line arguments that should apply only to this node. + * This can be used to provide remapping rules that only affect one instance. + * \param[in] use_global_arguments False to prevent node using arguments passed to the process. * \param[in] use_intra_process_comms True to use the optimized intra-process communication * pipeline to pass messages between nodes in the same process using shared memory. */ @@ -96,6 +99,8 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, const std::string & node_name, const std::string & namespace_, rclcpp::Context::SharedPtr context, + const std::vector & arguments, + bool use_global_arguments = true, bool use_intra_process_comms = false); RCLCPP_LIFECYCLE_PUBLIC diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index c03175ff93..06c82285d1 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -49,6 +49,8 @@ LifecycleNode::LifecycleNode( node_name, namespace_, rclcpp::contexts::default_context::get_global_default_context(), + {}, + true, use_intra_process_comms) {} @@ -56,8 +58,11 @@ LifecycleNode::LifecycleNode( const std::string & node_name, const std::string & namespace_, rclcpp::Context::SharedPtr context, + const std::vector & arguments, + bool use_global_arguments, bool use_intra_process_comms) -: node_base_(new rclcpp::node_interfaces::NodeBase(node_name, namespace_, context)), +: node_base_(new rclcpp::node_interfaces::NodeBase( + node_name, namespace_, context, arguments, use_global_arguments)), node_graph_(new rclcpp::node_interfaces::NodeGraph(node_base_.get())), node_logging_(new rclcpp::node_interfaces::NodeLogging(node_base_.get())), node_timers_(new rclcpp::node_interfaces::NodeTimers(node_base_.get())),