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

tf_prefix param: add to IMU Broadcaster and fix slashes in DiffDrive #1104

Open
wants to merge 12 commits into
base: master
Choose a base branch
from
+202 −189
Open

tf_prefix param: add to IMU Broadcaster and fix slashes in DiffDrive #1104

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
12 commits
Select commit Hold shift + click to select a range
2412651
tf_prefix param: fix slashes and add to IMU Broadcaster
rafal-gorecki Apr 22, 2024
a38ed95
Add namespace
rafal-gorecki Apr 26, 2024
e4aa06e
Use InitController
rafal-gorecki Apr 26, 2024
df173d8
Rename variable
rafal-gorecki Apr 26, 2024
bd24d4c
typo
rafal-gorecki Apr 26, 2024
92679fc
pre-commit
rafal-gorecki Apr 26, 2024
52104aa
Fix imu
rafal-gorecki Apr 26, 2024
1292fad
fix imu namespace
rafal-gorecki Apr 26, 2024
2f0ce56
Merge branch 'master' into master
rafal-gorecki Apr 29, 2024
aabcf74
Merge branch 'master' into master
rafal-gorecki May 13, 2024
70cfc29
Add sensor_name prefix
rafal-gorecki May 14, 2024
119144a
Merge branch 'master' into master
rafal-gorecki Aug 1, 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
187 changes: 31 additions & 156 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,42 @@ 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";

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
22 changes: 22 additions & 0 deletions diff_drive_controller/test/test_diff_drive_controller.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,22 @@
// Copyright 2020 PAL Robotics SL.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <string>

struct PrefixTestCase
{
std::string tf_prefix;
std::string ns;
std::string result_prefix;
};
38 changes: 36 additions & 2 deletions 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 @@ -45,8 +45,24 @@ controller_interface::CallbackReturn IMUSensorBroadcaster::on_configure(
{
params_ = param_listener_->get_params();

std::string namespace_for_sensor_name = "";
if (params_.use_namespace_as_sensor_name_prefix)
{
namespace_for_sensor_name = std::string(get_node()->get_namespace());
;
namespace_for_sensor_name.erase(0, 1);

if (*std::end(namespace_for_sensor_name) != '/' and namespace_for_sensor_name.size())
{
namespace_for_sensor_name = namespace_for_sensor_name + "/";
}
}

RCLCPP_INFO_STREAM(
get_node()->get_logger(), "Sensor name: " << namespace_for_sensor_name + params_.sensor_name);

imu_sensor_ = std::make_unique<semantic_components::IMUSensor>(
semantic_components::IMUSensor(params_.sensor_name));
semantic_components::IMUSensor(namespace_for_sensor_name + params_.sensor_name));
try
{
// register ft sensor data publisher
Expand All @@ -62,8 +78,26 @@ controller_interface::CallbackReturn IMUSensorBroadcaster::on_configure(
return CallbackReturn::ERROR;
}

std::string tf_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 += '/';
}
}
if (tf_prefix.front() == '/')
{
tf_prefix.erase(0, 1);
}

Comment on lines +81 to +98
Copy link
Member

@saikishor saikishor Apr 27, 2024 •
edited
Loading

Choose a reason for hiding this comment

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

Hello!

While reviewing this part again, I started to question myself, if we need this or not. I'm saying because in case of the diff_drive_controller, the odom message that is being published is a dynamic tf frame, but in case of the imu_state_broadcaster, as per the imu you always have a static transform link inside the URDF, I don't know if it make sense to have the tf_prefix in this case.

Right now, your changes will add namespace of the controller manager to the links, I'm not sure how right is it.

Does it make sense?. Should we check if the link exists from the loaded URDF?

Copy link
Contributor Author

Choose a reason for hiding this comment

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

Hi @saikishor,

Thank you, for reply!

I'm not sure what difference it makes whether fame is dynamic or static in the case of tf_prefix. I treat it as an option that may be useful to someone. What I care about most is the ability to correctly assign a frame to a robot when many robots are launched.

I'm still wondering about the issue of sensor_name, because while in the case of a real robot there should be no problems, in the simulation you need to create sensor objects with different namespaces.

Copy link
Member

Choose a reason for hiding this comment

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

Hello!

I think it is rather very important, because the sensor readings should correspond to a specific frame on the robot and this might be important. Regarding multi robot i agree that we would need this separation, then we can use the namespace, but why tf_prefix?. Moreover, i believe we have to make sure that it is a valid link.

Can you explain to me the instances that's a requirement for you?

Copy link
Contributor Author

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

Choose a reason for hiding this comment

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

If you are asking specifically about tf_prefix, I don't need it, but I added it to unify arg in the packages.

As for checking whether a frame exists, it makes sense.

Copy link
Contributor Author

Choose a reason for hiding this comment

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

What do you think about the problem with sensor_name? The simplest solution is to add a parameter that adds a namespace to sensor_name or add a sensor name prefix.

Generally, what I care about is being able to run many robots in simulation and on hardware while making a minimum number of changes to the code.

Copy link
Member

Choose a reason for hiding this comment

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

@rafal-gorecki ok now I got the use-case. Yes, it is tricky to make it work with simulation with this namespace. What can be done is probably, instead of using the tf_prefix, just add it to the sensor_name already. I'm not sure how to automatically configure it based on the robot. The current changes however, doesn't fix it right?

Any ideas on how to solve it are welcome

Copy link
Contributor Author

Choose a reason for hiding this comment

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

Thank you, so much for today. I will try to add this functionality soon. I would like to ask you to confirm whether this tf_prefix can be left for imu in order to standardize the packages, or should I get rid of it?

Copy link
Member

Choose a reason for hiding this comment

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

Let's discuss your idea on how you are planning to tackle the idea of sensor name automatically, and then it's better to start changing

Copy link
Contributor Author

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

Choose a reason for hiding this comment

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

  1. The best solution is probably to add some kind of standard that make robot_state_publisher with namespace add to all link reference_frame and sensor topic namespaces.
  2. Or add namespace argument to URDF and put it in all resonable field.

Then add the argument add_namespace_to_sensor_name (bool)

@saikishor what do you think?

Copy link
Contributor Author

@rafal-gorecki rafal-gorecki May 15, 2024 •
edited
Loading

Choose a reason for hiding this comment

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

Second solution added

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
10 changes: 10 additions & 0 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 @@ -41,3 +41,13 @@ imu_sensor_broadcaster:
fixed_size<>: [9],
}
}
tf_frame_prefix: {
type: string,
default_value: "",
description: "(optional) Prefix to be appended to the tf frames, will be added to the sensor's frame_id before publishing. If the parameter is empty, controller's namespace will be used.",
}
use_namespace_as_sensor_name_prefix: {
type: bool,
default_value: false,
description: "If true the '/namespace/' is added to the sensor name which causes changes in interfaces names e. g. /namespace/sensor_name/orientation.x",
}
9 changes: 5 additions & 4 deletions imu_sensor_broadcaster/test/imu_sensor_broadcaster_params.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
test_imu_sensor_broadcaster:
ros__parameters:
/**:
test_imu_sensor_broadcaster:
ros__parameters:

sensor_name: "imu_sensor"
frame_id: "imu_sensor_frame"
sensor_name: "imu_sensor"
frame_id: "imu_sensor_frame"
Loading