From 2412651d7027f56b2e2e69946f7c86dfc95348f3 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Mon, 22 Apr 2024 12:10:59 +0200 Subject: [PATCH 1/9] tf_prefix param: fix slashes and add to IMU Broadcaster --- .../src/diff_drive_controller.cpp | 28 ++- .../src/diff_drive_controller_parameter.yaml | 5 - .../test/test_diff_drive_controller.cpp | 193 ++++-------------- .../test/test_diff_drive_controller.hpp | 22 ++ .../src/imu_sensor_broadcaster.cpp | 18 ++ .../imu_sensor_broadcaster_parameters.yaml | 5 + .../test/imu_sensor_broadcaster_params.yaml | 9 +- .../test/test_imu_sensor_broadcaster.cpp | 36 +++- .../test/test_imu_sensor_broadcaster.hpp | 7 + 9 files changed, 141 insertions(+), 182 deletions(-) create mode 100644 diff_drive_controller/test/test_diff_drive_controller.hpp 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 9720e068e1..1483d5b23e 100644 --- a/diff_drive_controller/src/diff_drive_controller_parameter.yaml +++ b/diff_drive_controller/src/diff_drive_controller_parameter.yaml @@ -40,11 +40,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 9ab3022a9f..ccc654ab31 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 @@ -253,169 +255,48 @@ 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) +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(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) -{ - 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; + const std::vector test_prefix_matrix = { + {"", "", ""}, + {"/", "", ""}, + {"", "/", ""}, + {"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 & params : test_prefix_matrix) + { + const auto ret = controller_->init(controller_name, params.ns); + ASSERT_EQ(ret, controller_interface::return_type::OK); - /* 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); -} + std::string odom_id = "odom"; + std::string base_link_id = "base_link"; -TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_set_namespace) -{ - std::string test_namespace = "/test_namespace"; + controller_->get_node()->set_parameter( + rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - std::string odom_id = "odom"; - std::string base_link_id = "base_link"; - std::string frame_prefix = ""; + controller_->get_node()->set_parameter( + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(params.tf_prefix))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))); - 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, params.result_prefix + odom_id); + ASSERT_EQ(test_base_frame_id, params.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..a5b814c9c9 --- /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 TestPrefixParams +{ + 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..ee7ec3a6f1 100644 --- a/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp @@ -62,6 +62,24 @@ 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; // convert double vector to fixed-size array in the message diff --git a/imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml b/imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml index d45bf8583b..d75882729a 100644 --- a/imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml +++ b/imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml @@ -41,3 +41,8 @@ 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.", + } 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..65e429e075 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp @@ -74,7 +74,8 @@ 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"); @@ -208,6 +209,39 @@ TEST_F(IMUSensorBroadcasterTest, SensorName_Publish_Success) } } +TEST_F(IMUSensorBroadcasterTest, TfPrefixNamespaceParams) +{ + const std::vector test_prefix_matrix = { + {"", "", ""}, + {"/", "", ""}, + {"", "/", ""}, + {"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 & params : test_prefix_matrix) + { + const std::string & test_namespace = params.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", params.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, params.result_prefix + frame_id_); + } +} + 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..5d53e810e6 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp @@ -81,4 +81,11 @@ class IMUSensorBroadcasterTest : public ::testing::Test void subscribe_and_get_message(sensor_msgs::msg::Imu & imu_msg); }; +struct TestPrefixParams +{ + std::string tf_prefix; + std::string ns; + std::string result_prefix; +}; + #endif // TEST_IMU_SENSOR_BROADCASTER_HPP_ From a38ed953f6f3c54bc334de11e7f7d6434cad0ce9 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Fri, 26 Apr 2024 11:21:24 +0200 Subject: [PATCH 2/9] Add namespace --- .../test/test_imu_sensor_broadcaster.cpp | 8 ++++---- .../test/test_imu_sensor_broadcaster.hpp | 4 ++-- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp index 65e429e075..f595b7406e 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", ns, 0, "", imu_broadcaster_->define_custom_node_options()); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector state_ifs; @@ -78,10 +78,10 @@ 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 diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp index 5d53e810e6..08ba12ef11 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 = ""); }; struct TestPrefixParams From e4aa06e2d30e437e71a41eff639c17e721e8070a Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Fri, 26 Apr 2024 11:54:46 +0200 Subject: [PATCH 3/9] Use InitController --- .../test/test_diff_drive_controller.cpp | 26 +++++++------------ 1 file changed, 10 insertions(+), 16 deletions(-) diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index ccc654ab31..b1b57499df 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -268,25 +268,19 @@ TEST_F(TestDiffDriveController, TfPrefixNamespaceParams) {"test_prefix", "test_namespace", "test_prefix"}, }; - for (const auto & params : test_prefix_matrix) + for (const auto & test_case : test_prefix_matrix) { - const auto ret = controller_->init(controller_name, params.ns); - ASSERT_EQ(ret, controller_interface::return_type::OK); - std::string odom_id = "odom"; std::string base_link_id = "base_link"; - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); + std::vector params = + { + rclcpp::Parameter("tf_frame_prefix", test_case.tf_prefix), + rclcpp::Parameter("odom_frame_id", odom_id), + rclcpp::Parameter("base_frame_id", base_link_id), + } - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(params.tf_prefix))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))); + ASSERT_EQ(InitController(parameters = params, ns = test_case.ns), controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -294,8 +288,8 @@ TEST_F(TestDiffDriveController, TfPrefixNamespaceParams) std::string test_odom_frame_id = odometry_message.header.frame_id; std::string test_base_frame_id = odometry_message.child_frame_id; - ASSERT_EQ(test_odom_frame_id, params.result_prefix + odom_id); - ASSERT_EQ(test_base_frame_id, params.result_prefix + 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); } } From df173d80a7d7b8d1419a1efa23a8a41c2d24d366 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Fri, 26 Apr 2024 12:01:00 +0200 Subject: [PATCH 4/9] Rename variable --- .../test/test_diff_drive_controller.cpp | 8 +++++--- .../test/test_diff_drive_controller.hpp | 2 +- .../test/test_imu_sensor_broadcaster.cpp | 10 +++++----- .../test/test_imu_sensor_broadcaster.hpp | 2 +- 4 files changed, 12 insertions(+), 10 deletions(-) diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index b1b57499df..527d595ea6 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -257,7 +257,7 @@ TEST_F(TestDiffDriveController, configure_succeeds_when_wheels_are_specified) TEST_F(TestDiffDriveController, TfPrefixNamespaceParams) { - const std::vector test_prefix_matrix = { + const std::vector test_cases = { {"", "", ""}, {"/", "", ""}, {"", "/", ""}, @@ -268,7 +268,7 @@ TEST_F(TestDiffDriveController, TfPrefixNamespaceParams) {"test_prefix", "test_namespace", "test_prefix"}, }; - for (const auto & test_case : test_prefix_matrix) + for (const auto & test_case : test_cases) { std::string odom_id = "odom"; std::string base_link_id = "base_link"; @@ -280,7 +280,9 @@ TEST_F(TestDiffDriveController, TfPrefixNamespaceParams) rclcpp::Parameter("base_frame_id", base_link_id), } - ASSERT_EQ(InitController(parameters = params, ns = test_case.ns), controller_interface::return_type::OK); + ASSERT_EQ( + InitController(parameters = params, ns = test_case.ns), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); diff --git a/diff_drive_controller/test/test_diff_drive_controller.hpp b/diff_drive_controller/test/test_diff_drive_controller.hpp index a5b814c9c9..64368410fa 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.hpp +++ b/diff_drive_controller/test/test_diff_drive_controller.hpp @@ -14,7 +14,7 @@ #include -struct TestPrefixParams +struct PrefixTestCase { std::string tf_prefix; std::string ns; diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp index f595b7406e..c2fed8cfba 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp @@ -211,7 +211,7 @@ TEST_F(IMUSensorBroadcasterTest, SensorName_Publish_Success) TEST_F(IMUSensorBroadcasterTest, TfPrefixNamespaceParams) { - const std::vector test_prefix_matrix = { + const std::vector test_cases = { {"", "", ""}, {"/", "", ""}, {"", "/", ""}, @@ -222,15 +222,15 @@ TEST_F(IMUSensorBroadcasterTest, TfPrefixNamespaceParams) {"test_prefix", "test_namespace", "test_prefix"}, }; - for (const auto & params : test_prefix_matrix) + for (const auto & test_case : test_cases) { - const std::string & test_namespace = params.ns; + 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", params.tf_prefix}); + 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); @@ -238,7 +238,7 @@ TEST_F(IMUSensorBroadcasterTest, TfPrefixNamespaceParams) sensor_msgs::msg::Imu imu_msg; subscribe_and_get_message(imu_msg, test_namespace); - EXPECT_EQ(imu_msg.header.frame_id, params.result_prefix + frame_id_); + EXPECT_EQ(imu_msg.header.frame_id, test_case.result_prefix + frame_id_); } } diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp index 08ba12ef11..9e7e7f9dde 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp @@ -81,7 +81,7 @@ class IMUSensorBroadcasterTest : public ::testing::Test void subscribe_and_get_message(sensor_msgs::msg::Imu & imu_msg, const std::string & ns = ""); }; -struct TestPrefixParams +struct PrefixTestCase { std::string tf_prefix; std::string ns; From bd24d4c307d465df41a47f3c86ffb5225217e810 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Fri, 26 Apr 2024 12:07:31 +0200 Subject: [PATCH 5/9] typo --- .../test/test_diff_drive_controller.cpp | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index 527d595ea6..dc6f5ee106 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -257,6 +257,8 @@ TEST_F(TestDiffDriveController, configure_succeeds_when_wheels_are_specified) TEST_F(TestDiffDriveController, TfPrefixNamespaceParams) { + std::string odom_id = "odom"; + std::string base_link_id = "base_link"; const std::vector test_cases = { {"", "", ""}, {"/", "", ""}, @@ -270,18 +272,15 @@ TEST_F(TestDiffDriveController, TfPrefixNamespaceParams) for (const auto & test_case : test_cases) { - std::string odom_id = "odom"; - std::string base_link_id = "base_link"; - std::vector params = { - rclcpp::Parameter("tf_frame_prefix", test_case.tf_prefix), - rclcpp::Parameter("odom_frame_id", odom_id), - rclcpp::Parameter("base_frame_id", base_link_id), - } + 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)), + }; ASSERT_EQ( - InitController(parameters = params, ns = test_case.ns), + InitController(left_wheel_names, right_wheel_names, params, test_case.ns), controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); From 92679fcf173f7c1dd4cc9f2c1cf8def9db05fb96 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Fri, 26 Apr 2024 12:45:03 +0200 Subject: [PATCH 6/9] pre-commit --- .../test/test_diff_drive_controller.cpp | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index dc6f5ee106..0145142eaa 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -272,12 +272,11 @@ TEST_F(TestDiffDriveController, TfPrefixNamespaceParams) 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::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)), + }; ASSERT_EQ( InitController(left_wheel_names, right_wheel_names, params, test_case.ns), From 52104aa6258ad7d50f95cc84a780c63e2e564428 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Fri, 26 Apr 2024 13:02:36 +0200 Subject: [PATCH 7/9] Fix imu --- imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp index ee7ec3a6f1..baad857c23 100644 --- a/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp @@ -81,7 +81,7 @@ controller_interface::CallbackReturn IMUSensorBroadcaster::on_configure( } 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) { From 1292fad65c4a2bab39106e5bf995a5e626d78bf6 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Fri, 26 Apr 2024 17:56:00 +0200 Subject: [PATCH 8/9] fix imu namespace --- imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp index c2fed8cfba..dea0dd1c7e 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp @@ -56,7 +56,7 @@ void IMUSensorBroadcasterTest::TearDown() { imu_broadcaster_.reset(nullptr); } void IMUSensorBroadcasterTest::SetUpIMUBroadcaster(const std::string & ns) { const auto result = imu_broadcaster_->init( - "test_imu_sensor_broadcaster", ns, 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; From 70cfc298460e7717c7ae6a3aaa2b8a3916f99490 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Tue, 14 May 2024 16:28:51 +0200 Subject: [PATCH 9/9] Add sensor_name prefix --- .../src/imu_sensor_broadcaster.cpp | 18 ++++++- .../imu_sensor_broadcaster_parameters.yaml | 5 ++ .../test/test_imu_sensor_broadcaster.cpp | 50 ++++++++++++++++++- .../test/test_imu_sensor_broadcaster.hpp | 7 --- 4 files changed, 70 insertions(+), 10 deletions(-) diff --git a/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp index baad857c23..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 diff --git a/imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml b/imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml index d75882729a..82c9f3422c 100644 --- a/imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml +++ b/imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml @@ -46,3 +46,8 @@ imu_sensor_broadcaster: 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/test_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp index dea0dd1c7e..c0951020d9 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp @@ -56,7 +56,7 @@ void IMUSensorBroadcasterTest::TearDown() { imu_broadcaster_.reset(nullptr); } void IMUSensorBroadcasterTest::SetUpIMUBroadcaster(const std::string & ns) { const auto result = imu_broadcaster_->init( - "test_imu_sensor_broadcaster", 0, ns, 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; @@ -211,7 +211,14 @@ TEST_F(IMUSensorBroadcasterTest, SensorName_Publish_Success) TEST_F(IMUSensorBroadcasterTest, TfPrefixNamespaceParams) { - const std::vector test_cases = { + struct TestCase + { + std::string tf_prefix; + std::string ns; + std::string result_prefix; + }; + + const std::vector test_cases = { {"", "", ""}, {"/", "", ""}, {"", "/", ""}, @@ -242,6 +249,45 @@ TEST_F(IMUSensorBroadcasterTest, TfPrefixNamespaceParams) } } +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 9e7e7f9dde..ec7e86d6e8 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp @@ -81,11 +81,4 @@ class IMUSensorBroadcasterTest : public ::testing::Test void subscribe_and_get_message(sensor_msgs::msg::Imu & imu_msg, const std::string & ns = ""); }; -struct PrefixTestCase -{ - std::string tf_prefix; - std::string ns; - std::string result_prefix; -}; - #endif // TEST_IMU_SENSOR_BROADCASTER_HPP_