diff --git a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp index 2ffd28e0f5..f12c192831 100644 --- a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp +++ b/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 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 joint_states_data_; + + std::vector> dynamic_joint_states_data_; }; } // namespace joint_state_broadcaster diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index 2890617f50..c6857cf245 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/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 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(); diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index 370965e99e..9dbf3f2e06 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/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 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 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(elapsed); + + measures.push_back(elapsed_us.count()); + } + + const auto average = + std::accumulate(measures.begin(), measures.end(), 0.0) / static_cast(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(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) { diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp index 4f4241d3d7..a167d33bf5 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp +++ b/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 test_interfaces_; + std::unique_ptr state_broadcaster_; };