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

[Humble Backport] tf_prefix param: fix slashes and add to IMU Broadcaster #1102

Closed
wants to merge 9 commits into from
Closed

[Humble Backport] tf_prefix param: fix slashes and add to IMU Broadcaster #1102

Show file tree
Hide file tree
Changes from 2 commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
79ead8b
tf_prefix: fix slashes and add to IMU
rafal-gorecki Apr 20, 2024
38745aa
Formatting
rafal-gorecki Apr 20, 2024
2c434b1
Remove front slash suggestions
rafal-gorecki Apr 22, 2024
21eb1b5
Update imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml
rafal-gorecki Apr 22, 2024
765d8f6
Add matrix tests
rafal-gorecki Apr 22, 2024
577dd30
Use matrix in tests
rafal-gorecki Apr 22, 2024
958976d
Pre-commit
rafal-gorecki Apr 22, 2024
18487bc
Solve param issue
rafal-gorecki Apr 22, 2024
3c9cf6b
Quick fix namespace with sensor
rafal-gorecki Apr 22, 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
16 changes: 6 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 @@ -396,22 +396,18 @@ controller_interface::CallbackReturn DiffDriveController::on_configure(
std::string tf_prefix = "";
if (params_.tf_frame_prefix_enable)
{
if (params_.tf_frame_prefix != "")
if (!params_.tf_frame_prefix.empty())
{
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 + "/";
tf_prefix.erase(0, 1);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why do you always want to erase the first character? If the prefix is parsed without '/' then it removes the wrong character right?

I see that you did that when you are using node namespace, however, I think you need to correct the users tf_prefix as well, if he adds a '/' as well. Try to do same logic as done in #1060

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Makes sense

if (!tf_prefix.empty())
{
tf_prefix = tf_prefix + '/';
}
}
}

Expand Down
10 changes: 5 additions & 5 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 @@ -319,8 +319,8 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_no_namesp

/* 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);
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)
Expand Down Expand Up @@ -425,13 +425,13 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_set_names

/* 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);
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";
std::string test_namespace = "test_namespace";
rafal-gorecki marked this conversation as resolved.
Show resolved Hide resolved

const auto ret = controller_->init(controller_name, test_namespace);
ASSERT_EQ(ret, controller_interface::return_type::OK);
Expand Down
20 changes: 19 additions & 1 deletion imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -62,8 +62,26 @@ controller_interface::CallbackReturn IMUSensorBroadcaster::on_configure(
return CallbackReturn::ERROR;
}

std::string tf_prefix = "";
if (params_.tf_frame_prefix_enable)
{
if (!params_.tf_frame_prefix.empty())
{
tf_prefix = params_.tf_frame_prefix;
}
else
{
tf_prefix = std::string(get_node()->get_namespace());
tf_prefix.erase(0, 1);
if (!tf_prefix.empty())
{
tf_prefix = tf_prefix + '/';
}
}
}

realtime_publisher_->lock();
realtime_publisher_->msg_.header.frame_id = params_.frame_id;
realtime_publisher_->msg_.header.frame_id = tf_prefix + params_.frame_id;
// convert double vector to fixed-size array in the message
for (size_t i = 0; i < 9; ++i)
{
Expand Down
24 changes: 18 additions & 6 deletions imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,17 +5,19 @@ imu_sensor_broadcaster:
description: "Defines sensor name used as prefix for its interfaces.
Interface names are: ``<sensor_name>/orientation.x, ..., <sensor_name>/angular_velocity.x, ...,
<sensor_name>/linear_acceleration.x.``",
validation: {
not_empty<>: null
}
# FIXME: Currently does not work with namespace
# validation: {
# not_empty<>: null
# }
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Can you explain a bit more in what situation it doesn't work.

Copy link
Contributor Author

@rafal-gorecki rafal-gorecki Apr 22, 2024 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yes sure:

I don't know exactly what's going on. However, when this is checked it then gets an error:

    [ERROR] [1713772814.609746661] [test_namespace.test_imu_sensor_broadcaster]: Exception thrown during init stage with message: Invalid value set during initialization for parameter 'sensor_name': Parameter 'sensor_name' cannot be empty

The problem is probably that:

  1. Adding a namespace causes the frame_id parameter to be searched for, which is no longer there (because it is ~/frame_id)
  2. Something else with ControllerInterface

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think you have misinterpreted it. This information is need to get the proper state interfaces within the controller. Please look into the following code

ros2_controllers/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp

Lines 48 to 49 in 9e97c00

imu_sensor_ = std::make_unique<semantic_components::IMUSensor>(
semantic_components::IMUSensor(params_.sensor_name));

}
frame_id: {
type: string,
default_value: "",
description: "Sensor's frame_id in which values are published.",
validation: {
not_empty<>: null
}
# FIXME: Currently does not work with namespace
# validation: {
# not_empty<>: null
# }
}
static_covariance_orientation: {
type: double_array,
Expand All @@ -41,3 +43,13 @@ imu_sensor_broadcaster:
fixed_size<>: [9],
}
}
tf_frame_prefix_enable: {
type: bool,
default_value: true,
description: "Enables or disables appending tf_prefix to tf frame id's.",
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I don't understand the need for this new parameter. Can you explain me why it is needed ?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I followed the example of diff_drive. I think you can get rid of it.

}
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.",
rafal-gorecki marked this conversation as resolved.
Show resolved Hide resolved
}
159 changes: 154 additions & 5 deletions imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -54,9 +54,9 @@ void IMUSensorBroadcasterTest::SetUp()

void IMUSensorBroadcasterTest::TearDown() { imu_broadcaster_.reset(nullptr); }

void IMUSensorBroadcasterTest::SetUpIMUBroadcaster()
void IMUSensorBroadcasterTest::SetUpIMUBroadcaster(const std::string & ns)
{
const auto result = imu_broadcaster_->init("test_imu_sensor_broadcaster");
const auto result = imu_broadcaster_->init("test_imu_sensor_broadcaster", ns);
ASSERT_EQ(result, controller_interface::return_type::OK);

std::vector<LoanedStateInterface> state_ifs;
Expand All @@ -74,13 +74,14 @@ void IMUSensorBroadcasterTest::SetUpIMUBroadcaster()
imu_broadcaster_->assign_interfaces({}, std::move(state_ifs));
}

void IMUSensorBroadcasterTest::subscribe_and_get_message(sensor_msgs::msg::Imu & imu_msg)
void IMUSensorBroadcasterTest::subscribe_and_get_message(
sensor_msgs::msg::Imu & imu_msg, const std::string & ns)
{
// create a new subscriber
rclcpp::Node test_subscription_node("test_subscription_node");
rclcpp::Node test_subscription_node("test_subscription_node", ns);
auto subs_callback = [&](const sensor_msgs::msg::Imu::SharedPtr) {};
auto subscription = test_subscription_node.create_subscription<sensor_msgs::msg::Imu>(
"/test_imu_sensor_broadcaster/imu", 10, subs_callback);
"test_imu_sensor_broadcaster/imu", 10, subs_callback);

// call update to publish the test value
// since update doesn't guarantee a published message, republish until received
Expand Down Expand Up @@ -208,6 +209,154 @@ TEST_F(IMUSensorBroadcasterTest, SensorName_Publish_Success)
}
}

TEST_F(IMUSensorBroadcasterTest, NoPrefixNoNamespace)
{
SetUpIMUBroadcaster();

std::string frame_prefix = "test_prefix";

// set the params 'sensor_name' and 'frame_id'
imu_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_});
imu_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_});

imu_broadcaster_->get_node()->set_parameter({"tf_frame_prefix_enable", false});
imu_broadcaster_->get_node()->set_parameter({"tf_frame_prefix", frame_prefix});

ASSERT_EQ(imu_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
ASSERT_EQ(imu_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);

sensor_msgs::msg::Imu imu_msg;
subscribe_and_get_message(imu_msg);

EXPECT_EQ(imu_msg.header.frame_id, frame_id_);
}

TEST_F(IMUSensorBroadcasterTest, PrefixNoNamespace)
{
SetUpIMUBroadcaster();

std::string frame_prefix = "test_prefix";

// set the params 'sensor_name' and 'frame_id'
imu_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_});
imu_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_});

imu_broadcaster_->get_node()->set_parameter({"tf_frame_prefix_enable", true});
imu_broadcaster_->get_node()->set_parameter({"tf_frame_prefix", frame_prefix});

ASSERT_EQ(imu_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
ASSERT_EQ(imu_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);

sensor_msgs::msg::Imu imu_msg;
subscribe_and_get_message(imu_msg);

EXPECT_EQ(imu_msg.header.frame_id, frame_prefix + frame_id_);
}

TEST_F(IMUSensorBroadcasterTest, BlankPrefixNoNamespace)
{
SetUpIMUBroadcaster();

std::string frame_prefix = "";

// set the params 'sensor_name' and 'frame_id'
imu_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_});
imu_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_});

imu_broadcaster_->get_node()->set_parameter({"tf_frame_prefix_enable", true});
imu_broadcaster_->get_node()->set_parameter({"tf_frame_prefix", frame_prefix});

ASSERT_EQ(imu_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
ASSERT_EQ(imu_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);

sensor_msgs::msg::Imu imu_msg;
subscribe_and_get_message(imu_msg);

EXPECT_EQ(imu_msg.header.frame_id, frame_id_);
}

TEST_F(IMUSensorBroadcasterTest, NoPrefixWithNamespace)
{
std::string test_namespace = "test_namespace";

SetUpIMUBroadcaster(test_namespace);

std::string frame_prefix = "test_prefix";

// set the params 'sensor_name' and 'frame_id'
imu_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_});
imu_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_});

imu_broadcaster_->get_node()->set_parameter({"tf_frame_prefix_enable", false});
imu_broadcaster_->get_node()->set_parameter({"tf_frame_prefix", frame_prefix});

ASSERT_EQ(imu_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
ASSERT_EQ(imu_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);

sensor_msgs::msg::Imu imu_msg;
subscribe_and_get_message(imu_msg, test_namespace);

EXPECT_EQ(imu_msg.header.frame_id, frame_id_);
}

TEST_F(IMUSensorBroadcasterTest, PrefixWithNamespace)
{
std::string test_namespace = "test_namespace";

SetUpIMUBroadcaster(test_namespace);

std::string frame_prefix = "test_prefix";

// set the params 'sensor_name' and 'frame_id'
imu_broadcaster_->get_node()->set_parameter(
rclcpp::Parameter("sensor_name", rclcpp::ParameterValue(sensor_name_)));
imu_broadcaster_->get_node()->set_parameter(
rclcpp::Parameter("sensor_name", rclcpp::ParameterValue(sensor_name_)));
imu_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_});

imu_broadcaster_->get_node()->set_parameter(
rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true)));
imu_broadcaster_->get_node()->set_parameter(
rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)));

ASSERT_EQ(imu_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
ASSERT_EQ(imu_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);

sensor_msgs::msg::Imu imu_msg;
subscribe_and_get_message(imu_msg, test_namespace);

EXPECT_EQ(imu_msg.header.frame_id, frame_prefix + frame_id_);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Shouldn't there be a slash in between?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

A matter for discussion, but in my opinion the prefix can be arbitrary and the user does not necessarily have to want it to end slash

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

ok if you want it to be prefixed without a '/' at the end, why don't you parse it directly already appending it to the frame_id? This frame_id is only used for the published message. I don't see a point of adding a tf_prefix, if it is not namespaced.

ros2_controllers/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp

Line 66 in 9e97c00

realtime_publisher_->msg_.header.frame_id = params_.frame_id;

What do you think about this?

}

TEST_F(IMUSensorBroadcasterTest, BlankPrefixWithNamespace)
{
std::string test_namespace = "test_namespace";

SetUpIMUBroadcaster(test_namespace);

std::string frame_prefix = "";

// set the params 'sensor_name' and 'frame_id'
imu_broadcaster_->get_node()->set_parameter(
rclcpp::Parameter("sensor_name", rclcpp::ParameterValue(sensor_name_)));
imu_broadcaster_->get_node()->set_parameter(
rclcpp::Parameter("sensor_name", rclcpp::ParameterValue(sensor_name_)));
imu_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_});

imu_broadcaster_->get_node()->set_parameter(
rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true)));
imu_broadcaster_->get_node()->set_parameter(
rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)));

ASSERT_EQ(imu_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
ASSERT_EQ(imu_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);

sensor_msgs::msg::Imu imu_msg;
subscribe_and_get_message(imu_msg, test_namespace);

EXPECT_EQ(imu_msg.header.frame_id, test_namespace + "/" + frame_id_);
}

int main(int argc, char ** argv)
{
::testing::InitGoogleMock(&argc, argv);
Expand Down
5 changes: 3 additions & 2 deletions imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,7 @@ class IMUSensorBroadcasterTest : public ::testing::Test
void SetUp();
void TearDown();

void SetUpIMUBroadcaster();
void SetUpIMUBroadcaster(const std::string & ns = "");

protected:
const std::string sensor_name_ = "imu_sensor";
Expand Down Expand Up @@ -77,8 +77,9 @@ class IMUSensorBroadcasterTest : public ::testing::Test
sensor_name_, "linear_acceleration.z", &sensor_values_[9]};

std::unique_ptr<FriendIMUSensorBroadcaster> imu_broadcaster_;
std::string ns_;

void subscribe_and_get_message(sensor_msgs::msg::Imu & imu_msg);
void subscribe_and_get_message(sensor_msgs::msg::Imu & imu_msg, const std::string & ns = "");
};

#endif // TEST_IMU_SENSOR_BROADCASTER_HPP_
Loading