Skip to content

Added frame_id to Joint State Broadcaster #1746

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

Merged
merged 2 commits into from
Jun 11, 2025
Merged

Added frame_id to Joint State Broadcaster #1746

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
23d9db8
Added frame_id to Joint State Broadcaster
delipl Jun 7, 2025
13ae298
Run precommit
delipl Jun 7, 2025
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
2 changes: 2 additions & 0 deletions joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -97,6 +97,8 @@ class JointStateBroadcaster : public controller_interface::ControllerInterface
Params params_;
std::unordered_map<std::string, std::string> map_interface_to_joint_state_;

std::string frame_id_;

// For the JointState message,
// we store the name of joints with compatible interfaces
std::vector<std::string> joint_names_;
Expand Down
8 changes: 8 additions & 0 deletions joint_state_broadcaster/src/joint_state_broadcaster.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -181,6 +181,12 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_configure(
joint_state_msg.velocity.reserve(max_joints_size);
joint_state_msg.effort.reserve(max_joints_size);

frame_id_ = params_.frame_id;
if (frame_id_.empty())
{
RCLCPP_WARN(get_node()->get_logger(), "Frame ID is not set.");
}

return CallbackReturn::SUCCESS;
}

Expand Down Expand Up @@ -313,6 +319,7 @@ void JointStateBroadcaster::init_joint_state_msg()

// default initialization for joint state message
auto & joint_state_msg = realtime_joint_state_publisher_->msg_;
joint_state_msg.header.frame_id = frame_id_;
joint_state_msg.name = joint_names_;
joint_state_msg.position.resize(num_joints, kUninitializedValue);
joint_state_msg.velocity.resize(num_joints, kUninitializedValue);
Expand Down Expand Up @@ -346,6 +353,7 @@ void JointStateBroadcaster::init_joint_state_msg()
void JointStateBroadcaster::init_dynamic_joint_state_msg()
{
auto & dynamic_joint_state_msg = realtime_dynamic_joint_state_publisher_->msg_;
dynamic_joint_state_msg.header.frame_id = frame_id_;
dynamic_joint_state_msg.joint_names.clear();
dynamic_joint_state_msg.interface_values.clear();
for (const auto & name_ifv : name_if_value_mapping_)
Expand Down
5 changes: 5 additions & 0 deletions joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -46,3 +46,8 @@ joint_state_broadcaster:
If true, the broadcaster will publish the data of the joints present in the URDF alone.
If false, the broadcaster will publish the data of any interface that has type ``position``, ``velocity``, or ``effort``."
}
frame_id: {
type: string,
default_value: "base_link",
description: "The frame_id to be used in the published joint states. This parameter allows rviz2 to visualize the effort of the joints."
}
27 changes: 27 additions & 0 deletions joint_state_broadcaster/test/test_joint_state_broadcaster.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -77,6 +77,7 @@ void JointStateBroadcasterTest::init_broadcaster_and_set_parameters(

state_broadcaster_->get_node()->set_parameter({"joints", joint_names});
state_broadcaster_->get_node()->set_parameter({"interfaces", interfaces});
state_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_});
}

void JointStateBroadcasterTest::assign_state_interfaces(
Expand Down Expand Up @@ -177,6 +178,7 @@ TEST_F(JointStateBroadcasterTest, ActivateEmptyTest)

// joint state initialized
const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_);
ASSERT_THAT(joint_state_msg.name, ElementsAreArray(joint_names_));
ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS));
ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS));
Expand All @@ -185,6 +187,7 @@ TEST_F(JointStateBroadcasterTest, ActivateEmptyTest)
// dynamic joint state initialized
const auto & dynamic_joint_state_msg =
state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_);
ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS));
ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS));
ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(joint_names_));
Expand Down Expand Up @@ -227,6 +230,7 @@ TEST_F(JointStateBroadcasterTest, ReactivateTheControllerWithDifferentInterfaces

// joint state initialized
const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_);
ASSERT_THAT(joint_state_msg.name, ElementsAreArray(joint_names_));
ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS));
ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS));
Expand All @@ -235,6 +239,7 @@ TEST_F(JointStateBroadcasterTest, ReactivateTheControllerWithDifferentInterfaces
// dynamic joint state initialized
const auto & dynamic_joint_state_msg =
state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_);
ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS));
ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS));
ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(joint_names_));
Expand Down Expand Up @@ -272,6 +277,7 @@ TEST_F(JointStateBroadcasterTest, ReactivateTheControllerWithDifferentInterfaces

// joint state initialized
const auto & new_joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
ASSERT_EQ(new_joint_state_msg.header.frame_id, frame_id_);
ASSERT_THAT(new_joint_state_msg.name, ElementsAreArray(JOINT_NAMES));
ASSERT_THAT(new_joint_state_msg.position, SizeIs(NUM_JOINTS_WITH_ONE_DEACTIVATED));
ASSERT_THAT(new_joint_state_msg.velocity, SizeIs(NUM_JOINTS_WITH_ONE_DEACTIVATED));
Expand All @@ -280,6 +286,7 @@ TEST_F(JointStateBroadcasterTest, ReactivateTheControllerWithDifferentInterfaces
// dynamic joint state initialized
const auto & new_dynamic_joint_state_msg =
state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
ASSERT_EQ(new_dynamic_joint_state_msg.header.frame_id, frame_id_);
ASSERT_THAT(new_dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS_WITH_ONE_DEACTIVATED));
ASSERT_THAT(
new_dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS_WITH_ONE_DEACTIVATED));
Expand Down Expand Up @@ -322,6 +329,7 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithoutJointsParameter)

// joint state initialized
const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_);
ASSERT_THAT(joint_state_msg.name, ElementsAreArray(joint_names_));
ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS));
ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS));
Expand All @@ -330,6 +338,7 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithoutJointsParameter)
// dynamic joint state initialized
const auto & dynamic_joint_state_msg =
state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_);
ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS));
ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS));
ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(joint_names_));
Expand Down Expand Up @@ -577,6 +586,7 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithoutInterfacesParameter)

// joint state initialized
const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_);
ASSERT_THAT(joint_state_msg.name, ElementsAreArray(joint_names_));
ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS));
ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS));
Expand All @@ -585,6 +595,7 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithoutInterfacesParameter)
// dynamic joint state initialized
const auto & dynamic_joint_state_msg =
state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_);
ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS));
ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS));
ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(joint_names_));
Expand Down Expand Up @@ -626,6 +637,7 @@ TEST_F(JointStateBroadcasterTest, ActivateDeactivateTestTwoJointsOneInterface)

// joint state initialized
const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_);
ASSERT_THAT(joint_state_msg.name, ElementsAreArray(JOINT_NAMES));
ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS));
ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS));
Expand All @@ -642,6 +654,7 @@ TEST_F(JointStateBroadcasterTest, ActivateDeactivateTestTwoJointsOneInterface)
// dynamic joint state initialized
const auto & dynamic_joint_state_msg =
state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_);
ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS));
ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS));
ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(JOINT_NAMES));
Expand Down Expand Up @@ -690,6 +703,7 @@ TEST_F(JointStateBroadcasterTest, ActivateTestOneJointTwoInterfaces)

// joint state initialized
const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_);
ASSERT_THAT(joint_state_msg.name, ElementsAreArray(JOINT_NAMES));
ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS));
ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS));
Expand All @@ -702,6 +716,7 @@ TEST_F(JointStateBroadcasterTest, ActivateTestOneJointTwoInterfaces)
// dynamic joint state initialized
const auto & dynamic_joint_state_msg =
state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_);
ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS));
ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS));
ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(JOINT_NAMES));
Expand Down Expand Up @@ -766,6 +781,7 @@ TEST_F(JointStateBroadcasterTest, ActivateTestTwoJointTwoInterfacesOneMissing)

// joint state initialized
const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_);
ASSERT_THAT(joint_state_msg.name, ElementsAreArray(JOINT_NAMES));
ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS));
ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS));
Expand All @@ -780,6 +796,7 @@ TEST_F(JointStateBroadcasterTest, ActivateTestTwoJointTwoInterfacesOneMissing)
// dynamic joint state initialized
const auto & dynamic_joint_state_msg =
state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_);
ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS));
ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS));
ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(JOINT_NAMES));
Expand Down Expand Up @@ -813,6 +830,7 @@ TEST_F(JointStateBroadcasterTest, TestCustomInterfaceWithoutMapping)

// joint state initialized
const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_);
ASSERT_THAT(joint_state_msg.name, SizeIs(0));
ASSERT_THAT(joint_state_msg.position, SizeIs(0));
ASSERT_THAT(joint_state_msg.velocity, SizeIs(0));
Expand All @@ -821,6 +839,7 @@ TEST_F(JointStateBroadcasterTest, TestCustomInterfaceWithoutMapping)
// dynamic joint state initialized
const auto & dynamic_joint_state_msg =
state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_);
ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS));
ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS));
ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(JOINT_NAMES));
Expand Down Expand Up @@ -858,6 +877,7 @@ TEST_F(JointStateBroadcasterTest, TestCustomInterfaceMapping)

// joint state initialized
const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_);
ASSERT_THAT(joint_state_msg.name, ElementsAreArray(JOINT_NAMES));
ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS));
ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS));
Expand All @@ -874,6 +894,7 @@ TEST_F(JointStateBroadcasterTest, TestCustomInterfaceMapping)
// dynamic joint state initialized
const auto & dynamic_joint_state_msg =
state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_);
ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS));
ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS));
ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(JOINT_NAMES));
Expand Down Expand Up @@ -901,6 +922,7 @@ TEST_F(JointStateBroadcasterTest, TestCustomInterfaceMappingUpdate)
const size_t NUM_JOINTS = JOINT_NAMES.size();

// joint state initialized
ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_);
ASSERT_THAT(joint_state_msg.name, ElementsAreArray(JOINT_NAMES));
ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS));
ASSERT_EQ(joint_state_msg.position[0], custom_joint_value_);
Expand All @@ -918,6 +940,7 @@ TEST_F(JointStateBroadcasterTest, TestCustomInterfaceMappingUpdate)
// dynamic joint state initialized
const auto & dynamic_joint_state_msg =
state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_);
ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS));
ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS));
ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(JOINT_NAMES));
Expand Down Expand Up @@ -1107,6 +1130,7 @@ void JointStateBroadcasterTest::test_published_joint_state_message(const std::st
activate_and_get_joint_state_message(topic, joint_state_msg);

const size_t NUM_JOINTS = joint_names_.size();
ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_);
ASSERT_THAT(joint_state_msg.name, SizeIs(NUM_JOINTS));
// the order in the message may be different
// we only check that all values in this test are present in the message
Expand Down Expand Up @@ -1177,6 +1201,7 @@ void JointStateBroadcasterTest::test_published_dynamic_joint_state_message(

const size_t NUM_JOINTS = 3;
const std::vector<std::string> INTERFACE_NAMES = {HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_EFFORT};
ASSERT_EQ(dynamic_joint_state_msg->header.frame_id, frame_id_);
ASSERT_THAT(dynamic_joint_state_msg->joint_names, SizeIs(NUM_JOINTS));
// the order in the message may be different
// we only check that all values in this test are present in the message
Expand Down Expand Up @@ -1235,6 +1260,7 @@ TEST_F(JointStateBroadcasterTest, ExtraJointStatePublishTest)

// joint state initialized
const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_);
ASSERT_THAT(joint_state_msg.name, ElementsAreArray(all_joint_names));
ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS));
ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS));
Expand All @@ -1243,5 +1269,6 @@ TEST_F(JointStateBroadcasterTest, ExtraJointStatePublishTest)
// dynamic joint state initialized
const auto & dynamic_joint_state_msg =
state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_);
ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS));
}
1 change: 1 addition & 0 deletions joint_state_broadcaster/test/test_joint_state_broadcaster.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -112,6 +112,7 @@ class JointStateBroadcasterTest : public ::testing::Test
std::vector<hardware_interface::StateInterface> test_interfaces_;

std::unique_ptr<FriendJointStateBroadcaster> state_broadcaster_;
std::string frame_id_ = "base_link";
};

#endif // TEST_JOINT_STATE_BROADCASTER_HPP_
Loading