Fix joint_state_broadcaster performance issues

joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp

@@ -86,6 +86,7 @@ class JointStateBroadcaster : public controller_interface::ControllerInterface
 
 protected:
   bool init_joint_data();
+  void init_auxiliary_data();
   void init_joint_state_msg();
   void init_dynamic_joint_state_msg();
   bool use_all_available_interfaces() const;
@@ -113,6 +114,24 @@ class JointStateBroadcaster : public controller_interface::ControllerInterface
 
   urdf::Model model_;
   bool is_model_loaded_ = false;
+
+  std::vector<double *> mapped_values_;
+
+  struct JointStateData
+  {
+    JointStateData(const double & position, const double & velocity, const double & effort)
+    : position_(position), velocity_(velocity), effort_(effort)
+    {
+    }
+
+    const double & position_;
+    const double & velocity_;
+    const double & effort_;
+  };
+
+  std::vector<JointStateData> joint_states_data_;
+
+  std::vector<std::vector<const double *>> dynamic_joint_states_data_;
 };
 
 }  // namespace joint_state_broadcaster

joint_state_broadcaster/src/joint_state_broadcaster.cpp

@@ -195,6 +195,7 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_activate(
     return CallbackReturn::ERROR;
   }
 
+  init_auxiliary_data();
   init_joint_state_msg();
   init_dynamic_joint_state_msg();
 
@@ -287,6 +288,22 @@ bool JointStateBroadcaster::init_joint_data()
   return true;
 }
 
+void JointStateBroadcaster::init_auxiliary_data()
+{
+  // save the mapping of state interfaces to joint states
+  mapped_values_.clear();
+  for (auto i = 0u; i < state_interfaces_.size(); ++i)
+  {
+    std::string interface_name = state_interfaces_[i].get_interface_name();
+    if (map_interface_to_joint_state_.count(interface_name) > 0)
+    {
+      interface_name = map_interface_to_joint_state_[interface_name];
+    }
+    mapped_values_.push_back(
+      &name_if_value_mapping_[state_interfaces_[i].get_prefix_name()][interface_name]);
+  }
+}
+
 void JointStateBroadcaster::init_joint_state_msg()
 {
   const size_t num_joints = joint_names_.size();
@@ -300,6 +317,30 @@ void JointStateBroadcaster::init_joint_state_msg()
   joint_state_msg.position.resize(num_joints, kUninitializedValue);
   joint_state_msg.velocity.resize(num_joints, kUninitializedValue);
   joint_state_msg.effort.resize(num_joints, kUninitializedValue);
+
+  // save joint state data
+  auto get_address =
+    [&](const std::string & joint_name, const std::string & interface_name) -> const double &
+  {
+    const auto & interfaces_and_values = name_if_value_mapping_.at(joint_name);
+    const auto interface_and_value = interfaces_and_values.find(interface_name);
+    if (interface_and_value != interfaces_and_values.cend())
+    {
+      return interface_and_value->second;
+    }
+    else
+    {
+      return kUninitializedValue;
+    }
+  };
+
+  joint_states_data_.clear();
+  for (auto i = 0u; i < joint_names_.size(); ++i)
+  {
+    joint_states_data_.push_back(JointStateData(
+      get_address(joint_names_[i], HW_IF_POSITION), get_address(joint_names_[i], HW_IF_VELOCITY),
+      get_address(joint_names_[i], HW_IF_EFFORT)));
+  }
 }
 
 void JointStateBroadcaster::init_dynamic_joint_state_msg()
@@ -320,6 +361,22 @@ void JointStateBroadcaster::init_dynamic_joint_state_msg()
     }
     dynamic_joint_state_msg.interface_values.emplace_back(if_value);
   }
+
+  // save dynamic joint state data
+  dynamic_joint_states_data_.clear();
+  const auto & msg = realtime_dynamic_joint_state_publisher_->msg_;
+  for (auto ji = 0u; ji < msg.joint_names.size(); ++ji)
+  {
+    dynamic_joint_states_data_.push_back(std::vector<const double *>());
+
+    const auto & name = msg.joint_names[ji];
+
+    for (auto ii = 0u; ii < msg.interface_values[ji].interface_names.size(); ++ii)
+    {
+      const auto & interface_name = msg.interface_values[ji].interface_names[ii];
+      dynamic_joint_states_data_[ji].push_back(&name_if_value_mapping_[name][interface_name]);
+    }
+  }
 }
 
 bool JointStateBroadcaster::use_all_available_interfaces() const
@@ -346,18 +403,14 @@ double get_value(
 controller_interface::return_type JointStateBroadcaster::update(
   const rclcpp::Time & time, const rclcpp::Duration & /*period*/)
 {
-  for (const auto & state_interface : state_interfaces_)
+  for (auto i = 0u; i < state_interfaces_.size(); ++i)
   {
-    std::string interface_name = state_interface.get_interface_name();
-    if (map_interface_to_joint_state_.count(interface_name) > 0)
+    // no retries, just try to get the latest value once
+    const auto & opt = state_interfaces_[i].get_optional(0);
+    if (opt.has_value())
     {
-      interface_name = map_interface_to_joint_state_[interface_name];
+      *mapped_values_[i] = opt.value();
     }
-    name_if_value_mapping_[state_interface.get_prefix_name()][interface_name] =
-      state_interface.get_value();
-    RCLCPP_DEBUG(
-      get_node()->get_logger(), "%s: %f\n", state_interface.get_name().c_str(),
-      state_interface.get_value());
   }
 
   if (realtime_joint_state_publisher_ && realtime_joint_state_publisher_->trylock())
@@ -369,32 +422,22 @@ controller_interface::return_type JointStateBroadcaster::update(
     // update joint state message and dynamic joint state message
     for (size_t i = 0; i < joint_names_.size(); ++i)
     {
-      joint_state_msg.position[i] =
-        get_value(name_if_value_mapping_, joint_names_[i], HW_IF_POSITION);
-      joint_state_msg.velocity[i] =
-        get_value(name_if_value_mapping_, joint_names_[i], HW_IF_VELOCITY);
-      joint_state_msg.effort[i] = get_value(name_if_value_mapping_, joint_names_[i], HW_IF_EFFORT);
+      joint_state_msg.position[i] = joint_states_data_[i].position_;
+      joint_state_msg.velocity[i] = joint_states_data_[i].velocity_;
+      joint_state_msg.effort[i] = joint_states_data_[i].effort_;
     }
     realtime_joint_state_publisher_->unlockAndPublish();
   }
 
   if (realtime_dynamic_joint_state_publisher_ && realtime_dynamic_joint_state_publisher_->trylock())
   {
-    auto & dynamic_joint_state_msg = realtime_dynamic_joint_state_publisher_->msg_;
-    dynamic_joint_state_msg.header.stamp = time;
-    for (size_t joint_index = 0; joint_index < dynamic_joint_state_msg.joint_names.size();
-         ++joint_index)
+    auto & msg = realtime_dynamic_joint_state_publisher_->msg_;
+    msg.header.stamp = time;
+    for (auto ji = 0u; ji < msg.joint_names.size(); ++ji)
     {
-      const auto & name = dynamic_joint_state_msg.joint_names[joint_index];
-      for (size_t interface_index = 0;
-           interface_index <
-           dynamic_joint_state_msg.interface_values[joint_index].interface_names.size();
-           ++interface_index)
+      for (auto ii = 0u; ii < msg.interface_values[ji].interface_names.size(); ++ii)
       {
-        const auto & interface_name =
-          dynamic_joint_state_msg.interface_values[joint_index].interface_names[interface_index];
-        dynamic_joint_state_msg.interface_values[joint_index].values[interface_index] =
-          name_if_value_mapping_[name][interface_name];
+        msg.interface_values[ji].values[ii] = *dynamic_joint_states_data_[ji][ii];
       }
     }
     realtime_dynamic_joint_state_publisher_->unlockAndPublish();

joint_state_broadcaster/test/test_joint_state_broadcaster.cpp

@@ -942,6 +942,119 @@ TEST_F(JointStateBroadcasterTest, UpdateTest)
     controller_interface::return_type::OK);
 }
 
+TEST_F(JointStateBroadcasterTest, UpdatePerformanceTest)
+{
+  const auto result = state_broadcaster_->init(
+    "joint_state_broadcaster", "", 0, "", state_broadcaster_->define_custom_node_options());
+  ASSERT_EQ(result, controller_interface::return_type::OK);
+
+  custom_joint_value_ = 12.34;
+
+  // build our own test interfaces: robot has ~500 state interfaces
+  for (auto joint = 1u; joint < 30; ++joint)
+  {
+    const auto joint_name = "joint_" + std::to_string(joint);
+
+    // standard
+    test_interfaces_.emplace_back(
+      hardware_interface::StateInterface{joint_name, "position", &custom_joint_value_});
+    test_interfaces_.emplace_back(
+      hardware_interface::StateInterface{joint_name, "velocity", &custom_joint_value_});
+    test_interfaces_.emplace_back(
+      hardware_interface::StateInterface{joint_name, "effort", &custom_joint_value_});
+
+    // non standard
+    test_interfaces_.emplace_back(
+      hardware_interface::StateInterface{joint_name, "mode", &custom_joint_value_});
+    test_interfaces_.emplace_back(
+      hardware_interface::StateInterface{joint_name, "absolute_position", &custom_joint_value_});
+    test_interfaces_.emplace_back(
+      hardware_interface::StateInterface{joint_name, "acceleration", &custom_joint_value_});
+    test_interfaces_.emplace_back(
+      hardware_interface::StateInterface{joint_name, "current", &custom_joint_value_});
+    test_interfaces_.emplace_back(
+      hardware_interface::StateInterface{joint_name, "torque", &custom_joint_value_});
+    test_interfaces_.emplace_back(
+      hardware_interface::StateInterface{joint_name, "force", &custom_joint_value_});
+    test_interfaces_.emplace_back(
+      hardware_interface::StateInterface{joint_name, "temperature_board", &custom_joint_value_});
+    test_interfaces_.emplace_back(
+      hardware_interface::StateInterface{joint_name, "temperature_motor", &custom_joint_value_});
+
+    test_interfaces_.emplace_back(
+      hardware_interface::StateInterface{joint_name, "position.kd", &custom_joint_value_});
+    test_interfaces_.emplace_back(
+      hardware_interface::StateInterface{joint_name, "position.ki", &custom_joint_value_});
+    test_interfaces_.emplace_back(
+      hardware_interface::StateInterface{joint_name, "position.kp", &custom_joint_value_});
+
+    test_interfaces_.emplace_back(
+      hardware_interface::StateInterface{joint_name, "velocity.kd", &custom_joint_value_});
+    test_interfaces_.emplace_back(
+      hardware_interface::StateInterface{joint_name, "velocity.ki", &custom_joint_value_});
+    test_interfaces_.emplace_back(
+      hardware_interface::StateInterface{joint_name, "velocity.kp", &custom_joint_value_});
+
+    test_interfaces_.emplace_back(
+      hardware_interface::StateInterface{joint_name, "current.kd", &custom_joint_value_});
+    test_interfaces_.emplace_back(
+      hardware_interface::StateInterface{joint_name, "current.ki", &custom_joint_value_});
+    test_interfaces_.emplace_back(
+      hardware_interface::StateInterface{joint_name, "current.kp", &custom_joint_value_});
+  }
+
+  RCLCPP_INFO(
+    state_broadcaster_->get_node()->get_logger(), "Number of test interfaces: %lu",
+    test_interfaces_.size());
+
+  std::vector<LoanedStateInterface> state_interfaces;
+  for (const auto & tif : test_interfaces_)
+  {
+    state_interfaces.emplace_back(tif);
+  }
+
+  state_broadcaster_->assign_interfaces({}, std::move(state_interfaces));
+
+  auto node_state = state_broadcaster_->get_node()->configure();
+  node_state = state_broadcaster_->get_node()->activate();
+  ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
+
+  if (!realtime_tools::configure_sched_fifo(50))
+  {
+    RCLCPP_WARN(
+      state_broadcaster_->get_node()->get_logger(),
+      "Could not enable FIFO RT scheduling policy: with error number <%i>(%s)", errno,
+      strerror(errno));
+  }
+
+  constexpr auto kNumSamples = 10000u;
+  std::vector<int64_t> measures;
+  for (auto i = 0u; i < kNumSamples; ++i)
+  {
+    const auto now = std::chrono::steady_clock::now();
+
+    ASSERT_EQ(
+      state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
+      controller_interface::return_type::OK);
+
+    // print time taken
+    const auto elapsed = std::chrono::steady_clock::now() - now;
+    const auto elapsed_us = std::chrono::duration_cast<std::chrono::microseconds>(elapsed);
+
+    measures.push_back(elapsed_us.count());
+  }
+
+  const auto average =
+    std::accumulate(measures.begin(), measures.end(), 0.0) / static_cast<double>(measures.size());
+  const auto variance = std::accumulate(
+                          measures.begin(), measures.end(), 0.0, [average](double accum, double x)
+                          { return accum + (x - average) * (x - average); }) /
+                        static_cast<double>(measures.size());
+
+  RCLCPP_INFO(state_broadcaster_->get_node()->get_logger(), "Average update time: %lf us", average);
+  RCLCPP_INFO(state_broadcaster_->get_node()->get_logger(), "Variance: %lf us", variance);
+}
+
 void JointStateBroadcasterTest::activate_and_get_joint_state_message(
   const std::string & topic, sensor_msgs::msg::JointState & joint_state_msg)
 {

joint_state_broadcaster/test/test_joint_state_broadcaster.hpp

@@ -109,6 +109,8 @@ class JointStateBroadcasterTest : public ::testing::Test
   hardware_interface::StateInterface joint_X_custom_state{
     joint_names_[0], custom_interface_name_, &custom_joint_value_};
 
+  std::vector<hardware_interface::StateInterface> test_interfaces_;
+
   std::unique_ptr<FriendJointStateBroadcaster> state_broadcaster_;
 };