fix(joint_state_broadcaster): suppress confusing warning for standard interfaces (backport #2276)

joint_state_broadcaster/doc/joint_state_broadcaster_parameter_context.yml

@@ -18,6 +18,11 @@ map_interface_to_joint_state: |
         \t\tvelocity: <custom_interface>
         \t\teffort: <custom_interface>
 
+  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:
 

joint_state_broadcaster/src/joint_state_broadcaster.cpp

@@ -116,8 +116,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());

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.<field>`` 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: {

joint_state_broadcaster/test/test_joint_state_broadcaster.cpp

@@ -940,6 +940,52 @@ TEST_F(JointStateBroadcasterTest, TestCustomInterfaceMapping)
   ASSERT_TRUE(state_broadcaster_->dynamic_joint_state_publisher_);
 }
 
+TEST_F(JointStateBroadcasterTest, TestCustomInterfaceMappingIgnoredWhenVelocityInterfaceIsRequested)
+{
+  constexpr auto custom_velocity_interface = "derived_velocity";
+  const std::vector<std::string> JOINT_NAMES = {joint_names_[0]};
+  const std::vector<std::string> 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<std::string> JOINT_NAMES = {joint_names_[0]};

joint_state_broadcaster/test/test_joint_state_broadcaster.hpp

@@ -48,6 +48,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);