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

Fix tf_prefix slashes in diff drive #1145

Closed
wants to merge 6 commits into from
Closed

Fix tf_prefix slashes in diff drive #1145

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
82628ad
Fix tf_prefix slashes in diff drive
rafal-gorecki May 23, 2024
bf724ac
Merge branch 'master' into fix_diff_drive_tf_slashes
rafal-gorecki Jun 13, 2024
e15aff6
Merge branch 'master' into fix_diff_drive_tf_slashes
rafal-gorecki Jul 3, 2024
567f38f
Merge branch 'master' into fix_diff_drive_tf_slashes
bmagyar Jul 16, 2024
435a971
Merge branch 'master' into fix_diff_drive_tf_slashes
rafal-gorecki Aug 1, 2024
e7efdd0
Merge branch 'master' into fix_diff_drive_tf_slashes
bmagyar Aug 14, 2024
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
28 changes: 12 additions & 16 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 @@ -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;
Expand Down
5 changes: 0 additions & 5 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 @@ -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: "",
Expand Down
192 changes: 37 additions & 155 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 @@ -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 <gmock/gmock.h>

#include <memory>
Expand Down Expand Up @@ -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<PrefixTestCase> 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<rclcpp::Parameter> 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)
Expand Down
Loading