[Joint State Broadcaster] Add mapping of custom states to standard va…

…lues in "/joint_state" message (ros-controls#217)

* Joint state broadcaster support for specific joint_names and interface_names.
* Added option to map custom values to interface names to standard names in joint_state message.
* Document functionality.
* Add configuration examples

Co-authored-by: Bence Magyar <bence.magyar.robotics@gmail.com>

joint_state_broadcaster/doc/userdoc.rst

@@ -22,23 +22,68 @@ Parameters
 ----------
 
 ``use_local_topics``
-
   Optional parameter (boolean; default: ``False``) defining if ``joint_states`` and ``dynamic_joint_states`` messages should be published into local namespace, e.g., ``/my_state_broadcaster/joint_states``.
 
 
 ``joints``
-
   Optional parameter (string array) to support broadcasting of only specific joints and interfaces.
   It has to be used in combination with the ``interfaces`` parameter.
   Joint state broadcaster asks for access to all defined interfaces on all defined joints.
 
 
 ``interfaces``
-
   Optional parameter (string array) to support broadcasting of only specific joints and interfaces.
   It has to be used in combination with the ``joints`` parameter.
 
 
 ``extra_joints``
-
   Optional parameter (string array) with names of extra joints to be added to ``joint_states`` and ``dynamic_joint_states`` with state set to 0.
+
+
+``map_interface_to_joint_state``
+  Optional parameter (map) providing mapping between custom interface names to standard fields in ``joint_states`` message.
+  Usecases:
+
+    1. Hydraulics robots where feedback and commanded values often have an offset and reliance on open-loop control is common.
+       Typically one would map both values in separate interfaces in the framework.
+       To visualize those data multiple joint_state_broadcaster instances and robot_state_publishers would be used to visualize both values in RViz.
+
+    1. A robot provides multiple measuring techniques for its joint values which results in slightly different values.
+       Typically one would use separate interface for providing those values in the framework.
+       Using multiple joint_state_broadcaster instances we could publish and show both in RViz.
+
+  Format (each line is optional):
+
+  .. code-block:: yaml
+
+      map_interface_to_joint_state:
+        position: <custom_interface>
+        velocity: <custom_interface>
+        effort: <custom_interface>
+
+
+  Examples:
+
+  .. code-block:: yaml
+
+      map_interface_to_joint_state:
+        position: kf_estimated_position
+
+
+  .. code-block:: yaml
+
+      map_interface_to_joint_state:
+        velocity: derived_velocity
+        effort: derived_effort
+
+
+  .. code-block:: yaml
+
+      map_interface_to_joint_state:
+        effort: torque_sensor
+
+
+  .. code-block:: yaml
+
+      map_interface_to_joint_state:
+        effort: current_sensor

joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp

@@ -38,11 +38,16 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface
  * There is a possibility to publish all available states (typical use), or only specific ones.
  * The latter is, for example, used when hardware provides multiple measurement sources for some
  * of its states, e.g., position.
+ * It is possible to define a mapping of measurements
+ * from different sources stored in custom interfaces to standard dynamic names in JointState
+ * message.
  * If "joints" or "interfaces" parameter is empty, all available states are published.
  *
  * \param use_local_topics Flag to publish topics in local namespace.
  * \param joints Names of the joints to publish.
  * \param interfaces Names of interfaces to publish.
+ * \param map_interface_to_joint_state.{HW_IF_POSITION|HW_IF_VELOCITY|HW_IF_EFFORT} mapping
+ * between custom interface names and standard names in sensor_msgs::msg::JointState message.
  *
  * Publishes to:
  * - \b joint_states (sensor_msgs::msg::JointState): Joint states related to movement
@@ -89,6 +94,7 @@ class JointStateBroadcaster : public controller_interface::ControllerInterface
   bool use_local_topics_;
   std::vector<std::string> joints_;
   std::vector<std::string> interfaces_;
+  std::unordered_map<std::string, std::string> map_interface_to_joint_state_;
 
   //  For the JointState message,
   //  we store the name of joints with compatible interfaces

joint_state_broadcaster/src/joint_state_broadcaster.cpp

@@ -53,6 +53,12 @@ CallbackReturn JointStateBroadcaster::on_init()
     auto_declare<bool>("use_local_topics", false);
     auto_declare<std::vector<std::string>>("joints", std::vector<std::string>({}));
     auto_declare<std::vector<std::string>>("interfaces", std::vector<std::string>({}));
+    auto_declare<std::string>(
+      std::string("map_interface_to_joint_state.") + HW_IF_POSITION, HW_IF_POSITION);
+    auto_declare<std::string>(
+      std::string("map_interface_to_joint_state.") + HW_IF_VELOCITY, HW_IF_VELOCITY);
+    auto_declare<std::string>(
+      std::string("map_interface_to_joint_state.") + HW_IF_EFFORT, HW_IF_EFFORT);
   }
   catch (const std::exception & e)
   {
@@ -104,17 +110,45 @@ CallbackReturn JointStateBroadcaster::on_configure(
   if (use_all_available_interfaces())
   {
     RCLCPP_INFO(
-      node_->get_logger(),
+      get_node()->get_logger(),
       "'joints' or 'interfaces' parameter is empty. "
       "All available state interfaces will be published");
+    joints_.clear();
+    interfaces_.clear();
   }
   else
   {
     RCLCPP_INFO(
-      node_->get_logger(),
+      get_node()->get_logger(),
       "Publishing state interfaces defined in 'joints' and 'interfaces' parameters.");
   }
 
+  auto get_map_interface_parameter = [&](const std::string & interface) {
+    std::string interface_to_map =
+      get_node()
+        ->get_parameter(std::string("map_interface_to_joint_state.") + interface)
+        .as_string();
+
+    if (std::find(interfaces_.begin(), interfaces_.end(), interface) != 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());
+    }
+    else
+    {
+      map_interface_to_joint_state_[interface_to_map] = interface;
+    }
+  };
+
+  map_interface_to_joint_state_ = {};
+  get_map_interface_parameter(HW_IF_POSITION);
+  get_map_interface_parameter(HW_IF_VELOCITY);
+  get_map_interface_parameter(HW_IF_EFFORT);
+
   try
   {
     const std::string topic_name_prefix = use_local_topics_ ? "~/" : "";
@@ -200,7 +234,12 @@ bool JointStateBroadcaster::init_joint_data()
       name_if_value_mapping_[si->get_name()] = {};
     }
     // add interface name
-    name_if_value_mapping_[si->get_name()][si->get_interface_name()] = kUninitializedValue;
+    std::string interface_name = si->get_interface_name();
+    if (map_interface_to_joint_state_.count(interface_name) > 0)
+    {
+      interface_name = map_interface_to_joint_state_[interface_name];
+    }
+    name_if_value_mapping_[si->get_name()][interface_name] = kUninitializedValue;
   }
 
   // filter state interfaces that have at least one of the joint_states fields,
@@ -291,11 +330,16 @@ controller_interface::return_type JointStateBroadcaster::update(
 {
   for (const auto & state_interface : state_interfaces_)
   {
-    name_if_value_mapping_[state_interface.get_name()][state_interface.get_interface_name()] =
+    std::string interface_name = state_interface.get_interface_name();
+    if (map_interface_to_joint_state_.count(interface_name) > 0)
+    {
+      interface_name = map_interface_to_joint_state_[interface_name];
+    }
+    name_if_value_mapping_[state_interface.get_name()][interface_name] =
       state_interface.get_value();
     RCLCPP_DEBUG(
-      get_node()->get_logger(), "%s/%s: %f\n", state_interface.get_name().c_str(),
-      state_interface.get_interface_name().c_str(), state_interface.get_value());
+      get_node()->get_logger(), "%s: %f\n", state_interface.get_full_name().c_str(),
+      state_interface.get_value());
   }
 
   joint_state_msg_.header.stamp = time;

joint_state_broadcaster/test/test_joint_state_broadcaster.cpp

@@ -145,6 +145,10 @@ void JointStateBroadcasterTest::assign_state_interfaces(
         {
           state_ifs.emplace_back(joint_3_eff_state_);
         }
+        if (interface == custom_interface_name_)
+        {
+          state_ifs.emplace_back(joint_X_custom_state);
+        }
       }
     }
   }
@@ -324,12 +328,9 @@ TEST_F(JointStateBroadcasterTest, ActivateTestTwoJointsOneInterface)
   SetUpStateBroadcaster(JOINT_NAMES, IF_NAMES);
 
   // configure ok
-  std::cout << "Now will try to configure controller, ..." << std::endl;
   ASSERT_EQ(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
 
-  std::cout << "Now will try to activate controller, ..." << std::endl;
   ASSERT_EQ(state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
-  std::cout << "After activation, ..." << std::endl;
 
   const size_t NUM_JOINTS = JOINT_NAMES.size();
 
@@ -473,6 +474,132 @@ TEST_F(JointStateBroadcasterTest, ActivateTestTwoJointTwoInterfacesOneMissing)
   ASSERT_TRUE(state_broadcaster_->dynamic_joint_state_publisher_);
 }
 
+TEST_F(JointStateBroadcasterTest, TestCustomInterfaceWithoutMapping)
+{
+  const std::vector<std::string> JOINT_NAMES = {joint_names_[0]};
+  const std::vector<std::string> IF_NAMES = {custom_interface_name_};
+  SetUpStateBroadcaster(JOINT_NAMES, IF_NAMES);
+
+  // configure ok
+  ASSERT_EQ(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
+
+  ASSERT_EQ(state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
+
+  const size_t NUM_JOINTS = JOINT_NAMES.size();
+
+  // joint state initialized
+  ASSERT_THAT(state_broadcaster_->joint_state_msg_.name, SizeIs(0));
+  ASSERT_THAT(state_broadcaster_->joint_state_msg_.position, SizeIs(0));
+  ASSERT_THAT(state_broadcaster_->joint_state_msg_.velocity, SizeIs(0));
+  ASSERT_THAT(state_broadcaster_->joint_state_msg_.effort, SizeIs(0));
+
+  // dynamic joint state initialized
+  ASSERT_THAT(state_broadcaster_->dynamic_joint_state_msg_.joint_names, SizeIs(NUM_JOINTS));
+  ASSERT_THAT(state_broadcaster_->dynamic_joint_state_msg_.interface_values, SizeIs(NUM_JOINTS));
+  ASSERT_THAT(
+    state_broadcaster_->dynamic_joint_state_msg_.joint_names, ElementsAreArray(JOINT_NAMES));
+  ASSERT_THAT(
+    state_broadcaster_->dynamic_joint_state_msg_.interface_values[0].interface_names,
+    ElementsAreArray(IF_NAMES));
+
+  // publishers initialized
+  ASSERT_TRUE(state_broadcaster_->joint_state_publisher_);
+  ASSERT_TRUE(state_broadcaster_->dynamic_joint_state_publisher_);
+}
+
+TEST_F(JointStateBroadcasterTest, TestCustomInterfaceMapping)
+{
+  const std::vector<std::string> JOINT_NAMES = {joint_names_[0]};
+  const std::vector<std::string> IF_NAMES = {custom_interface_name_};
+  SetUpStateBroadcaster(JOINT_NAMES, IF_NAMES);
+
+  state_broadcaster_->get_node()->set_parameter(
+    {std::string("map_interface_to_joint_state.") + HW_IF_POSITION, custom_interface_name_});
+
+  // configure ok
+  ASSERT_EQ(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
+
+  ASSERT_EQ(state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
+
+  const size_t NUM_JOINTS = JOINT_NAMES.size();
+
+  // joint state initialized
+  ASSERT_THAT(state_broadcaster_->joint_state_msg_.name, ElementsAreArray(JOINT_NAMES));
+  ASSERT_THAT(state_broadcaster_->joint_state_msg_.position, SizeIs(NUM_JOINTS));
+  ASSERT_THAT(state_broadcaster_->joint_state_msg_.velocity, SizeIs(NUM_JOINTS));
+  for (auto i = 0ul; i < NUM_JOINTS; ++i)
+  {
+    ASSERT_TRUE(std::isnan(state_broadcaster_->joint_state_msg_.velocity[i]));
+  }
+  ASSERT_THAT(state_broadcaster_->joint_state_msg_.effort, SizeIs(NUM_JOINTS));
+  for (auto i = 0ul; i < NUM_JOINTS; ++i)
+  {
+    ASSERT_TRUE(std::isnan(state_broadcaster_->joint_state_msg_.effort[i]));
+  }
+
+  // dynamic joint state initialized
+  ASSERT_THAT(state_broadcaster_->dynamic_joint_state_msg_.joint_names, SizeIs(NUM_JOINTS));
+  ASSERT_THAT(state_broadcaster_->dynamic_joint_state_msg_.interface_values, SizeIs(NUM_JOINTS));
+  ASSERT_THAT(
+    state_broadcaster_->dynamic_joint_state_msg_.joint_names, ElementsAreArray(JOINT_NAMES));
+  ASSERT_THAT(
+    state_broadcaster_->dynamic_joint_state_msg_.interface_values[0].interface_names,
+    ElementsAreArray({HW_IF_POSITION}));  // mapping to this value
+
+  // publishers initialized
+  ASSERT_TRUE(state_broadcaster_->joint_state_publisher_);
+  ASSERT_TRUE(state_broadcaster_->dynamic_joint_state_publisher_);
+}
+
+TEST_F(JointStateBroadcasterTest, TestCustomInterfaceMappingUpdate)
+{
+  const std::vector<std::string> JOINT_NAMES = {joint_names_[0]};
+  const std::vector<std::string> IF_NAMES = {custom_interface_name_};
+  SetUpStateBroadcaster(JOINT_NAMES, IF_NAMES);
+
+  state_broadcaster_->get_node()->set_parameter(
+    {std::string("map_interface_to_joint_state.") + HW_IF_POSITION, custom_interface_name_});
+
+  // configure ok
+  ASSERT_EQ(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
+
+  ASSERT_EQ(state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
+
+  ASSERT_EQ(
+    state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
+    controller_interface::return_type::OK);
+
+  const size_t NUM_JOINTS = JOINT_NAMES.size();
+
+  // joint state initialized
+  ASSERT_THAT(state_broadcaster_->joint_state_msg_.name, ElementsAreArray(JOINT_NAMES));
+  ASSERT_THAT(state_broadcaster_->joint_state_msg_.position, SizeIs(NUM_JOINTS));
+  ASSERT_EQ(state_broadcaster_->joint_state_msg_.position[0], custom_joint_value_);
+  ASSERT_THAT(state_broadcaster_->joint_state_msg_.velocity, SizeIs(NUM_JOINTS));
+  for (auto i = 0ul; i < NUM_JOINTS; ++i)
+  {
+    ASSERT_TRUE(std::isnan(state_broadcaster_->joint_state_msg_.velocity[i]));
+  }
+  ASSERT_THAT(state_broadcaster_->joint_state_msg_.effort, SizeIs(NUM_JOINTS));
+  for (auto i = 0ul; i < NUM_JOINTS; ++i)
+  {
+    ASSERT_TRUE(std::isnan(state_broadcaster_->joint_state_msg_.effort[i]));
+  }
+
+  // dynamic joint state initialized
+  ASSERT_THAT(state_broadcaster_->dynamic_joint_state_msg_.joint_names, SizeIs(NUM_JOINTS));
+  ASSERT_THAT(state_broadcaster_->dynamic_joint_state_msg_.interface_values, SizeIs(NUM_JOINTS));
+  ASSERT_THAT(
+    state_broadcaster_->dynamic_joint_state_msg_.joint_names, ElementsAreArray(JOINT_NAMES));
+  ASSERT_THAT(
+    state_broadcaster_->dynamic_joint_state_msg_.interface_values[0].interface_names,
+    ElementsAreArray({HW_IF_POSITION}));  // mapping to this value
+
+  // publishers initialized
+  ASSERT_TRUE(state_broadcaster_->joint_state_publisher_);
+  ASSERT_TRUE(state_broadcaster_->dynamic_joint_state_publisher_);
+}
+
 TEST_F(JointStateBroadcasterTest, UpdateTest)
 {
   SetUpStateBroadcaster();

joint_state_broadcaster/test/test_joint_state_broadcaster.hpp

@@ -40,6 +40,9 @@ class FriendJointStateBroadcaster : public joint_state_broadcaster::JointStateBr
   FRIEND_TEST(JointStateBroadcasterTest, ActivateTestOneJointTwoInterfaces);
   FRIEND_TEST(JointStateBroadcasterTest, ActivateTestTwoJointTwoInterfacesAllMissing);
   FRIEND_TEST(JointStateBroadcasterTest, ActivateTestTwoJointTwoInterfacesOneMissing);
+  FRIEND_TEST(JointStateBroadcasterTest, TestCustomInterfaceWithoutMapping);
+  FRIEND_TEST(JointStateBroadcasterTest, TestCustomInterfaceMapping);
+  FRIEND_TEST(JointStateBroadcasterTest, TestCustomInterfaceMappingUpdate);
   FRIEND_TEST(JointStateBroadcasterTest, ExtraJointStatePublishTest);
 };
 
@@ -72,7 +75,9 @@ class JointStateBroadcasterTest : public ::testing::Test
   // dummy joint state values used for tests
   const std::vector<std::string> joint_names_ = {"joint1", "joint2", "joint3"};
   const std::vector<std::string> interface_names_ = {HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_EFFORT};
+  std::string custom_interface_name_ = "measured_position";
   std::vector<double> joint_values_ = {1.1, 2.1, 3.1};
+  double custom_joint_value_ = 3.5;
 
   hardware_interface::StateInterface joint_1_pos_state_{
     joint_names_[0], interface_names_[0], &joint_values_[0]};
@@ -93,6 +98,9 @@ class JointStateBroadcasterTest : public ::testing::Test
   hardware_interface::StateInterface joint_3_eff_state_{
     joint_names_[2], interface_names_[2], &joint_values_[2]};
 
+  hardware_interface::StateInterface joint_X_custom_state{
+    joint_names_[0], custom_interface_name_, &custom_joint_value_};
+
   std::unique_ptr<FriendJointStateBroadcaster> state_broadcaster_;
 };