diff --git a/joint_state_broadcaster/doc/joint_state_broadcaster_parameter_context.yml b/joint_state_broadcaster/doc/joint_state_broadcaster_parameter_context.yml index c96f8301ae..a631ffce90 100644 --- a/joint_state_broadcaster/doc/joint_state_broadcaster_parameter_context.yml +++ b/joint_state_broadcaster/doc/joint_state_broadcaster_parameter_context.yml @@ -18,6 +18,11 @@ map_interface_to_joint_state: | \t\tvelocity: \t\teffort: + If ``interfaces`` explicitly contains ``position``, ``velocity``, or ``effort``, the matching + ``map_interface_to_joint_state`` entry is ignored because the standard interface is already + requested directly. In that wrong configuration, a warning is printed only when the configured + mapping value differs from the standard interface name. + Examples: diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index b24152b0d8..95437b2c8d 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -124,8 +124,9 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_configure( params_.interfaces.end()) { map_interface_to_joint_state_[interface] = interface; - RCLCPP_WARN( - get_node()->get_logger(), + // Warn if custom mapping is being ignored + RCLCPP_WARN_EXPRESSION( + get_node()->get_logger(), interface != interface_to_map, "Mapping from '%s' to interface '%s' will not be done, because '%s' is defined " "in 'interface' parameter.", interface_to_map.c_str(), interface.c_str(), interface.c_str()); diff --git a/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml b/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml index 2993dddceb..cfe4104ac8 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml +++ b/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml @@ -28,7 +28,11 @@ joint_state_broadcaster: description: "Parameter to support broadcasting of only specific joints and interfaces. It has to be used in combination with the ``joints`` parameter. If either ``joints`` or ``interfaces`` is left empty, all available state interfaces will be - published." + published. + If this parameter contains ``position``, ``velocity``, or ``effort``, the corresponding + ``map_interface_to_joint_state.`` setting is ignored because the standard interface is + already requested directly. A warning is printed only when such an ignored mapping is set to a + different custom interface name." } map_interface_to_joint_state: position: { diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index 53c8c0bee0..621aed3847 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -703,6 +703,52 @@ TEST_F(JointStateBroadcasterTest, TestCustomInterfaceMapping) ASSERT_TRUE(state_broadcaster_->joint_state_publisher_); } +TEST_F(JointStateBroadcasterTest, TestCustomInterfaceMappingIgnoredWhenVelocityInterfaceIsRequested) +{ + constexpr auto custom_velocity_interface = "derived_velocity"; + const std::vector JOINT_NAMES = {joint_names_[0]}; + const std::vector IF_NAMES = {HW_IF_VELOCITY}; + SetUpStateBroadcaster( + JOINT_NAMES, IF_NAMES, + {rclcpp::Parameter( + std::string("map_interface_to_joint_state.") + HW_IF_VELOCITY, custom_velocity_interface)}); + + // configure ok; a warning is expected because the custom velocity mapping is ignored + ASSERT_EQ(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + const auto velocity_mapping = + state_broadcaster_->map_interface_to_joint_state_.find(HW_IF_VELOCITY); + ASSERT_NE(velocity_mapping, state_broadcaster_->map_interface_to_joint_state_.end()); + EXPECT_EQ(velocity_mapping->second, HW_IF_VELOCITY); + EXPECT_EQ(state_broadcaster_->map_interface_to_joint_state_.count(custom_velocity_interface), 0u); + + ASSERT_EQ(state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + const size_t NUM_JOINTS = JOINT_NAMES.size(); + + // check interface configuration + auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); + auto state_if_conf = state_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size())); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + + // joint state initialized + const auto & joint_state_msg = state_broadcaster_->joint_state_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_TRUE(std::isnan(joint_state_msg.position[0])); + ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS)); + ASSERT_TRUE(std::isnan(joint_state_msg.velocity[0])); + ASSERT_THAT(joint_state_msg.effort, SizeIs(NUM_JOINTS)); + ASSERT_TRUE(std::isnan(joint_state_msg.effort[0])); + + // publisher initialized + ASSERT_TRUE(state_broadcaster_->joint_state_publisher_); +} + TEST_F(JointStateBroadcasterTest, TestCustomInterfaceMappingUpdate) { const std::vector JOINT_NAMES = {joint_names_[0]}; diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp index 4a4e9345e9..35980611d5 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp @@ -47,6 +47,8 @@ class FriendJointStateBroadcaster : public joint_state_broadcaster::JointStateBr FRIEND_TEST(JointStateBroadcasterTest, ActivateTestTwoJointTwoInterfacesOneMissing); FRIEND_TEST(JointStateBroadcasterTest, TestCustomInterfaceWithoutMapping); FRIEND_TEST(JointStateBroadcasterTest, TestCustomInterfaceMapping); + FRIEND_TEST( + JointStateBroadcasterTest, TestCustomInterfaceMappingIgnoredWhenVelocityInterfaceIsRequested); FRIEND_TEST(JointStateBroadcasterTest, TestCustomInterfaceMappingUpdate); FRIEND_TEST(JointStateBroadcasterTest, ExtraJointStatePublishTest); FRIEND_TEST(JointStateBroadcasterTest, NoThrowWithBooleanInterfaceTest);