Skip to content

Commit

Permalink
Merge branch 'master' into feature/add-range-sensor-broadcast
Browse files Browse the repository at this point in the history
  • Loading branch information
@flochre
flochre authored Aug 2, 2023
2 parents 814c944 + 5c0327d commit 1281b48
Show file tree
Hide file tree
Showing 3 changed files with 255 additions and 12 deletions.
36 changes: 24 additions & 12 deletions diff_drive_controller/src/diff_drive_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -392,23 +392,35 @@ 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 + "/";
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 = controller_namespace + odom_frame_id;
odometry_message.child_frame_id = controller_namespace + base_frame_id;
odometry_message.header.frame_id = odom_frame_id;
odometry_message.child_frame_id = base_frame_id;

// limit the publication on the topics /odom and /tf
publish_rate_ = params_.publish_rate;
Expand Down
10 changes: 10 additions & 0 deletions diff_drive_controller/src/diff_drive_controller_parameter.yaml
View file
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,
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
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

0 comments on commit 1281b48

Please sign in to comment.