diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 2913838138..c8032e5dc5 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -392,19 +392,31 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( std::make_shared>( odometry_publisher_); - std::string controller_namespace = std::string(get_node()->get_namespace()); - - if (controller_namespace == "/") - { - controller_namespace = ""; - } - else + // Append the tf prefix if there is one + std::string tf_prefix = ""; + if (params_.tf_frame_prefix_enable) { - controller_namespace = controller_namespace.erase(0, 1) + "/"; + 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 = tf_prefix + "/"; + } } - 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);