diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 4aa2b289b8..374433a1ac 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/tf_prefix.hpp" #include "diff_drive_controller/diff_drive_controller.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "lifecycle_msgs/msg/state.hpp" @@ -410,6 +411,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()); @@ -417,30 +429,15 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( std::make_shared>( odometry_publisher_); - // Append the tf prefix if there is one + // resolve prefix: substitute tilde (~) with the namespace if contains and normalize slashes (/) 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); - } + 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 8a3bd3139d..59a30b54b9 100644 --- a/doc/migration.rst +++ b/doc/migration.rst @@ -4,7 +4,6 @@ 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. - effort_controllers ***************************** * ``effort_controllers/JointGroupEffortController`` is deprecated. Use :ref:`forward_command_controller ` instead by adding the ``interface_name`` parameter and set it to ``effort``. (`#1913 `_). @@ -16,3 +15,8 @@ position_controllers velocity_controllers ***************************** * ``velocity_controllers/JointGroupVelocityController`` is deprecated. Use :ref:`forward_command_controller ` instead by adding the ``interface_name`` parameter and set it to ``velocity``. (`#1913 `_). + +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 091a82d716..fc44975d1b 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -8,3 +8,8 @@ This list summarizes important changes between Kilted Kaiju (previous) and Lyric force_torque_sensor_broadcaster ******************************* * Added support for transforming Wrench messages to a given list of target frames. This is useful when applications need force/torque data in their preferred coordinate frames. (`#2021 `__). + +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 `_).