diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 42b6cda8e1..f7ee34836d 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -361,26 +361,22 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( // Append the tf prefix if there is one std::string tf_prefix = ""; - if (params_.tf_frame_prefix_enable) + if (!params_.tf_frame_prefix.empty()) { - if (params_.tf_frame_prefix != "") - { - tf_prefix = params_.tf_frame_prefix; - } - else - { - tf_prefix = std::string(get_node()->get_namespace()); - } - - if (tf_prefix == "/") - { - tf_prefix = ""; - } - else + tf_prefix = params_.tf_frame_prefix; + } + else + { + tf_prefix = std::string(get_node()->get_namespace()); + if (tf_prefix != "/") { - tf_prefix = tf_prefix + "/"; + tf_prefix += '/'; } } + if (tf_prefix.front() == '/') + { + tf_prefix.erase(0, 1); + } const auto odom_frame_id = tf_prefix + params_.odom_frame_id; const auto base_frame_id = tf_prefix + params_.base_frame_id; diff --git a/diff_drive_controller/src/diff_drive_controller_parameter.yaml b/diff_drive_controller/src/diff_drive_controller_parameter.yaml index 755259cc2a..a1739901b3 100644 --- a/diff_drive_controller/src/diff_drive_controller_parameter.yaml +++ b/diff_drive_controller/src/diff_drive_controller_parameter.yaml @@ -46,11 +46,6 @@ diff_drive_controller: default_value: 1.0, description: "Correction factor when radius of right wheels differs from the nominal value in ``wheel_radius`` parameter.", } - tf_frame_prefix_enable: { - type: bool, - default_value: true, - description: "Enables or disables appending tf_prefix to tf frame id's.", - } tf_frame_prefix: { type: string, default_value: "", diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index f3f99ac8d8..3c6a9ab5fa 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "test_diff_drive_controller.hpp" + #include #include @@ -257,169 +259,42 @@ TEST_F(TestDiffDriveController, configure_succeeds_when_wheels_are_specified) EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); } -TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_no_namespace) -{ - std::string odom_id = "odom"; - std::string base_link_id = "base_link"; - std::string frame_prefix = "test_prefix"; - - ASSERT_EQ( - InitController( - left_wheel_names, right_wheel_names, - {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(false)), - rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), - rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), - rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}), - controller_interface::return_type::OK); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - - auto odometry_message = controller_->get_rt_odom_publisher()->msg_; - std::string test_odom_frame_id = odometry_message.header.frame_id; - std::string test_base_frame_id = odometry_message.child_frame_id; - /* tf_frame_prefix_enable is false so no modifications to the frame id's */ - ASSERT_EQ(test_odom_frame_id, odom_id); - ASSERT_EQ(test_base_frame_id, base_link_id); -} - -TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_no_namespace) +TEST_F(TestDiffDriveController, TfPrefixNamespaceParams) { std::string odom_id = "odom"; std::string base_link_id = "base_link"; - std::string frame_prefix = "test_prefix"; - - ASSERT_EQ( - InitController( - left_wheel_names, right_wheel_names, - {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true)), - rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), - rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), - rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}), - controller_interface::return_type::OK); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - - auto odometry_message = controller_->get_rt_odom_publisher()->msg_; - std::string test_odom_frame_id = odometry_message.header.frame_id; - std::string test_base_frame_id = odometry_message.child_frame_id; - - /* tf_frame_prefix_enable is true and frame_prefix is not blank so should be appended to the frame - * id's */ - ASSERT_EQ(test_odom_frame_id, frame_prefix + "/" + odom_id); - ASSERT_EQ(test_base_frame_id, frame_prefix + "/" + base_link_id); -} - -TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_no_namespace) -{ - std::string odom_id = "odom"; - std::string base_link_id = "base_link"; - std::string frame_prefix = ""; - - ASSERT_EQ( - InitController( - left_wheel_names, right_wheel_names, - {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true)), - rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), - rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), - rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}), - controller_interface::return_type::OK); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - - auto odometry_message = controller_->get_rt_odom_publisher()->msg_; - std::string test_odom_frame_id = odometry_message.header.frame_id; - std::string test_base_frame_id = odometry_message.child_frame_id; - /* tf_frame_prefix_enable is true but frame_prefix is blank so should not be appended to the frame - * id's */ - ASSERT_EQ(test_odom_frame_id, odom_id); - ASSERT_EQ(test_base_frame_id, base_link_id); -} - -TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_set_namespace) -{ - std::string test_namespace = "/test_namespace"; - - std::string odom_id = "odom"; - std::string base_link_id = "base_link"; - std::string frame_prefix = "test_prefix"; - - ASSERT_EQ( - InitController( - left_wheel_names, right_wheel_names, - {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(false)), - rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), - rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), - rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}, - test_namespace), - controller_interface::return_type::OK); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - - auto odometry_message = controller_->get_rt_odom_publisher()->msg_; - std::string test_odom_frame_id = odometry_message.header.frame_id; - std::string test_base_frame_id = odometry_message.child_frame_id; - /* tf_frame_prefix_enable is false so no modifications to the frame id's */ - ASSERT_EQ(test_odom_frame_id, odom_id); - ASSERT_EQ(test_base_frame_id, base_link_id); -} - -TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_set_namespace) -{ - std::string test_namespace = "/test_namespace"; - - std::string odom_id = "odom"; - std::string base_link_id = "base_link"; - std::string frame_prefix = "test_prefix"; - - ASSERT_EQ( - InitController( - left_wheel_names, right_wheel_names, - {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true)), - rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), - rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), - rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}, - test_namespace), - controller_interface::return_type::OK); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - - auto odometry_message = controller_->get_rt_odom_publisher()->msg_; - std::string test_odom_frame_id = odometry_message.header.frame_id; - std::string test_base_frame_id = odometry_message.child_frame_id; - - /* tf_frame_prefix_enable is true and frame_prefix is not blank so should be appended to the frame - * id's instead of the namespace*/ - ASSERT_EQ(test_odom_frame_id, frame_prefix + "/" + odom_id); - ASSERT_EQ(test_base_frame_id, frame_prefix + "/" + base_link_id); -} - -TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_set_namespace) -{ - std::string test_namespace = "/test_namespace"; + const std::vector test_cases = { + {"", "", ""}, + {"/", "", ""}, + {"", "/", ""}, + {"test_prefix", "", "test_prefix"}, + {"/test_prefix", "", "test_prefix"}, + {"", "test_namespace", "test_namespace/"}, + {"", "/test_namespace", "test_namespace/"}, + {"test_prefix", "test_namespace", "test_prefix"}, + }; + + for (const auto & test_case : test_cases) + { + std::vector params = { + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(test_case.tf_prefix)), + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id)), + }; - std::string odom_id = "odom"; - std::string base_link_id = "base_link"; - std::string frame_prefix = ""; + ASSERT_EQ( + InitController(left_wheel_names, right_wheel_names, params, test_case.ns), + controller_interface::return_type::OK); - ASSERT_EQ( - InitController( - left_wheel_names, right_wheel_names, - {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true)), - rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), - rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), - rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}, - test_namespace), - controller_interface::return_type::OK); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + auto odometry_message = controller_->get_rt_odom_publisher()->msg_; + std::string test_odom_frame_id = odometry_message.header.frame_id; + std::string test_base_frame_id = odometry_message.child_frame_id; - auto odometry_message = controller_->get_rt_odom_publisher()->msg_; - std::string test_odom_frame_id = odometry_message.header.frame_id; - std::string test_base_frame_id = odometry_message.child_frame_id; - /* tf_frame_prefix_enable is true but frame_prefix is blank so namespace should be appended to the - * frame id's */ - ASSERT_EQ(test_odom_frame_id, test_namespace + "/" + odom_id); - ASSERT_EQ(test_base_frame_id, test_namespace + "/" + base_link_id); + ASSERT_EQ(test_odom_frame_id, test_case.result_prefix + odom_id); + ASSERT_EQ(test_base_frame_id, test_case.result_prefix + base_link_id); + } } TEST_F(TestDiffDriveController, activate_fails_without_resources_assigned) diff --git a/diff_drive_controller/test/test_diff_drive_controller.hpp b/diff_drive_controller/test/test_diff_drive_controller.hpp new file mode 100644 index 0000000000..64368410fa --- /dev/null +++ b/diff_drive_controller/test/test_diff_drive_controller.hpp @@ -0,0 +1,22 @@ +// Copyright 2020 PAL Robotics SL. +// +// 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 + +struct PrefixTestCase +{ + std::string tf_prefix; + std::string ns; + std::string result_prefix; +}; diff --git a/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp index 04a28e5368..8552e5bd96 100644 --- a/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp @@ -45,8 +45,24 @@ controller_interface::CallbackReturn IMUSensorBroadcaster::on_configure( { params_ = param_listener_->get_params(); + std::string namespace_for_sensor_name = ""; + if (params_.use_namespace_as_sensor_name_prefix) + { + namespace_for_sensor_name = std::string(get_node()->get_namespace()); + ; + namespace_for_sensor_name.erase(0, 1); + + if (*std::end(namespace_for_sensor_name) != '/' and namespace_for_sensor_name.size()) + { + namespace_for_sensor_name = namespace_for_sensor_name + "/"; + } + } + + RCLCPP_INFO_STREAM( + get_node()->get_logger(), "Sensor name: " << namespace_for_sensor_name + params_.sensor_name); + imu_sensor_ = std::make_unique( - semantic_components::IMUSensor(params_.sensor_name)); + semantic_components::IMUSensor(namespace_for_sensor_name + params_.sensor_name)); try { // register ft sensor data publisher @@ -62,8 +78,26 @@ controller_interface::CallbackReturn IMUSensorBroadcaster::on_configure( return CallbackReturn::ERROR; } + std::string tf_prefix = ""; + if (!params_.tf_frame_prefix.empty()) + { + tf_prefix = params_.tf_frame_prefix; + } + else + { + tf_prefix = std::string(get_node()->get_namespace()); + if (tf_prefix != "/") + { + tf_prefix += '/'; + } + } + if (tf_prefix.front() == '/') + { + tf_prefix.erase(0, 1); + } + realtime_publisher_->lock(); - realtime_publisher_->msg_.header.frame_id = params_.frame_id; + realtime_publisher_->msg_.header.frame_id = tf_prefix + params_.frame_id; // convert double vector to fixed-size array in the message for (size_t i = 0; i < 9; ++i) { diff --git a/imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml b/imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml index d45bf8583b..82c9f3422c 100644 --- a/imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml +++ b/imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml @@ -41,3 +41,13 @@ imu_sensor_broadcaster: fixed_size<>: [9], } } + tf_frame_prefix: { + type: string, + default_value: "", + description: "(optional) Prefix to be appended to the tf frames, will be added to the sensor's frame_id before publishing. If the parameter is empty, controller's namespace will be used.", + } + use_namespace_as_sensor_name_prefix: { + type: bool, + default_value: false, + description: "If true the '/namespace/' is added to the sensor name which causes changes in interfaces names e. g. /namespace/sensor_name/orientation.x", + } diff --git a/imu_sensor_broadcaster/test/imu_sensor_broadcaster_params.yaml b/imu_sensor_broadcaster/test/imu_sensor_broadcaster_params.yaml index c1c660d2c4..be299ba1ea 100644 --- a/imu_sensor_broadcaster/test/imu_sensor_broadcaster_params.yaml +++ b/imu_sensor_broadcaster/test/imu_sensor_broadcaster_params.yaml @@ -1,5 +1,6 @@ -test_imu_sensor_broadcaster: - ros__parameters: +/**: + test_imu_sensor_broadcaster: + ros__parameters: - sensor_name: "imu_sensor" - frame_id: "imu_sensor_frame" + sensor_name: "imu_sensor" + frame_id: "imu_sensor_frame" diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp index 25a39a8b4d..c0951020d9 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp @@ -53,10 +53,10 @@ void IMUSensorBroadcasterTest::SetUp() void IMUSensorBroadcasterTest::TearDown() { imu_broadcaster_.reset(nullptr); } -void IMUSensorBroadcasterTest::SetUpIMUBroadcaster() +void IMUSensorBroadcasterTest::SetUpIMUBroadcaster(const std::string & ns) { const auto result = imu_broadcaster_->init( - "test_imu_sensor_broadcaster", "", 0, "", imu_broadcaster_->define_custom_node_options()); + "test_imu_sensor_broadcaster", "", 0, ns, imu_broadcaster_->define_custom_node_options()); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector state_ifs; @@ -74,13 +74,14 @@ void IMUSensorBroadcasterTest::SetUpIMUBroadcaster() imu_broadcaster_->assign_interfaces({}, std::move(state_ifs)); } -void IMUSensorBroadcasterTest::subscribe_and_get_message(sensor_msgs::msg::Imu & imu_msg) +void IMUSensorBroadcasterTest::subscribe_and_get_message( + sensor_msgs::msg::Imu & imu_msg, const std::string & ns) { // create a new subscriber - rclcpp::Node test_subscription_node("test_subscription_node"); + rclcpp::Node test_subscription_node("test_subscription_node", ns); auto subs_callback = [&](const sensor_msgs::msg::Imu::SharedPtr) {}; auto subscription = test_subscription_node.create_subscription( - "/test_imu_sensor_broadcaster/imu", 10, subs_callback); + "test_imu_sensor_broadcaster/imu", 10, subs_callback); // call update to publish the test value // since update doesn't guarantee a published message, republish until received @@ -208,6 +209,85 @@ TEST_F(IMUSensorBroadcasterTest, SensorName_Publish_Success) } } +TEST_F(IMUSensorBroadcasterTest, TfPrefixNamespaceParams) +{ + struct TestCase + { + std::string tf_prefix; + std::string ns; + std::string result_prefix; + }; + + const std::vector test_cases = { + {"", "", ""}, + {"/", "", ""}, + {"", "/", ""}, + {"test_prefix", "", "test_prefix"}, + {"/test_prefix", "", "test_prefix"}, + {"", "test_namespace", "test_namespace/"}, + {"", "/test_namespace", "test_namespace/"}, + {"test_prefix", "test_namespace", "test_prefix"}, + }; + + for (const auto & test_case : test_cases) + { + const std::string & test_namespace = test_case.ns; + + SetUpIMUBroadcaster(test_namespace); + + imu_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); + imu_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + imu_broadcaster_->get_node()->set_parameter({"tf_frame_prefix", test_case.tf_prefix}); + + ASSERT_EQ(imu_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(imu_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + sensor_msgs::msg::Imu imu_msg; + subscribe_and_get_message(imu_msg, test_namespace); + + EXPECT_EQ(imu_msg.header.frame_id, test_case.result_prefix + frame_id_); + } +} + +TEST_F(IMUSensorBroadcasterTest, SensorNameNamespaced) +{ + struct TestCase + { + bool use_namespace_as_sensor_name_prefix; + std::string result; + }; + + const std::string & test_namespace = "test_namespace"; + const std::vector test_cases = { + {false, ""}, + {true, test_namespace + "/"}, + }; + + for (const auto & test_case : test_cases) + { + SetUpIMUBroadcaster(test_namespace); + + imu_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); + imu_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + imu_broadcaster_->get_node()->set_parameter({"use_namespace_as_sensor_name_prefix", test_case.use_namespace_as_sensor_name_prefix}); + + ASSERT_EQ(imu_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto interface_names = imu_broadcaster_->imu_sensor_->get_state_interface_names(); + + EXPECT_EQ(interface_names[0], test_case.result + imu_orientation_x_.get_name()); + EXPECT_EQ(interface_names[1], test_case.result + imu_orientation_y_.get_name()); + EXPECT_EQ(interface_names[2], test_case.result + imu_orientation_z_.get_name()); + EXPECT_EQ(interface_names[3], test_case.result + imu_orientation_w_.get_name()); + EXPECT_EQ(interface_names[4], test_case.result + imu_angular_velocity_x_.get_name()); + EXPECT_EQ(interface_names[5], test_case.result + imu_angular_velocity_y_.get_name()); + EXPECT_EQ(interface_names[6], test_case.result + imu_angular_velocity_z_.get_name()); + EXPECT_EQ(interface_names[7], test_case.result + imu_linear_acceleration_x_.get_name()); + EXPECT_EQ(interface_names[8], test_case.result + imu_linear_acceleration_y_.get_name()); + EXPECT_EQ(interface_names[9], test_case.result + imu_linear_acceleration_z_.get_name()); + } +} + int main(int argc, char ** argv) { ::testing::InitGoogleMock(&argc, argv); diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp index 01423724b8..ec7e86d6e8 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp @@ -49,7 +49,7 @@ class IMUSensorBroadcasterTest : public ::testing::Test void SetUp(); void TearDown(); - void SetUpIMUBroadcaster(); + void SetUpIMUBroadcaster(const std::string & ns = ""); protected: const std::string sensor_name_ = "imu_sensor"; @@ -78,7 +78,7 @@ class IMUSensorBroadcasterTest : public ::testing::Test std::unique_ptr imu_broadcaster_; - void subscribe_and_get_message(sensor_msgs::msg::Imu & imu_msg); + void subscribe_and_get_message(sensor_msgs::msg::Imu & imu_msg, const std::string & ns = ""); }; #endif // TEST_IMU_SENSOR_BROADCASTER_HPP_