From 091acebe747ff207fcb613f222f24797c15ea329 Mon Sep 17 00:00:00 2001 From: Ben Holden <94607409+bobbleballs@users.noreply.github.com> Date: Tue, 1 Aug 2023 16:24:34 +0100 Subject: [PATCH 1/4] [DiffDriveController] Optional tf namespace prefixes instead of using node namespace (#533) * [DiffDiveController] Changed the namespace appending from '/' to '_' with optional tf_frame_prefix parameter. If not set it will not append anything. * [DiffDriveController] Add option to enable/disable TF prefix. On by default. If prefix not set and is enabled then namespace is used by default. (cherry picked from commit 5c0327d4e72b164aaa4a365a40d7a3fa913c0f97) # Conflicts: # diff_drive_controller/src/diff_drive_controller.cpp --- .../src/diff_drive_controller.cpp | 28 ++- .../src/diff_drive_controller_parameter.yaml | 10 + .../test/test_diff_drive_controller.cpp | 221 ++++++++++++++++++ 3 files changed, 256 insertions(+), 3 deletions(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 2913838138..d8c98f92d6 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -392,8 +392,20 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( std::make_shared>( odometry_publisher_); - std::string controller_namespace = std::string(get_node()->get_namespace()); + // Append the tf prefix if there is one + std::string tf_prefix = ""; + if (params_.tf_frame_prefix_enable) + { + if (params_.tf_frame_prefix != "") + { + tf_prefix = params_.tf_frame_prefix; + } + else + { + tf_prefix = std::string(get_node()->get_namespace()); + } +<<<<<<< HEAD if (controller_namespace == "/") { controller_namespace = ""; @@ -401,10 +413,20 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( else { controller_namespace = controller_namespace.erase(0, 1) + "/"; +======= + if (tf_prefix == "/") + { + tf_prefix = ""; + } + else + { + tf_prefix = tf_prefix + "/"; + } +>>>>>>> 5c0327d ([DiffDriveController] Optional tf namespace prefixes instead of using node namespace (#533)) } - const auto odom_frame_id = controller_namespace + params_.odom_frame_id; - const auto base_frame_id = controller_namespace + params_.base_frame_id; + const auto odom_frame_id = tf_prefix + params_.odom_frame_id; + const auto base_frame_id = tf_prefix + params_.base_frame_id; auto & odometry_message = realtime_odometry_publisher_->msg_; odometry_message.header.frame_id = odom_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 7541f61d59..e878ad5481 100644 --- a/diff_drive_controller/src/diff_drive_controller_parameter.yaml +++ b/diff_drive_controller/src/diff_drive_controller_parameter.yaml @@ -39,6 +39,16 @@ 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: "", + 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.", + } odom_frame_id: { type: string, default_value: "odom", diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index adf4e79afc..edaaa84e4b 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -68,6 +68,17 @@ class TestableDiffDriveController : public diff_drive_controller::DiffDriveContr } return false; } + + /** + * @brief Used to get the real_time_odometry_publisher to verify its contents + * + * @return Copy of realtime_odometry_publisher_ object + */ + std::shared_ptr> + get_rt_odom_publisher() + { + return realtime_odometry_publisher_; + } }; class TestDiffDriveController : public ::testing::Test @@ -244,6 +255,216 @@ TEST_F(TestDiffDriveController, configure_succeeds_when_wheels_are_specified) SizeIs(left_wheel_names.size() + right_wheel_names.size())); } +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, activate_fails_without_resources_assigned) { const auto ret = controller_->init(controller_name); From 6b114b513b8b934c4db09f24fb59f5a87082bade Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 2 Aug 2023 14:25:40 +0100 Subject: [PATCH 2/4] Feature off by default --- diff_drive_controller/src/diff_drive_controller_parameter.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/diff_drive_controller/src/diff_drive_controller_parameter.yaml b/diff_drive_controller/src/diff_drive_controller_parameter.yaml index e878ad5481..96d8062aa3 100644 --- a/diff_drive_controller/src/diff_drive_controller_parameter.yaml +++ b/diff_drive_controller/src/diff_drive_controller_parameter.yaml @@ -41,7 +41,7 @@ diff_drive_controller: } tf_frame_prefix_enable: { type: bool, - default_value: true, + default_value: false, description: "Enables or disables appending tf_prefix to tf frame id's.", } tf_frame_prefix: { From 5a336879e03e91852fc2b92b7753931a5184ac8d Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 3 Aug 2023 12:09:33 +0100 Subject: [PATCH 3/4] undo the wrong fix --- diff_drive_controller/src/diff_drive_controller_parameter.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/diff_drive_controller/src/diff_drive_controller_parameter.yaml b/diff_drive_controller/src/diff_drive_controller_parameter.yaml index 96d8062aa3..e878ad5481 100644 --- a/diff_drive_controller/src/diff_drive_controller_parameter.yaml +++ b/diff_drive_controller/src/diff_drive_controller_parameter.yaml @@ -41,7 +41,7 @@ diff_drive_controller: } tf_frame_prefix_enable: { type: bool, - default_value: false, + default_value: true, description: "Enables or disables appending tf_prefix to tf frame id's.", } tf_frame_prefix: { From b84fe038fe58c54b089ee1e2bd6d388d72e7c2fd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 5 Dec 2023 22:48:13 +0100 Subject: [PATCH 4/4] Fix merge conflicts Co-authored-by: Ben Holden <94607409+bobbleballs@users.noreply.github.com> --- diff_drive_controller/src/diff_drive_controller.cpp | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index d8c98f92d6..c8032e5dc5 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -405,15 +405,6 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( tf_prefix = std::string(get_node()->get_namespace()); } -<<<<<<< HEAD - if (controller_namespace == "/") - { - controller_namespace = ""; - } - else - { - controller_namespace = controller_namespace.erase(0, 1) + "/"; -======= if (tf_prefix == "/") { tf_prefix = ""; @@ -422,7 +413,6 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( { tf_prefix = tf_prefix + "/"; } ->>>>>>> 5c0327d ([DiffDriveController] Optional tf namespace prefixes instead of using node namespace (#533)) } const auto odom_frame_id = tf_prefix + params_.odom_frame_id;