Force explicit --ros-args in NodeOptions::arguments(). (#845)
Signed-off-by: Michel Hidalgo <michel@ekumenlabs.com>
This commit is contained in:
parent
a6e80fcaea
commit
1fff8cbac1
6 changed files with 12 additions and 48 deletions
|
@ -99,8 +99,7 @@ public:
|
||||||
/// Set the arguments, return this for parameter idiom.
|
/// Set the arguments, return this for parameter idiom.
|
||||||
/**
|
/**
|
||||||
* These arguments are used to extract remappings used by the node and other
|
* These arguments are used to extract remappings used by the node and other
|
||||||
* settings. Being specific to a ROS node, an implicit `--ros-args` scope flag
|
* ROS specific settings, as well as user defined non-ROS arguments.
|
||||||
* always precedes these arguments.
|
|
||||||
*
|
*
|
||||||
* This will cause the internal rcl_node_options_t struct to be invalidated.
|
* This will cause the internal rcl_node_options_t struct to be invalidated.
|
||||||
*/
|
*/
|
||||||
|
|
|
@ -93,29 +93,18 @@ NodeOptions::get_rcl_node_options() const
|
||||||
int c_argc = 0;
|
int c_argc = 0;
|
||||||
std::unique_ptr<const char *[]> c_argv;
|
std::unique_ptr<const char *[]> c_argv;
|
||||||
if (!this->arguments_.empty()) {
|
if (!this->arguments_.empty()) {
|
||||||
auto it = std::find(
|
|
||||||
this->arguments_.cbegin(), this->arguments_.cend(), RCL_ROS_ARGS_FLAG);
|
|
||||||
|
|
||||||
c_argc = static_cast<int>(this->arguments_.size());
|
|
||||||
if (it == this->arguments_.cend()) {
|
|
||||||
c_argc += 1;
|
|
||||||
}
|
|
||||||
|
|
||||||
c_argv.reset(new const char *[c_argc]);
|
|
||||||
|
|
||||||
std::size_t i = 0;
|
|
||||||
if (it == this->arguments_.cend()) {
|
|
||||||
c_argv[i++] = RCL_ROS_ARGS_FLAG;
|
|
||||||
}
|
|
||||||
for (std::size_t j = 0; j < this->arguments_.size(); ++i, ++j) {
|
|
||||||
c_argv[i] = this->arguments_[j].c_str();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
if (this->arguments_.size() > static_cast<size_t>(std::numeric_limits<int>::max())) {
|
if (this->arguments_.size() > static_cast<size_t>(std::numeric_limits<int>::max())) {
|
||||||
throw_from_rcl_error(RCL_RET_INVALID_ARGUMENT, "Too many args");
|
throw_from_rcl_error(RCL_RET_INVALID_ARGUMENT, "Too many args");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
c_argc = static_cast<int>(this->arguments_.size());
|
||||||
|
c_argv.reset(new const char *[c_argc]);
|
||||||
|
|
||||||
|
for (std::size_t i = 0; i < this->arguments_.size(); ++i) {
|
||||||
|
c_argv[i] = this->arguments_[i].c_str();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
rmw_ret_t ret = rcl_parse_arguments(
|
rmw_ret_t ret = rcl_parse_arguments(
|
||||||
c_argc, c_argv.get(), this->allocator_, &(node_options_->arguments));
|
c_argc, c_argv.get(), this->allocator_, &(node_options_->arguments));
|
||||||
|
|
||||||
|
|
|
@ -66,7 +66,7 @@ TEST_F(TestNode, get_name_and_namespace) {
|
||||||
}
|
}
|
||||||
{
|
{
|
||||||
auto options = rclcpp::NodeOptions()
|
auto options = rclcpp::NodeOptions()
|
||||||
.arguments({"-r", "__ns:=/another_ns"});
|
.arguments({"--ros-args", "-r", "__ns:=/another_ns"});
|
||||||
auto node = std::make_shared<rclcpp::Node>("my_node", "/ns", options);
|
auto node = std::make_shared<rclcpp::Node>("my_node", "/ns", options);
|
||||||
EXPECT_STREQ("my_node", node->get_name());
|
EXPECT_STREQ("my_node", node->get_name());
|
||||||
EXPECT_STREQ("/another_ns", node->get_namespace());
|
EXPECT_STREQ("/another_ns", node->get_namespace());
|
||||||
|
|
|
@ -36,7 +36,7 @@ protected:
|
||||||
|
|
||||||
TEST_F(TestNodeWithGlobalArgs, local_arguments_before_global) {
|
TEST_F(TestNodeWithGlobalArgs, local_arguments_before_global) {
|
||||||
auto options = rclcpp::NodeOptions()
|
auto options = rclcpp::NodeOptions()
|
||||||
.arguments({"-r", "__node:=local_arguments_test"});
|
.arguments({"--ros-args", "-r", "__node:=local_arguments_test"});
|
||||||
|
|
||||||
auto node = rclcpp::Node::make_shared("orig_name", options);
|
auto node = rclcpp::Node::make_shared("orig_name", options);
|
||||||
EXPECT_STREQ("local_arguments_test", node->get_name());
|
EXPECT_STREQ("local_arguments_test", node->get_name());
|
||||||
|
|
|
@ -24,32 +24,7 @@
|
||||||
#include "rclcpp/node_options.hpp"
|
#include "rclcpp/node_options.hpp"
|
||||||
|
|
||||||
|
|
||||||
TEST(TestNodeOptions, implicit_ros_args) {
|
TEST(TestNodeOptions, ros_args_only) {
|
||||||
rcl_allocator_t allocator = rcl_get_default_allocator();
|
|
||||||
auto options = rclcpp::NodeOptions(allocator)
|
|
||||||
.arguments({"-r", "__node:=some_node", "-r", "__ns:=/some_ns"});
|
|
||||||
|
|
||||||
const rcl_node_options_t * rcl_options = options.get_rcl_node_options();
|
|
||||||
ASSERT_TRUE(rcl_options != nullptr);
|
|
||||||
ASSERT_EQ(0, rcl_arguments_get_count_unparsed(&rcl_options->arguments));
|
|
||||||
ASSERT_EQ(0, rcl_arguments_get_count_unparsed_ros(&rcl_options->arguments));
|
|
||||||
|
|
||||||
char * node_name = nullptr;
|
|
||||||
EXPECT_EQ(RCL_RET_OK, rcl_remap_node_name(
|
|
||||||
&rcl_options->arguments, nullptr, "my_node", allocator, &node_name));
|
|
||||||
ASSERT_TRUE(node_name != nullptr);
|
|
||||||
EXPECT_STREQ("some_node", node_name);
|
|
||||||
allocator.deallocate(node_name, allocator.state);
|
|
||||||
|
|
||||||
char * namespace_name = nullptr;
|
|
||||||
EXPECT_EQ(RCL_RET_OK, rcl_remap_node_namespace(
|
|
||||||
&rcl_options->arguments, nullptr, "my_ns", allocator, &namespace_name));
|
|
||||||
ASSERT_TRUE(namespace_name != nullptr);
|
|
||||||
EXPECT_STREQ("/some_ns", namespace_name);
|
|
||||||
allocator.deallocate(namespace_name, allocator.state);
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST(TestNodeOptions, explicit_ros_args) {
|
|
||||||
rcl_allocator_t allocator = rcl_get_default_allocator();
|
rcl_allocator_t allocator = rcl_get_default_allocator();
|
||||||
auto options = rclcpp::NodeOptions(allocator)
|
auto options = rclcpp::NodeOptions(allocator)
|
||||||
.arguments({"--ros-args", "-r", "__node:=some_node", "-r", "__ns:=/some_ns"});
|
.arguments({"--ros-args", "-r", "__node:=some_node", "-r", "__ns:=/some_ns"});
|
||||||
|
@ -74,7 +49,7 @@ TEST(TestNodeOptions, explicit_ros_args) {
|
||||||
allocator.deallocate(namespace_name, allocator.state);
|
allocator.deallocate(namespace_name, allocator.state);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST(TestNodeOptions, explicit_ros_args_and_non_ros_args) {
|
TEST(TestNodeOptions, ros_args_and_non_ros_args) {
|
||||||
rcl_allocator_t allocator = rcl_get_default_allocator();
|
rcl_allocator_t allocator = rcl_get_default_allocator();
|
||||||
auto options = rclcpp::NodeOptions(allocator).arguments({
|
auto options = rclcpp::NodeOptions(allocator).arguments({
|
||||||
"--non-ros-flag", "--ros-args", "-r", "__node:=some_node",
|
"--non-ros-flag", "--ros-args", "-r", "__node:=some_node",
|
||||||
|
|
|
@ -143,7 +143,8 @@ ComponentManager::OnLoadNode(
|
||||||
}
|
}
|
||||||
|
|
||||||
std::vector<std::string> remap_rules;
|
std::vector<std::string> remap_rules;
|
||||||
remap_rules.reserve(request->remap_rules.size() * 2);
|
remap_rules.reserve(request->remap_rules.size() * 2 + 1);
|
||||||
|
remap_rules.push_back("--ros-args");
|
||||||
for (const std::string & rule : request->remap_rules) {
|
for (const std::string & rule : request->remap_rules) {
|
||||||
remap_rules.push_back("-r");
|
remap_rules.push_back("-r");
|
||||||
remap_rules.push_back(rule);
|
remap_rules.push_back(rule);
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue