diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index a64f210503..d007925862 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -361,26 +361,22 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( // Append the tf prefix if there is one std::string tf_prefix = ""; - if (params_.tf_frame_prefix_enable) + if (!params_.tf_frame_prefix.empty()) { - if (params_.tf_frame_prefix != "") - { - tf_prefix = params_.tf_frame_prefix; - } - else - { - tf_prefix = std::string(get_node()->get_namespace()); - } - - if (tf_prefix == "/") - { - tf_prefix = ""; - } - else + tf_prefix = params_.tf_frame_prefix; + } + else + { + tf_prefix = std::string(get_node()->get_namespace()); + if (tf_prefix != "/") { - tf_prefix = tf_prefix + "/"; + tf_prefix += '/'; } } + if (tf_prefix.front() == '/') + { + tf_prefix.erase(0, 1); + } const auto odom_frame_id = tf_prefix + params_.odom_frame_id; const auto base_frame_id = tf_prefix + params_.base_frame_id; diff --git a/diff_drive_controller/src/diff_drive_controller_parameter.yaml b/diff_drive_controller/src/diff_drive_controller_parameter.yaml index 755259cc2a..a1739901b3 100644 --- a/diff_drive_controller/src/diff_drive_controller_parameter.yaml +++ b/diff_drive_controller/src/diff_drive_controller_parameter.yaml @@ -46,11 +46,6 @@ diff_drive_controller: default_value: 1.0, description: "Correction factor when radius of right wheels differs from the nominal value in ``wheel_radius`` parameter.", } - tf_frame_prefix_enable: { - type: bool, - default_value: true, - description: "Enables or disables appending tf_prefix to tf frame id's.", - } tf_frame_prefix: { type: string, default_value: "", diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index 72ae4dbfd7..4511dea235 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 @@ -252,169 +254,49 @@ TEST_F(TestDiffDriveController, configure_succeeds_when_wheels_are_specified) EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); } -TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_no_namespace) -{ - std::string odom_id = "odom"; - std::string base_link_id = "base_link"; - std::string frame_prefix = "test_prefix"; - - ASSERT_EQ( - InitController( - left_wheel_names, right_wheel_names, - {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(false)), - rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), - rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), - rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}), - controller_interface::return_type::OK); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - - auto odometry_message = controller_->get_rt_odom_publisher()->msg_; - std::string test_odom_frame_id = odometry_message.header.frame_id; - std::string test_base_frame_id = odometry_message.child_frame_id; - /* tf_frame_prefix_enable is false so no modifications to the frame id's */ - ASSERT_EQ(test_odom_frame_id, odom_id); - ASSERT_EQ(test_base_frame_id, base_link_id); -} - -TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_no_namespace) +TEST_F(TestDiffDriveController, TfPrefixNamespaceParams) { - std::string odom_id = "odom"; - std::string base_link_id = "base_link"; - std::string frame_prefix = "test_prefix"; - - ASSERT_EQ( - InitController( - left_wheel_names, right_wheel_names, - {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true)), - rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), - rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), - rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}), - controller_interface::return_type::OK); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - - auto odometry_message = controller_->get_rt_odom_publisher()->msg_; - std::string test_odom_frame_id = odometry_message.header.frame_id; - std::string test_base_frame_id = odometry_message.child_frame_id; - - /* tf_frame_prefix_enable is true and frame_prefix is not blank so should be appended to the frame - * id's */ - ASSERT_EQ(test_odom_frame_id, frame_prefix + "/" + odom_id); - ASSERT_EQ(test_base_frame_id, frame_prefix + "/" + base_link_id); -} - -TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_no_namespace) -{ - std::string odom_id = "odom"; - std::string base_link_id = "base_link"; - std::string frame_prefix = ""; - - ASSERT_EQ( - InitController( - left_wheel_names, right_wheel_names, - {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true)), - rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), - rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), - rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}), - controller_interface::return_type::OK); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - - auto odometry_message = controller_->get_rt_odom_publisher()->msg_; - std::string test_odom_frame_id = odometry_message.header.frame_id; - std::string test_base_frame_id = odometry_message.child_frame_id; - /* tf_frame_prefix_enable is true but frame_prefix is blank so should not be appended to the frame - * id's */ - ASSERT_EQ(test_odom_frame_id, odom_id); - ASSERT_EQ(test_base_frame_id, base_link_id); -} - -TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_set_namespace) -{ - std::string test_namespace = "/test_namespace"; - - std::string odom_id = "odom"; - std::string base_link_id = "base_link"; - std::string frame_prefix = "test_prefix"; - - ASSERT_EQ( - InitController( - left_wheel_names, right_wheel_names, - {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(false)), - rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), - rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), - rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}, - test_namespace), - controller_interface::return_type::OK); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - - auto odometry_message = controller_->get_rt_odom_publisher()->msg_; - std::string test_odom_frame_id = odometry_message.header.frame_id; - std::string test_base_frame_id = odometry_message.child_frame_id; - /* tf_frame_prefix_enable is false so no modifications to the frame id's */ - ASSERT_EQ(test_odom_frame_id, odom_id); - ASSERT_EQ(test_base_frame_id, base_link_id); -} - -TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_set_namespace) -{ - std::string test_namespace = "/test_namespace"; + struct PrefixTestCase + { + std::string tf_prefix; + std::string ns; + std::string result_prefix; + }; std::string odom_id = "odom"; std::string base_link_id = "base_link"; - std::string frame_prefix = "test_prefix"; - - ASSERT_EQ( - InitController( - left_wheel_names, right_wheel_names, - {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true)), - rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), - rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), - rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}, - test_namespace), - controller_interface::return_type::OK); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - - auto odometry_message = controller_->get_rt_odom_publisher()->msg_; - std::string test_odom_frame_id = odometry_message.header.frame_id; - std::string test_base_frame_id = odometry_message.child_frame_id; - - /* tf_frame_prefix_enable is true and frame_prefix is not blank so should be appended to the frame - * id's instead of the namespace*/ - ASSERT_EQ(test_odom_frame_id, frame_prefix + "/" + odom_id); - ASSERT_EQ(test_base_frame_id, frame_prefix + "/" + base_link_id); -} - -TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_set_namespace) -{ - std::string test_namespace = "/test_namespace"; + const std::vector test_cases = { + {"", "", ""}, + {"/", "", ""}, + {"", "/", ""}, + {"test_prefix", "", "test_prefix"}, + {"/test_prefix", "", "test_prefix"}, + {"", "test_namespace", "test_namespace/"}, + {"", "/test_namespace", "test_namespace/"}, + {"test_prefix", "test_namespace", "test_prefix"}, + }; + + for (const auto & test_case : test_cases) + { + std::vector params = { + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(test_case.tf_prefix)), + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id)), + }; - std::string odom_id = "odom"; - std::string base_link_id = "base_link"; - std::string frame_prefix = ""; + ASSERT_EQ( + InitController(left_wheel_names, right_wheel_names, params, test_case.ns), + controller_interface::return_type::OK); - ASSERT_EQ( - InitController( - left_wheel_names, right_wheel_names, - {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true)), - rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), - rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), - rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}, - test_namespace), - controller_interface::return_type::OK); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + auto odometry_message = controller_->get_rt_odom_publisher()->msg_; + std::string test_odom_frame_id = odometry_message.header.frame_id; + std::string test_base_frame_id = odometry_message.child_frame_id; - auto odometry_message = controller_->get_rt_odom_publisher()->msg_; - std::string test_odom_frame_id = odometry_message.header.frame_id; - std::string test_base_frame_id = odometry_message.child_frame_id; - /* tf_frame_prefix_enable is true but frame_prefix is blank so namespace should be appended to the - * frame id's */ - ASSERT_EQ(test_odom_frame_id, test_namespace + "/" + odom_id); - ASSERT_EQ(test_base_frame_id, test_namespace + "/" + base_link_id); + ASSERT_EQ(test_odom_frame_id, test_case.result_prefix + odom_id); + ASSERT_EQ(test_base_frame_id, test_case.result_prefix + base_link_id); + } } TEST_F(TestDiffDriveController, activate_fails_without_resources_assigned)