diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 4057121895..cd0293d863 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -22,6 +22,7 @@ #include #include +#include "controller_interface/helpers.hpp" #include "diff_drive_controller/diff_drive_controller.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "lifecycle_msgs/msg/state.hpp" @@ -404,6 +405,17 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( } }); + // deprecation warning if tf_frame_prefix_enable set to non-default value + const bool default_tf_frame_prefix_enable = true; + if (params_.tf_frame_prefix_enable != default_tf_frame_prefix_enable) + { + RCLCPP_WARN( + get_node()->get_logger(), + "Parameter 'tf_frame_prefix_enable' is DEPRECATED and set to a non-default value (%s). " + "Please migrate to 'tf_frame_prefix'.", + params_.tf_frame_prefix_enable ? "true" : "false"); + } + // initialize odometry publisher and message odometry_publisher_ = get_node()->create_publisher( DEFAULT_ODOMETRY_TOPIC, rclcpp::SystemDefaultsQoS()); @@ -411,30 +423,11 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( std::make_shared>( odometry_publisher_); - // 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()); - } - - // Make sure prefix does not start with '/' and always ends with '/' - if (tf_prefix.back() != '/') - { - tf_prefix = tf_prefix + "/"; - } - if (tf_prefix.front() == '/') - { - tf_prefix.erase(0, 1); - } - } + // resolve prefix: substitute tilde (~) with the namespace if contains and normalize slashes (/) + const auto tf_prefix = + controller_interface::resolve_tf_prefix(params_.tf_frame_prefix, get_node()->get_namespace()); + // prepend resolved TF prefix to frame ids 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 1fc8b66e88..81e8bc645b 100644 --- a/diff_drive_controller/src/diff_drive_controller_parameter.yaml +++ b/diff_drive_controller/src/diff_drive_controller_parameter.yaml @@ -49,7 +49,7 @@ diff_drive_controller: tf_frame_prefix_enable: { type: bool, default_value: true, - description: "Enables or disables appending tf_prefix to tf frame id's.", + description: "Deprecated: this parameter will be removed in a future release. Use 'tf_frame_prefix' instead.", } tf_frame_prefix: { type: string, diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index 49a7493e42..6d5d86487a 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -74,12 +74,10 @@ class TestableDiffDriveController : public diff_drive_controller::DiffDriveContr return realtime_odometry_publisher_; } // Declare these tests as friends so we can access odometry_message_ - FRIEND_TEST(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_no_namespace); - FRIEND_TEST(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_no_namespace); - FRIEND_TEST(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_no_namespace); - FRIEND_TEST(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_set_namespace); - FRIEND_TEST(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_set_namespace); - FRIEND_TEST(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_set_namespace); + FRIEND_TEST(TestDiffDriveController, configure_succeeds_tf_prefix_no_namespace); + FRIEND_TEST(TestDiffDriveController, configure_succeeds_tf_blank_prefix_no_namespace); + FRIEND_TEST(TestDiffDriveController, configure_succeeds_tf_prefix_set_namespace); + FRIEND_TEST(TestDiffDriveController, configure_succeeds_tf_tilde_prefix_set_namespace); // Declare these tests as friends so we can access controller_->reference_interfaces_ FRIEND_TEST(TestDiffDriveController, chainable_controller_unchained_mode); FRIEND_TEST(TestDiffDriveController, chainable_controller_chained_mode); @@ -310,29 +308,7 @@ TEST_F( 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); - - /* tf_frame_prefix_enable is false so no modifications to the frame id's */ - ASSERT_EQ(controller_->odometry_message_.header.frame_id, odom_id); - ASSERT_EQ(controller_->odometry_message_.child_frame_id, base_link_id); -} - -TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_no_namespace) +TEST_F(TestDiffDriveController, configure_succeeds_tf_prefix_no_namespace) { std::string odom_id = "odom"; std::string base_link_id = "base_link"; @@ -349,13 +325,12 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_no_namesp ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - /* tf_frame_prefix_enable is true and frame_prefix is not blank so should be appended to the frame - * id's */ + // frame_prefix is not blank so should be prepended to the frame id's ASSERT_EQ(controller_->odometry_message_.header.frame_id, frame_prefix + "/" + odom_id); ASSERT_EQ(controller_->odometry_message_.child_frame_id, frame_prefix + "/" + base_link_id); } -TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_no_namespace) +TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_no_namespace) { std::string odom_id = "odom"; std::string base_link_id = "base_link"; @@ -372,38 +347,12 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_no_names ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - /* tf_frame_prefix_enable is true but frame_prefix is blank so should not be appended to the frame - * id's */ - ASSERT_EQ(controller_->odometry_message_.header.frame_id, odom_id); - ASSERT_EQ(controller_->odometry_message_.child_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); - - /* tf_frame_prefix_enable is false so no modifications to the frame id's */ + // frame_prefix is blank so nothing added to the frame id's ASSERT_EQ(controller_->odometry_message_.header.frame_id, odom_id); ASSERT_EQ(controller_->odometry_message_.child_frame_id, base_link_id); } -TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_set_namespace) +TEST_F(TestDiffDriveController, configure_succeeds_tf_prefix_set_namespace) { std::string test_namespace = "/test_namespace"; @@ -423,18 +372,17 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_set_names ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - /* 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*/ + // frame_prefix is not blank so should be prepended to the frame id's instead of the namespace ASSERT_EQ(controller_->odometry_message_.header.frame_id, frame_prefix + "/" + odom_id); ASSERT_EQ(controller_->odometry_message_.child_frame_id, frame_prefix + "/" + base_link_id); } -TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_set_namespace) +TEST_F(TestDiffDriveController, configure_succeeds_tf_tilde_prefix_set_namespace) { std::string test_namespace = "/test_namespace"; std::string odom_id = "odom"; std::string base_link_id = "base_link"; - std::string frame_prefix = ""; + std::string frame_prefix = "~"; ASSERT_EQ( InitController( @@ -448,9 +396,8 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_set_name ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + // frame_prefix has tilde (~) character so node namespace should be prepended to the frame id's std::string ns_prefix = test_namespace.erase(0, 1) + "/"; - /* tf_frame_prefix_enable is true but frame_prefix is blank so namespace should be appended to the - * frame id's */ ASSERT_EQ(controller_->odometry_message_.header.frame_id, ns_prefix + odom_id); ASSERT_EQ(controller_->odometry_message_.child_frame_id, ns_prefix + base_link_id); } diff --git a/doc/migration.rst b/doc/migration.rst index 6b0875f73d..391e96859a 100644 --- a/doc/migration.rst +++ b/doc/migration.rst @@ -4,3 +4,8 @@ Migration Guides: Kilted Kaiju to Lyrical Luth ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ This list summarizes important changes between Kilted Kaiju (previous) and Lyrical Luth (current) releases, where changes to user code might be necessary. + +diff_drive_controller +***************************** +* Instead of using ``tf_frame_prefix_enable:=false``, set an empty ``tf_frame_prefix:=""`` parameter instead. (`#1997 `_). +* For using node namespace as tf prefix: Set ``tf_frame_prefix:="~"``, where the ("~") character is substituted with node namespace. (`#1997 `_). diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 8622620620..1272709927 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -4,3 +4,8 @@ Release Notes: Kilted Kaiju to Lyrical Luth ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ This list summarizes important changes between Kilted Kaiju (previous) and Lyrical Luth (current) releases. + +diff_drive_controller +***************************** +* Parameter ``tf_frame_prefix_enable`` got deprecated and will be removed in a future release (`#1997 `_). +* Now any tilde ("~") character in ``tf_frame_prefix`` is substituted with node namespace. (`#1997 `_). diff --git a/ros2_controllers.rolling.repos b/ros2_controllers.rolling.repos index 067271620f..c2342b18d2 100644 --- a/ros2_controllers.rolling.repos +++ b/ros2_controllers.rolling.repos @@ -1,8 +1,8 @@ repositories: ros2_control: type: git - url: https://github.com/ros-controls/ros2_control.git - version: master + url: https://github.com/kuralme/ros2_control.git + version: tf_prefix_helper realtime_tools: type: git url: https://github.com/ros-controls/realtime_tools.git