fix(joint_state_broadcaster): suppress confusing warning for standard interfaces

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

@@ -124,11 +124,15 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_configure(
       params_.interfaces.end())
     {
       map_interface_to_joint_state_[interface] = interface;
-      RCLCPP_WARN(
-        get_node()->get_logger(),
-        "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());
+      // Warn if custom mapping is being ignored
+      if (interface != interface_to_map)
+      {
+        RCLCPP_WARN(
+          get_node()->get_logger(),
+          "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());
+      }
     }
     else
     {

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

@@ -703,6 +703,98 @@ 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<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_EQ(joint_state_msg.velocity[0], joint_values_[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, TestStandardVelocityInterfaceNoWarning)
+{
+  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, {});
+
+  // configure ok; no warning should be printed because velocity interface matches standard
+  // mapping (no custom mapping was specified)
+  ASSERT_EQ(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
+
+  // verify the mapping is velocity -> velocity (no custom mapping)
+  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);
+
+  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_EQ(joint_state_msg.velocity[0], joint_values_[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

@@ -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);