Skip to content

Commit fc576ee

Browse files
delipldelipl
authored
Added frame_id to Joint State Broadcaster (#1746)
1 parent 63448a3 commit fc576ee

File tree

5 files changed

+43
-0
lines changed

5 files changed

+43
-0
lines changed

‎joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -97,6 +97,8 @@ class JointStateBroadcaster : public controller_interface::ControllerInterface
9797
Params params_;
9898
std::unordered_map<std::string, std::string> map_interface_to_joint_state_;
9999

100+
std::string frame_id_;
101+
100102
// For the JointState message,
101103
// we store the name of joints with compatible interfaces
102104
std::vector<std::string> joint_names_;

‎joint_state_broadcaster/src/joint_state_broadcaster.cpp‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -181,6 +181,12 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_configure(
181181
joint_state_msg.velocity.reserve(max_joints_size);
182182
joint_state_msg.effort.reserve(max_joints_size);
183183

184+
frame_id_ = params_.frame_id;
185+
if (frame_id_.empty())
186+
{
187+
RCLCPP_WARN(get_node()->get_logger(), "Frame ID is not set.");
188+
}
189+
184190
return CallbackReturn::SUCCESS;
185191
}
186192

@@ -313,6 +319,7 @@ void JointStateBroadcaster::init_joint_state_msg()
313319

314320
// default initialization for joint state message
315321
auto & joint_state_msg = realtime_joint_state_publisher_->msg_;
322+
joint_state_msg.header.frame_id = frame_id_;
316323
joint_state_msg.name = joint_names_;
317324
joint_state_msg.position.resize(num_joints, kUninitializedValue);
318325
joint_state_msg.velocity.resize(num_joints, kUninitializedValue);
@@ -346,6 +353,7 @@ void JointStateBroadcaster::init_joint_state_msg()
346353
void JointStateBroadcaster::init_dynamic_joint_state_msg()
347354
{
348355
auto & dynamic_joint_state_msg = realtime_dynamic_joint_state_publisher_->msg_;
356+
dynamic_joint_state_msg.header.frame_id = frame_id_;
349357
dynamic_joint_state_msg.joint_names.clear();
350358
dynamic_joint_state_msg.interface_values.clear();
351359
for (const auto & name_ifv : name_if_value_mapping_)

‎joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -46,3 +46,8 @@ joint_state_broadcaster:
4646
If true, the broadcaster will publish the data of the joints present in the URDF alone.
4747
If false, the broadcaster will publish the data of any interface that has type ``position``, ``velocity``, or ``effort``."
4848
}
49+
frame_id: {
50+
type: string,
51+
default_value: "base_link",
52+
description: "The frame_id to be used in the published joint states. This parameter allows rviz2 to visualize the effort of the joints."
53+
}

‎joint_state_broadcaster/test/test_joint_state_broadcaster.cpp‎

Lines changed: 27 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -77,6 +77,7 @@ void JointStateBroadcasterTest::init_broadcaster_and_set_parameters(
7777

7878
state_broadcaster_->get_node()->set_parameter({"joints", joint_names});
7979
state_broadcaster_->get_node()->set_parameter({"interfaces", interfaces});
80+
state_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_});
8081
}
8182

8283
void JointStateBroadcasterTest::assign_state_interfaces(
@@ -177,6 +178,7 @@ TEST_F(JointStateBroadcasterTest, ActivateEmptyTest)
177178

178179
// joint state initialized
179180
const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
181+
ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_);
180182
ASSERT_THAT(joint_state_msg.name, ElementsAreArray(joint_names_));
181183
ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS));
182184
ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS));
@@ -185,6 +187,7 @@ TEST_F(JointStateBroadcasterTest, ActivateEmptyTest)
185187
// dynamic joint state initialized
186188
const auto & dynamic_joint_state_msg =
187189
state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
190+
ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_);
188191
ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS));
189192
ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS));
190193
ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(joint_names_));
@@ -227,6 +230,7 @@ TEST_F(JointStateBroadcasterTest, ReactivateTheControllerWithDifferentInterfaces
227230

228231
// joint state initialized
229232
const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
233+
ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_);
230234
ASSERT_THAT(joint_state_msg.name, ElementsAreArray(joint_names_));
231235
ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS));
232236
ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS));
@@ -235,6 +239,7 @@ TEST_F(JointStateBroadcasterTest, ReactivateTheControllerWithDifferentInterfaces
235239
// dynamic joint state initialized
236240
const auto & dynamic_joint_state_msg =
237241
state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
242+
ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_);
238243
ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS));
239244
ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS));
240245
ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(joint_names_));
@@ -272,6 +277,7 @@ TEST_F(JointStateBroadcasterTest, ReactivateTheControllerWithDifferentInterfaces
272277

273278
// joint state initialized
274279
const auto & new_joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
280+
ASSERT_EQ(new_joint_state_msg.header.frame_id, frame_id_);
275281
ASSERT_THAT(new_joint_state_msg.name, ElementsAreArray(JOINT_NAMES));
276282
ASSERT_THAT(new_joint_state_msg.position, SizeIs(NUM_JOINTS_WITH_ONE_DEACTIVATED));
277283
ASSERT_THAT(new_joint_state_msg.velocity, SizeIs(NUM_JOINTS_WITH_ONE_DEACTIVATED));
@@ -280,6 +286,7 @@ TEST_F(JointStateBroadcasterTest, ReactivateTheControllerWithDifferentInterfaces
280286
// dynamic joint state initialized
281287
const auto & new_dynamic_joint_state_msg =
282288
state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
289+
ASSERT_EQ(new_dynamic_joint_state_msg.header.frame_id, frame_id_);
283290
ASSERT_THAT(new_dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS_WITH_ONE_DEACTIVATED));
284291
ASSERT_THAT(
285292
new_dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS_WITH_ONE_DEACTIVATED));
@@ -322,6 +329,7 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithoutJointsParameter)
322329

323330
// joint state initialized
324331
const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
332+
ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_);
325333
ASSERT_THAT(joint_state_msg.name, ElementsAreArray(joint_names_));
326334
ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS));
327335
ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS));
@@ -330,6 +338,7 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithoutJointsParameter)
330338
// dynamic joint state initialized
331339
const auto & dynamic_joint_state_msg =
332340
state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
341+
ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_);
333342
ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS));
334343
ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS));
335344
ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(joint_names_));
@@ -577,6 +586,7 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithoutInterfacesParameter)
577586

578587
// joint state initialized
579588
const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
589+
ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_);
580590
ASSERT_THAT(joint_state_msg.name, ElementsAreArray(joint_names_));
581591
ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS));
582592
ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS));
@@ -585,6 +595,7 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithoutInterfacesParameter)
585595
// dynamic joint state initialized
586596
const auto & dynamic_joint_state_msg =
587597
state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
598+
ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_);
588599
ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS));
589600
ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS));
590601
ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(joint_names_));
@@ -626,6 +637,7 @@ TEST_F(JointStateBroadcasterTest, ActivateDeactivateTestTwoJointsOneInterface)
626637

627638
// joint state initialized
628639
const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
640+
ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_);
629641
ASSERT_THAT(joint_state_msg.name, ElementsAreArray(JOINT_NAMES));
630642
ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS));
631643
ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS));
@@ -642,6 +654,7 @@ TEST_F(JointStateBroadcasterTest, ActivateDeactivateTestTwoJointsOneInterface)
642654
// dynamic joint state initialized
643655
const auto & dynamic_joint_state_msg =
644656
state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
657+
ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_);
645658
ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS));
646659
ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS));
647660
ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(JOINT_NAMES));
@@ -690,6 +703,7 @@ TEST_F(JointStateBroadcasterTest, ActivateTestOneJointTwoInterfaces)
690703

691704
// joint state initialized
692705
const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
706+
ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_);
693707
ASSERT_THAT(joint_state_msg.name, ElementsAreArray(JOINT_NAMES));
694708
ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS));
695709
ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS));
@@ -702,6 +716,7 @@ TEST_F(JointStateBroadcasterTest, ActivateTestOneJointTwoInterfaces)
702716
// dynamic joint state initialized
703717
const auto & dynamic_joint_state_msg =
704718
state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
719+
ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_);
705720
ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS));
706721
ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS));
707722
ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(JOINT_NAMES));
@@ -766,6 +781,7 @@ TEST_F(JointStateBroadcasterTest, ActivateTestTwoJointTwoInterfacesOneMissing)
766781

767782
// joint state initialized
768783
const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
784+
ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_);
769785
ASSERT_THAT(joint_state_msg.name, ElementsAreArray(JOINT_NAMES));
770786
ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS));
771787
ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS));
@@ -780,6 +796,7 @@ TEST_F(JointStateBroadcasterTest, ActivateTestTwoJointTwoInterfacesOneMissing)
780796
// dynamic joint state initialized
781797
const auto & dynamic_joint_state_msg =
782798
state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
799+
ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_);
783800
ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS));
784801
ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS));
785802
ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(JOINT_NAMES));
@@ -813,6 +830,7 @@ TEST_F(JointStateBroadcasterTest, TestCustomInterfaceWithoutMapping)
813830

814831
// joint state initialized
815832
const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
833+
ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_);
816834
ASSERT_THAT(joint_state_msg.name, SizeIs(0));
817835
ASSERT_THAT(joint_state_msg.position, SizeIs(0));
818836
ASSERT_THAT(joint_state_msg.velocity, SizeIs(0));
@@ -821,6 +839,7 @@ TEST_F(JointStateBroadcasterTest, TestCustomInterfaceWithoutMapping)
821839
// dynamic joint state initialized
822840
const auto & dynamic_joint_state_msg =
823841
state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
842+
ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_);
824843
ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS));
825844
ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS));
826845
ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(JOINT_NAMES));
@@ -858,6 +877,7 @@ TEST_F(JointStateBroadcasterTest, TestCustomInterfaceMapping)
858877

859878
// joint state initialized
860879
const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
880+
ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_);
861881
ASSERT_THAT(joint_state_msg.name, ElementsAreArray(JOINT_NAMES));
862882
ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS));
863883
ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS));
@@ -874,6 +894,7 @@ TEST_F(JointStateBroadcasterTest, TestCustomInterfaceMapping)
874894
// dynamic joint state initialized
875895
const auto & dynamic_joint_state_msg =
876896
state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
897+
ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_);
877898
ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS));
878899
ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS));
879900
ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(JOINT_NAMES));
@@ -901,6 +922,7 @@ TEST_F(JointStateBroadcasterTest, TestCustomInterfaceMappingUpdate)
901922
const size_t NUM_JOINTS = JOINT_NAMES.size();
902923

903924
// joint state initialized
925+
ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_);
904926
ASSERT_THAT(joint_state_msg.name, ElementsAreArray(JOINT_NAMES));
905927
ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS));
906928
ASSERT_EQ(joint_state_msg.position[0], custom_joint_value_);
@@ -918,6 +940,7 @@ TEST_F(JointStateBroadcasterTest, TestCustomInterfaceMappingUpdate)
918940
// dynamic joint state initialized
919941
const auto & dynamic_joint_state_msg =
920942
state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
943+
ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_);
921944
ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS));
922945
ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS));
923946
ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(JOINT_NAMES));
@@ -1107,6 +1130,7 @@ void JointStateBroadcasterTest::test_published_joint_state_message(const std::st
11071130
activate_and_get_joint_state_message(topic, joint_state_msg);
11081131

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

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

12361261
// joint state initialized
12371262
const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
1263+
ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_);
12381264
ASSERT_THAT(joint_state_msg.name, ElementsAreArray(all_joint_names));
12391265
ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS));
12401266
ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS));
@@ -1243,5 +1269,6 @@ TEST_F(JointStateBroadcasterTest, ExtraJointStatePublishTest)
12431269
// dynamic joint state initialized
12441270
const auto & dynamic_joint_state_msg =
12451271
state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
1272+
ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_);
12461273
ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS));
12471274
}

‎joint_state_broadcaster/test/test_joint_state_broadcaster.hpp‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -112,6 +112,7 @@ class JointStateBroadcasterTest : public ::testing::Test
112112
std::vector<hardware_interface::StateInterface> test_interfaces_;
113113

114114
std::unique_ptr<FriendJointStateBroadcaster> state_broadcaster_;
115+
std::string frame_id_ = "base_link";
115116
};
116117

117118
#endif // TEST_JOINT_STATE_BROADCASTER_HPP_

0 commit comments

Comments
 (0)