Skip to content

Commit 4b488cb

Browse files
saikishorsaikishor
authored
[Humble] [JSB] Add frame_id to the joint states message (#1751)
1 parent 304665a commit 4b488cb

File tree

4 files changed

+39
-0
lines changed

4 files changed

+39
-0
lines changed

‎joint_state_broadcaster/src/joint_state_broadcaster.cpp

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -169,6 +169,10 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_configure(
169169
fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
170170
return CallbackReturn::ERROR;
171171
}
172+
173+
RCLCPP_WARN_EXPRESSION(
174+
get_node()->get_logger(), params_.frame_id.empty(), "Frame ID is not set.");
175+
172176
return CallbackReturn::SUCCESS;
173177
}
174178

@@ -289,6 +293,7 @@ void JointStateBroadcaster::init_joint_state_msg()
289293

290294
// default initialization for joint state message
291295
auto & joint_state_msg = realtime_joint_state_publisher_->msg_;
296+
joint_state_msg.header.frame_id = params_.frame_id;
292297
joint_state_msg.name = joint_names_;
293298
joint_state_msg.position.resize(num_joints, kUninitializedValue);
294299
joint_state_msg.velocity.resize(num_joints, kUninitializedValue);
@@ -298,6 +303,7 @@ void JointStateBroadcaster::init_joint_state_msg()
298303
void JointStateBroadcaster::init_dynamic_joint_state_msg()
299304
{
300305
auto & dynamic_joint_state_msg = realtime_dynamic_joint_state_publisher_->msg_;
306+
dynamic_joint_state_msg.header.frame_id = params_.frame_id;
301307
dynamic_joint_state_msg.joint_names.clear();
302308
dynamic_joint_state_msg.interface_values.clear();
303309
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
@@ -39,3 +39,8 @@ joint_state_broadcaster:
3939
type: string,
4040
default_value: "effort",
4141
}
42+
frame_id: {
43+
type: string,
44+
default_value: "base_link",
45+
description: "The frame_id to be used in the published joint states. This parameter allows rviz2 to visualize the effort of the joints."
46+
}

‎joint_state_broadcaster/test/test_joint_state_broadcaster.cpp

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

7575
state_broadcaster_->get_node()->set_parameter({"joints", joint_names});
7676
state_broadcaster_->get_node()->set_parameter({"interfaces", interfaces});
77+
state_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_});
7778
}
7879

7980
void JointStateBroadcasterTest::assign_state_interfaces(
@@ -191,6 +192,7 @@ TEST_F(JointStateBroadcasterTest, ActivateEmptyTest)
191192

192193
// joint state initialized
193194
const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
195+
ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_);
194196
ASSERT_THAT(joint_state_msg.name, ElementsAreArray(joint_names_));
195197
ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS));
196198
ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS));
@@ -199,6 +201,7 @@ TEST_F(JointStateBroadcasterTest, ActivateEmptyTest)
199201
// dynamic joint state initialized
200202
const auto & dynamic_joint_state_msg =
201203
state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
204+
ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_);
202205
ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS));
203206
ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS));
204207
ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(joint_names_));
@@ -241,6 +244,7 @@ TEST_F(JointStateBroadcasterTest, ReactivateTheControllerWithDifferentInterfaces
241244

242245
// joint state initialized
243246
const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
247+
ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_);
244248
ASSERT_THAT(joint_state_msg.name, ElementsAreArray(joint_names_));
245249
ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS));
246250
ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS));
@@ -249,6 +253,7 @@ TEST_F(JointStateBroadcasterTest, ReactivateTheControllerWithDifferentInterfaces
249253
// dynamic joint state initialized
250254
const auto & dynamic_joint_state_msg =
251255
state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
256+
ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_);
252257
ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS));
253258
ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS));
254259
ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(joint_names_));
@@ -286,6 +291,7 @@ TEST_F(JointStateBroadcasterTest, ReactivateTheControllerWithDifferentInterfaces
286291

287292
// joint state initialized
288293
const auto & new_joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
294+
ASSERT_EQ(new_joint_state_msg.header.frame_id, frame_id_);
289295
ASSERT_THAT(new_joint_state_msg.name, ElementsAreArray(JOINT_NAMES));
290296
ASSERT_THAT(new_joint_state_msg.position, SizeIs(NUM_JOINTS_WITH_ONE_DEACTIVATED));
291297
ASSERT_THAT(new_joint_state_msg.velocity, SizeIs(NUM_JOINTS_WITH_ONE_DEACTIVATED));
@@ -294,6 +300,7 @@ TEST_F(JointStateBroadcasterTest, ReactivateTheControllerWithDifferentInterfaces
294300
// dynamic joint state initialized
295301
const auto & new_dynamic_joint_state_msg =
296302
state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
303+
ASSERT_EQ(new_dynamic_joint_state_msg.header.frame_id, frame_id_);
297304
ASSERT_THAT(new_dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS_WITH_ONE_DEACTIVATED));
298305
ASSERT_THAT(
299306
new_dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS_WITH_ONE_DEACTIVATED));
@@ -336,6 +343,7 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithoutJointsParameter)
336343

337344
// joint state initialized
338345
const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
346+
ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_);
339347
ASSERT_THAT(joint_state_msg.name, ElementsAreArray(joint_names_));
340348
ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS));
341349
ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS));
@@ -344,6 +352,7 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithoutJointsParameter)
344352
// dynamic joint state initialized
345353
const auto & dynamic_joint_state_msg =
346354
state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
355+
ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_);
347356
ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS));
348357
ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS));
349358
ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(joint_names_));
@@ -385,6 +394,7 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithoutInterfacesParameter)
385394

386395
// joint state initialized
387396
const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
397+
ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_);
388398
ASSERT_THAT(joint_state_msg.name, ElementsAreArray(joint_names_));
389399
ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS));
390400
ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS));
@@ -393,6 +403,7 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithoutInterfacesParameter)
393403
// dynamic joint state initialized
394404
const auto & dynamic_joint_state_msg =
395405
state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
406+
ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_);
396407
ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS));
397408
ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS));
398409
ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(joint_names_));
@@ -434,6 +445,7 @@ TEST_F(JointStateBroadcasterTest, ActivateDeactivateTestTwoJointsOneInterface)
434445

435446
// joint state initialized
436447
const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
448+
ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_);
437449
ASSERT_THAT(joint_state_msg.name, ElementsAreArray(JOINT_NAMES));
438450
ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS));
439451
ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS));
@@ -450,6 +462,7 @@ TEST_F(JointStateBroadcasterTest, ActivateDeactivateTestTwoJointsOneInterface)
450462
// dynamic joint state initialized
451463
const auto & dynamic_joint_state_msg =
452464
state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
465+
ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_);
453466
ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS));
454467
ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS));
455468
ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(JOINT_NAMES));
@@ -498,6 +511,7 @@ TEST_F(JointStateBroadcasterTest, ActivateTestOneJointTwoInterfaces)
498511

499512
// joint state initialized
500513
const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
514+
ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_);
501515
ASSERT_THAT(joint_state_msg.name, ElementsAreArray(JOINT_NAMES));
502516
ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS));
503517
ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS));
@@ -510,6 +524,7 @@ TEST_F(JointStateBroadcasterTest, ActivateTestOneJointTwoInterfaces)
510524
// dynamic joint state initialized
511525
const auto & dynamic_joint_state_msg =
512526
state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
527+
ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_);
513528
ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS));
514529
ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS));
515530
ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(JOINT_NAMES));
@@ -574,6 +589,7 @@ TEST_F(JointStateBroadcasterTest, ActivateTestTwoJointTwoInterfacesOneMissing)
574589

575590
// joint state initialized
576591
const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
592+
ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_);
577593
ASSERT_THAT(joint_state_msg.name, ElementsAreArray(JOINT_NAMES));
578594
ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS));
579595
ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS));
@@ -588,6 +604,7 @@ TEST_F(JointStateBroadcasterTest, ActivateTestTwoJointTwoInterfacesOneMissing)
588604
// dynamic joint state initialized
589605
const auto & dynamic_joint_state_msg =
590606
state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
607+
ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_);
591608
ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS));
592609
ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS));
593610
ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(JOINT_NAMES));
@@ -621,6 +638,7 @@ TEST_F(JointStateBroadcasterTest, TestCustomInterfaceWithoutMapping)
621638

622639
// joint state initialized
623640
const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
641+
ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_);
624642
ASSERT_THAT(joint_state_msg.name, SizeIs(0));
625643
ASSERT_THAT(joint_state_msg.position, SizeIs(0));
626644
ASSERT_THAT(joint_state_msg.velocity, SizeIs(0));
@@ -629,6 +647,7 @@ TEST_F(JointStateBroadcasterTest, TestCustomInterfaceWithoutMapping)
629647
// dynamic joint state initialized
630648
const auto & dynamic_joint_state_msg =
631649
state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
650+
ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_);
632651
ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS));
633652
ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS));
634653
ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(JOINT_NAMES));
@@ -666,6 +685,7 @@ TEST_F(JointStateBroadcasterTest, TestCustomInterfaceMapping)
666685

667686
// joint state initialized
668687
const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
688+
ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_);
669689
ASSERT_THAT(joint_state_msg.name, ElementsAreArray(JOINT_NAMES));
670690
ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS));
671691
ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS));
@@ -682,6 +702,7 @@ TEST_F(JointStateBroadcasterTest, TestCustomInterfaceMapping)
682702
// dynamic joint state initialized
683703
const auto & dynamic_joint_state_msg =
684704
state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
705+
ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_);
685706
ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS));
686707
ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS));
687708
ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(JOINT_NAMES));
@@ -709,6 +730,7 @@ TEST_F(JointStateBroadcasterTest, TestCustomInterfaceMappingUpdate)
709730
const size_t NUM_JOINTS = JOINT_NAMES.size();
710731

711732
// joint state initialized
733+
ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_);
712734
ASSERT_THAT(joint_state_msg.name, ElementsAreArray(JOINT_NAMES));
713735
ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS));
714736
ASSERT_EQ(joint_state_msg.position[0], custom_joint_value_);
@@ -726,6 +748,7 @@ TEST_F(JointStateBroadcasterTest, TestCustomInterfaceMappingUpdate)
726748
// dynamic joint state initialized
727749
const auto & dynamic_joint_state_msg =
728750
state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
751+
ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_);
729752
ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS));
730753
ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS));
731754
ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(JOINT_NAMES));
@@ -802,6 +825,7 @@ void JointStateBroadcasterTest::test_published_joint_state_message(const std::st
802825
activate_and_get_joint_state_message(topic, joint_state_msg);
803826

804827
const size_t NUM_JOINTS = joint_names_.size();
828+
ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_);
805829
ASSERT_THAT(joint_state_msg.name, SizeIs(NUM_JOINTS));
806830
// the order in the message may be different
807831
// we only check that all values in this test are present in the message
@@ -872,6 +896,7 @@ void JointStateBroadcasterTest::test_published_dynamic_joint_state_message(
872896

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

931956
// joint state initialized
932957
const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
958+
ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_);
933959
ASSERT_THAT(joint_state_msg.name, ElementsAreArray(all_joint_names));
934960
ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS));
935961
ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS));
@@ -938,5 +964,6 @@ TEST_F(JointStateBroadcasterTest, ExtraJointStatePublishTest)
938964
// dynamic joint state initialized
939965
const auto & dynamic_joint_state_msg =
940966
state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
967+
ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_);
941968
ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS));
942969
}

‎joint_state_broadcaster/test/test_joint_state_broadcaster.hpp

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -106,6 +106,7 @@ class JointStateBroadcasterTest : public ::testing::Test
106106
joint_names_[0], custom_interface_name_, &custom_joint_value_};
107107

108108
std::unique_ptr<FriendJointStateBroadcaster> state_broadcaster_;
109+
std::string frame_id_ = "base_link";
109110
};
110111

111112
#endif // TEST_JOINT_STATE_BROADCASTER_HPP_

0 commit comments

Comments
 (0)