Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

[DiffDriveController] Optional tf namespace prefixes instead of using node namespace (backport #533) #726

Merged
merged 4 commits into from
Dec 6, 2023
Merged

[DiffDriveController] Optional tf namespace prefixes instead of using node namespace (backport #533) #726

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
091aceb
[DiffDriveController] Optional tf namespace prefixes instead of using…
bobbleballs Aug 1, 2023
6b114b5
Feature off by default
bmagyar Aug 2, 2023
5a33687
undo the wrong fix
bmagyar Aug 3, 2023
b84fe03
Fix merge conflicts
christophfroehlich Dec 5, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
32 changes: 22 additions & 10 deletions diff_drive_controller/src/diff_drive_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -392,19 +392,31 @@ controller_interface::CallbackReturn DiffDriveController::on_configure(
std::make_shared<realtime_tools::RealtimePublisher<nav_msgs::msg::Odometry>>(
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;
Expand Down
10 changes: 10 additions & 0 deletions diff_drive_controller/src/diff_drive_controller_parameter.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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,
bmagyar marked this conversation as resolved.
Show resolved Hide resolved
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",
Expand Down
221 changes: 221 additions & 0 deletions diff_drive_controller/test/test_diff_drive_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<realtime_tools::RealtimePublisher<nav_msgs::msg::Odometry>>
get_rt_odom_publisher()
{
return realtime_odometry_publisher_;
}
};

class TestDiffDriveController : public ::testing::Test
Expand Down Expand Up @@ -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);
Expand Down
Loading