From 79ead8ba9d46f4f718b0be00bfa27a94ee28d93b Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Sat, 20 Apr 2024 13:20:59 +0200 Subject: [PATCH 1/9] tf_prefix: fix slashes and add to IMU --- .../src/diff_drive_controller.cpp | 16 +- .../test/test_diff_drive_controller.cpp | 10 +- .../src/imu_sensor_broadcaster.cpp | 20 ++- .../imu_sensor_broadcaster_parameters.yaml | 24 ++- .../test/test_imu_sensor_broadcaster.cpp | 158 +++++++++++++++++- .../test/test_imu_sensor_broadcaster.hpp | 5 +- 6 files changed, 204 insertions(+), 29 deletions(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index c8032e5dc5..07c348bbb7 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -396,22 +396,18 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( std::string tf_prefix = ""; if (params_.tf_frame_prefix_enable) { - if (params_.tf_frame_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 = ""; - } - else - { - tf_prefix = tf_prefix + "/"; + tf_prefix.erase(0, 1); + if (!tf_prefix.empty()) + { + tf_prefix = tf_prefix + '/'; + } } } diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index db89d4f873..c9025bd33d 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -319,8 +319,8 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_no_namesp /* 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); + 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) @@ -425,13 +425,13 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_set_names /* 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); + 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"; + std::string test_namespace = "test_namespace"; const auto ret = controller_->init(controller_name, test_namespace); ASSERT_EQ(ret, controller_interface::return_type::OK); diff --git a/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp index 04a28e5368..a5f51b8728 100644 --- a/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp @@ -62,8 +62,26 @@ controller_interface::CallbackReturn IMUSensorBroadcaster::on_configure( return CallbackReturn::ERROR; } + std::string tf_prefix = ""; + if (params_.tf_frame_prefix_enable) + { + if (!params_.tf_frame_prefix.empty()) + { + tf_prefix = params_.tf_frame_prefix; + } + else + { + tf_prefix = std::string(get_node()->get_namespace()); + tf_prefix.erase(0, 1); + if (!tf_prefix.empty()) + { + tf_prefix = tf_prefix + '/'; + } + } + } + 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..60490ba259 100644 --- a/imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml +++ b/imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml @@ -5,17 +5,19 @@ imu_sensor_broadcaster: description: "Defines sensor name used as prefix for its interfaces. Interface names are: ``/orientation.x, ..., /angular_velocity.x, ..., /linear_acceleration.x.``", - validation: { - not_empty<>: null - } + # FIXME: Currently does not work with namespace + # validation: { + # not_empty<>: null + # } } frame_id: { type: string, default_value: "", description: "Sensor's frame_id in which values are published.", - validation: { - not_empty<>: null - } + # FIXME: Currently does not work with namespace + # validation: { + # not_empty<>: null + # } } static_covariance_orientation: { type: double_array, @@ -41,3 +43,13 @@ imu_sensor_broadcaster: fixed_size<>: [9], } } + 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: "", + description: "(optional) Prefix to be appended to the tf frames, will be added to odom_id and base_frame_id before publishing. If the parameter is empty, controller's namespace will be used.", + } diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp index 0886748293..1ba851505a 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp @@ -54,9 +54,9 @@ 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"); + const auto result = imu_broadcaster_->init("test_imu_sensor_broadcaster", ns); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector state_ifs; @@ -74,13 +74,13 @@ 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 +208,154 @@ TEST_F(IMUSensorBroadcasterTest, SensorName_Publish_Success) } } +TEST_F(IMUSensorBroadcasterTest, NoPrefixNoNamespace) +{ + SetUpIMUBroadcaster(); + + std::string frame_prefix = "test_prefix"; + + // set the params 'sensor_name' and 'frame_id' + 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_enable", false}); + imu_broadcaster_->get_node()->set_parameter({"tf_frame_prefix", frame_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); + + EXPECT_EQ(imu_msg.header.frame_id, frame_id_); +} + +TEST_F(IMUSensorBroadcasterTest, PrefixNoNamespace) +{ + SetUpIMUBroadcaster(); + + std::string frame_prefix = "test_prefix"; + + // set the params 'sensor_name' and 'frame_id' + 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_enable", true}); + imu_broadcaster_->get_node()->set_parameter({"tf_frame_prefix", frame_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); + + EXPECT_EQ(imu_msg.header.frame_id, frame_prefix + frame_id_); +} + +TEST_F(IMUSensorBroadcasterTest, BlankPrefixNoNamespace) +{ + SetUpIMUBroadcaster(); + + std::string frame_prefix = ""; + + // set the params 'sensor_name' and 'frame_id' + 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_enable", true}); + imu_broadcaster_->get_node()->set_parameter({"tf_frame_prefix", frame_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); + + EXPECT_EQ(imu_msg.header.frame_id, frame_id_); +} + +TEST_F(IMUSensorBroadcasterTest, NoPrefixWithNamespace) +{ + std::string test_namespace = "test_namespace"; + + SetUpIMUBroadcaster(test_namespace); + + std::string frame_prefix = "test_prefix"; + + // set the params 'sensor_name' and 'frame_id' + 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_enable", false}); + imu_broadcaster_->get_node()->set_parameter({"tf_frame_prefix", frame_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, frame_id_); +} + +TEST_F(IMUSensorBroadcasterTest, PrefixWithNamespace) +{ + std::string test_namespace = "test_namespace"; + + SetUpIMUBroadcaster(test_namespace); + + std::string frame_prefix = "test_prefix"; + + // set the params 'sensor_name' and 'frame_id' + imu_broadcaster_->get_node()->set_parameter( + rclcpp::Parameter("sensor_name", rclcpp::ParameterValue(sensor_name_))); + imu_broadcaster_->get_node()->set_parameter( + rclcpp::Parameter("sensor_name", rclcpp::ParameterValue(sensor_name_))); + imu_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + + imu_broadcaster_->get_node()->set_parameter( + rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true))); + imu_broadcaster_->get_node()->set_parameter( + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_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, frame_prefix + frame_id_); +} + +TEST_F(IMUSensorBroadcasterTest, BlankPrefixWithNamespace) +{ + std::string test_namespace = "test_namespace"; + + SetUpIMUBroadcaster(test_namespace); + + std::string frame_prefix = ""; + + // set the params 'sensor_name' and 'frame_id' + imu_broadcaster_->get_node()->set_parameter( + rclcpp::Parameter("sensor_name", rclcpp::ParameterValue(sensor_name_))); + imu_broadcaster_->get_node()->set_parameter( + rclcpp::Parameter("sensor_name", rclcpp::ParameterValue(sensor_name_))); + imu_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + + imu_broadcaster_->get_node()->set_parameter( + rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true))); + imu_broadcaster_->get_node()->set_parameter( + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_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_namespace + "/" + 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..6db8afe3c1 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"; @@ -77,8 +77,9 @@ class IMUSensorBroadcasterTest : public ::testing::Test sensor_name_, "linear_acceleration.z", &sensor_values_[9]}; std::unique_ptr imu_broadcaster_; + std::string ns_; - 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_ From 38745aa70511f2c296ef494244ae4369b3b88ba5 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Sat, 20 Apr 2024 13:23:55 +0200 Subject: [PATCH 2/9] Formatting --- imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp | 3 ++- 1 file changed, 2 insertions(+), 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 1ba851505a..610463c7ac 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(const std::string & ns) imu_broadcaster_->assign_interfaces({}, std::move(state_ifs)); } -void IMUSensorBroadcasterTest::subscribe_and_get_message(sensor_msgs::msg::Imu & imu_msg, const std::string & ns) +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", ns); From 2c434b18a344c21b12a5261804afaed856cb4bc1 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Mon, 22 Apr 2024 09:57:33 +0200 Subject: [PATCH 3/9] Remove front slash suggestions --- diff_drive_controller/src/diff_drive_controller.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 07c348bbb7..126c21b1c9 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -403,12 +403,15 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( else { tf_prefix = std::string(get_node()->get_namespace()); - tf_prefix.erase(0, 1); - if (!tf_prefix.empty()) + 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; From 21eb1b52b3a62035164774d7a29e2d6083003733 Mon Sep 17 00:00:00 2001 From: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com> Date: Mon, 22 Apr 2024 10:12:52 +0200 Subject: [PATCH 4/9] Update imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml Co-authored-by: Sai Kishor Kothakota --- .../src/imu_sensor_broadcaster_parameters.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml b/imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml index 60490ba259..07e8c3fbe9 100644 --- a/imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml +++ b/imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml @@ -51,5 +51,5 @@ imu_sensor_broadcaster: tf_frame_prefix: { type: string, default_value: "", - description: "(optional) Prefix to be appended to the tf frames, will be added to odom_id and base_frame_id before publishing. If the parameter is empty, controller's namespace will be used.", + 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.", } From 765d8f62fb80f324cef0f90d39fc4420ec7e02d3 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Mon, 22 Apr 2024 11:17:40 +0200 Subject: [PATCH 5/9] Add matrix tests --- .../src/imu_sensor_broadcaster.cpp | 24 +-- .../imu_sensor_broadcaster_parameters.yaml | 5 - .../test/test_imu_sensor_broadcaster.cpp | 179 ++++-------------- .../test/test_imu_sensor_broadcaster.hpp | 6 + 4 files changed, 51 insertions(+), 163 deletions(-) diff --git a/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp index a5f51b8728..baad857c23 100644 --- a/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp @@ -63,22 +63,22 @@ controller_interface::CallbackReturn IMUSensorBroadcaster::on_configure( } std::string tf_prefix = ""; - if (params_.tf_frame_prefix_enable) + if (!params_.tf_frame_prefix.empty()) { - if (!params_.tf_frame_prefix.empty()) - { - tf_prefix = params_.tf_frame_prefix; - } - else + tf_prefix = params_.tf_frame_prefix; + } + else + { + tf_prefix = std::string(get_node()->get_namespace()); + if (tf_prefix != "/") { - tf_prefix = std::string(get_node()->get_namespace()); - tf_prefix.erase(0, 1); - if (!tf_prefix.empty()) - { - tf_prefix = tf_prefix + '/'; - } + tf_prefix += '/'; } } + if (tf_prefix.front() == '/') + { + tf_prefix.erase(0, 1); + } realtime_publisher_->lock(); realtime_publisher_->msg_.header.frame_id = tf_prefix + params_.frame_id; diff --git a/imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml b/imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml index 07e8c3fbe9..332f7f7dfd 100644 --- a/imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml +++ b/imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml @@ -43,11 +43,6 @@ imu_sensor_broadcaster: fixed_size<>: [9], } } - 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/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp index 610463c7ac..bd11d142bf 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp @@ -209,152 +209,39 @@ TEST_F(IMUSensorBroadcasterTest, SensorName_Publish_Success) } } -TEST_F(IMUSensorBroadcasterTest, NoPrefixNoNamespace) -{ - SetUpIMUBroadcaster(); - - std::string frame_prefix = "test_prefix"; - - // set the params 'sensor_name' and 'frame_id' - 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_enable", false}); - imu_broadcaster_->get_node()->set_parameter({"tf_frame_prefix", frame_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); - - EXPECT_EQ(imu_msg.header.frame_id, frame_id_); -} - -TEST_F(IMUSensorBroadcasterTest, PrefixNoNamespace) -{ - SetUpIMUBroadcaster(); - - std::string frame_prefix = "test_prefix"; - - // set the params 'sensor_name' and 'frame_id' - 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_enable", true}); - imu_broadcaster_->get_node()->set_parameter({"tf_frame_prefix", frame_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); - - EXPECT_EQ(imu_msg.header.frame_id, frame_prefix + frame_id_); -} - -TEST_F(IMUSensorBroadcasterTest, BlankPrefixNoNamespace) -{ - SetUpIMUBroadcaster(); - - std::string frame_prefix = ""; - - // set the params 'sensor_name' and 'frame_id' - 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_enable", true}); - imu_broadcaster_->get_node()->set_parameter({"tf_frame_prefix", frame_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); - - EXPECT_EQ(imu_msg.header.frame_id, frame_id_); -} - -TEST_F(IMUSensorBroadcasterTest, NoPrefixWithNamespace) -{ - std::string test_namespace = "test_namespace"; - - SetUpIMUBroadcaster(test_namespace); - - std::string frame_prefix = "test_prefix"; - - // set the params 'sensor_name' and 'frame_id' - 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_enable", false}); - imu_broadcaster_->get_node()->set_parameter({"tf_frame_prefix", frame_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, frame_id_); -} - -TEST_F(IMUSensorBroadcasterTest, PrefixWithNamespace) -{ - std::string test_namespace = "test_namespace"; - - SetUpIMUBroadcaster(test_namespace); - - std::string frame_prefix = "test_prefix"; - - // set the params 'sensor_name' and 'frame_id' - imu_broadcaster_->get_node()->set_parameter( - rclcpp::Parameter("sensor_name", rclcpp::ParameterValue(sensor_name_))); - imu_broadcaster_->get_node()->set_parameter( - rclcpp::Parameter("sensor_name", rclcpp::ParameterValue(sensor_name_))); - imu_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); - - imu_broadcaster_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true))); - imu_broadcaster_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_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, frame_prefix + frame_id_); -} - -TEST_F(IMUSensorBroadcasterTest, BlankPrefixWithNamespace) -{ - std::string test_namespace = "test_namespace"; - - SetUpIMUBroadcaster(test_namespace); - - std::string frame_prefix = ""; - - // set the params 'sensor_name' and 'frame_id' - imu_broadcaster_->get_node()->set_parameter( - rclcpp::Parameter("sensor_name", rclcpp::ParameterValue(sensor_name_))); - imu_broadcaster_->get_node()->set_parameter( - rclcpp::Parameter("sensor_name", rclcpp::ParameterValue(sensor_name_))); - imu_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); - - imu_broadcaster_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true))); - imu_broadcaster_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_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_namespace + "/" + frame_id_); +TEST_F(IMUSensorBroadcasterTest, TfPrefixNamespaceTests) { + 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"}, + }; + + int test_number = 0; // Counter for test number + for (const auto& params : test_prefix_matrix) { + ++test_number; // Increment the test number + const std::string& tf_prefix = params.tf_prefix; + 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", 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); + + std::cout << "Test number: " << test_number; + EXPECT_EQ(imu_msg.header.frame_id, params.result_prefix + frame_id_); + } } int main(int argc, char ** argv) diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp index 6db8afe3c1..befa8c1d5a 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp @@ -82,4 +82,10 @@ class IMUSensorBroadcasterTest : public ::testing::Test void subscribe_and_get_message(sensor_msgs::msg::Imu & imu_msg, const std::string & ns = ""); }; +struct TestPrefixParams { + std::string tf_prefix; + std::string ns; + std::string result_prefix; +}; + #endif // TEST_IMU_SENSOR_BROADCASTER_HPP_ From 577dd307a7c9884470d69b3a23744d73c3f943d8 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Mon, 22 Apr 2024 11:33:37 +0200 Subject: [PATCH 6/9] Use matrix in tests --- .../src/diff_drive_controller.cpp | 27 +- .../src/diff_drive_controller_parameter.yaml | 5 - .../test/test_diff_drive_controller.cpp | 250 +++--------------- .../test/test_diff_drive_controller.hpp | 7 + .../test/test_imu_sensor_broadcaster.cpp | 8 +- 5 files changed, 63 insertions(+), 234 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 126c21b1c9..245ce3405b 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -394,25 +394,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.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 = params_.tf_frame_prefix; + } + else + { + tf_prefix = std::string(get_node()->get_namespace()); + if (tf_prefix != "/") { - tf_prefix.erase(0, 1); + 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 e878ad5481..c874cab03f 100644 --- a/diff_drive_controller/src/diff_drive_controller_parameter.yaml +++ b/diff_drive_controller/src/diff_drive_controller_parameter.yaml @@ -39,11 +39,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 c9025bd33d..84471c8e30 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 @@ -255,214 +257,46 @@ 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) -{ - const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - std::string odom_id = "odom"; - std::string base_link_id = "base_link"; - std::string frame_prefix = "test_prefix"; - - 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))); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(false))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_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(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) -{ - const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - std::string odom_id = "odom"; - std::string base_link_id = "base_link"; - std::string frame_prefix = "test_prefix"; - - 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))); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_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(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) -{ - const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - std::string odom_id = "odom"; - std::string base_link_id = "base_link"; - std::string frame_prefix = ""; - - 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))); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_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(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"; - - const auto ret = controller_->init(controller_name, test_namespace); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - std::string odom_id = "odom"; - std::string base_link_id = "base_link"; - std::string frame_prefix = "test_prefix"; - - 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))); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(false))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_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(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"; - - const auto ret = controller_->init(controller_name, test_namespace); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - std::string odom_id = "odom"; - std::string base_link_id = "base_link"; - std::string frame_prefix = "test_prefix"; - - 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))); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_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(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 auto ret = controller_->init(controller_name, test_namespace); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - std::string odom_id = "odom"; - std::string base_link_id = "base_link"; - std::string frame_prefix = ""; - - 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))); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_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(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 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); +TEST_F(TestDiffDriveController, 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 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))); + + 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(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; + + 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..fef9fd095a --- /dev/null +++ b/diff_drive_controller/test/test_diff_drive_controller.hpp @@ -0,0 +1,7 @@ +#include + +struct TestPrefixParams { + std::string tf_prefix; + std::string ns; + std::string result_prefix; +}; diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp index bd11d142bf..9ea162ab8b 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp @@ -209,7 +209,7 @@ TEST_F(IMUSensorBroadcasterTest, SensorName_Publish_Success) } } -TEST_F(IMUSensorBroadcasterTest, TfPrefixNamespaceTests) { +TEST_F(IMUSensorBroadcasterTest, TfPrefixNamespaceParams) { const std::vector test_prefix_matrix = { {"", "", ""}, {"/", "", ""}, @@ -221,17 +221,14 @@ TEST_F(IMUSensorBroadcasterTest, TfPrefixNamespaceTests) { {"test_prefix", "test_namespace", "test_prefix"}, }; - int test_number = 0; // Counter for test number for (const auto& params : test_prefix_matrix) { - ++test_number; // Increment the test number - const std::string& tf_prefix = params.tf_prefix; 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", tf_prefix}); + 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); @@ -239,7 +236,6 @@ TEST_F(IMUSensorBroadcasterTest, TfPrefixNamespaceTests) { sensor_msgs::msg::Imu imu_msg; subscribe_and_get_message(imu_msg, test_namespace); - std::cout << "Test number: " << test_number; EXPECT_EQ(imu_msg.header.frame_id, params.result_prefix + frame_id_); } } From 958976d4650037a26211eab8929d7df09b715500 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Mon, 22 Apr 2024 11:37:45 +0200 Subject: [PATCH 7/9] Pre-commit --- .../test/test_diff_drive_controller.cpp | 82 ++++++++++--------- .../test/test_diff_drive_controller.hpp | 9 +- .../test/test_imu_sensor_broadcaster.cpp | 60 +++++++------- .../test/test_imu_sensor_broadcaster.hpp | 10 +-- 4 files changed, 83 insertions(+), 78 deletions(-) diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index 84471c8e30..7749ad9028 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -257,46 +257,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, 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 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))); - - 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(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; - - 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, 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 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))); + + 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(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; + + 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 index fef9fd095a..c4cfe17470 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.hpp +++ b/diff_drive_controller/test/test_diff_drive_controller.hpp @@ -1,7 +1,8 @@ #include -struct TestPrefixParams { - std::string tf_prefix; - std::string ns; - std::string result_prefix; +struct TestPrefixParams +{ + std::string tf_prefix; + std::string ns; + std::string result_prefix; }; diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp index 9ea162ab8b..0add6c86c2 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp @@ -209,35 +209,37 @@ 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_); - } +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) diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp index befa8c1d5a..08ba12ef11 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp @@ -77,15 +77,15 @@ class IMUSensorBroadcasterTest : public ::testing::Test sensor_name_, "linear_acceleration.z", &sensor_values_[9]}; std::unique_ptr imu_broadcaster_; - std::string ns_; void subscribe_and_get_message(sensor_msgs::msg::Imu & imu_msg, const std::string & ns = ""); }; -struct TestPrefixParams { - std::string tf_prefix; - std::string ns; - std::string result_prefix; +struct TestPrefixParams +{ + std::string tf_prefix; + std::string ns; + std::string result_prefix; }; #endif // TEST_IMU_SENSOR_BROADCASTER_HPP_ From 18487bcb0f36c08a22f69e66f64ac7933a61b4bf Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Mon, 22 Apr 2024 11:53:04 +0200 Subject: [PATCH 8/9] Solve param issue --- .../src/imu_sensor_broadcaster_parameters.yaml | 14 ++++++-------- .../test/imu_sensor_broadcaster_params.yaml | 9 +++++---- 2 files changed, 11 insertions(+), 12 deletions(-) diff --git a/imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml b/imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml index 332f7f7dfd..d75882729a 100644 --- a/imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml +++ b/imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml @@ -5,19 +5,17 @@ imu_sensor_broadcaster: description: "Defines sensor name used as prefix for its interfaces. Interface names are: ``/orientation.x, ..., /angular_velocity.x, ..., /linear_acceleration.x.``", - # FIXME: Currently does not work with namespace - # validation: { - # not_empty<>: null - # } + validation: { + not_empty<>: null + } } frame_id: { type: string, default_value: "", description: "Sensor's frame_id in which values are published.", - # FIXME: Currently does not work with namespace - # validation: { - # not_empty<>: null - # } + validation: { + not_empty<>: null + } } static_covariance_orientation: { type: double_array, 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" From 3c9cf6b1aa68aee49602db9c610610491adbd695 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Mon, 22 Apr 2024 14:05:56 +0200 Subject: [PATCH 9/9] Quick fix namespace with sensor --- .../src/imu_sensor_broadcaster.cpp | 18 ++++++- .../imu_sensor_broadcaster_parameters.yaml | 5 ++ .../test/test_imu_sensor_broadcaster.cpp | 50 +++++++++++++++++++ 3 files changed, 72 insertions(+), 1 deletion(-) 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 0add6c86c2..7c653b3bc3 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp @@ -242,6 +242,56 @@ TEST_F(IMUSensorBroadcasterTest, TfPrefixNamespaceParams) } } +// TEST_F(IMUSensorBroadcasterTest, SensorNameNamespaced_Configure_Success) +// { +// const std::string & test_namespace = "test_namespace"; +// 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", true}); + +// 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_namespace + "/" + imu_orientation_x_.get_name()); +// EXPECT_EQ(interface_names[1], test_namespace + "/" + imu_orientation_y_.get_name()); +// EXPECT_EQ(interface_names[2], test_namespace + "/" + imu_orientation_z_.get_name()); +// EXPECT_EQ(interface_names[3], test_namespace + "/" + imu_orientation_w_.get_name()); +// EXPECT_EQ(interface_names[4], test_namespace + "/" + imu_angular_velocity_x_.get_name()); +// EXPECT_EQ(interface_names[5], test_namespace + "/" + imu_angular_velocity_y_.get_name()); +// EXPECT_EQ(interface_names[6], test_namespace + "/" + imu_angular_velocity_z_.get_name()); +// EXPECT_EQ(interface_names[7], test_namespace + "/" + imu_linear_acceleration_x_.get_name()); +// EXPECT_EQ(interface_names[8], test_namespace + "/" + imu_linear_acceleration_y_.get_name()); +// EXPECT_EQ(interface_names[9], test_namespace + "/" + imu_linear_acceleration_z_.get_name()); +// } + +// TEST_F(IMUSensorBroadcasterTest, SensorNameNamespaced_Configure_Fail) +// { +// const std::string & test_namespace = "test_namespace"; +// 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", false}); + +// 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], imu_orientation_x_.get_name()); +// EXPECT_EQ(interface_names[1], imu_orientation_y_.get_name()); +// EXPECT_EQ(interface_names[2], imu_orientation_z_.get_name()); +// EXPECT_EQ(interface_names[3], imu_orientation_w_.get_name()); +// EXPECT_EQ(interface_names[4], imu_angular_velocity_x_.get_name()); +// EXPECT_EQ(interface_names[5], imu_angular_velocity_y_.get_name()); +// EXPECT_EQ(interface_names[6], imu_angular_velocity_z_.get_name()); +// EXPECT_EQ(interface_names[7], imu_linear_acceleration_x_.get_name()); +// EXPECT_EQ(interface_names[8], imu_linear_acceleration_y_.get_name()); +// EXPECT_EQ(interface_names[9], imu_linear_acceleration_z_.get_name()); +// } + int main(int argc, char ** argv) { ::testing::InitGoogleMock(&argc, argv);