From 1c4d58e55efd86630451eb615ade5d1f7da7e952 Mon Sep 17 00:00:00 2001 From: Takashi Sato Date: Sun, 25 Aug 2024 01:55:15 +0900 Subject: [PATCH] [JSB] Move the initialize of urdf::Model from on_activate to on_configure to improve real-time performance (#1269) --- .../joint_state_broadcaster.hpp | 4 +++ .../src/joint_state_broadcaster.cpp | 27 ++++++++++--------- 2 files changed, 18 insertions(+), 13 deletions(-) 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 b3fa69f94c..ecc3c767f6 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 @@ -27,6 +27,7 @@ #include "joint_state_broadcaster_parameters.hpp" #include "realtime_tools/realtime_publisher.h" #include "sensor_msgs/msg/joint_state.hpp" +#include "urdf/model.h" namespace joint_state_broadcaster { @@ -111,6 +112,9 @@ class JointStateBroadcaster : public controller_interface::ControllerInterface dynamic_joint_state_publisher_; std::shared_ptr> realtime_dynamic_joint_state_publisher_; + + urdf::Model model_; + bool is_model_loaded_ = false; }; } // 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 209b44e557..fe0b32213a 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -164,6 +164,18 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_configure( fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); return CallbackReturn::ERROR; } + + const std::string & urdf = get_robot_description(); + + is_model_loaded_ = !urdf.empty() && model_.initString(urdf); + if (!is_model_loaded_) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Failed to parse robot description. Will publish all the interfaces with '%s', '%s' and '%s'", + HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_EFFORT); + } + return CallbackReturn::SUCCESS; } @@ -243,17 +255,6 @@ bool JointStateBroadcaster::init_joint_data() name_if_value_mapping_[si->get_prefix_name()][interface_name] = kUninitializedValue; } - const std::string & urdf = get_robot_description(); - - urdf::Model model; - const bool is_model_loaded = !urdf.empty() && model.initString(urdf); - if (!is_model_loaded) - { - RCLCPP_ERROR( - get_node()->get_logger(), - "Failed to parse robot description. Will publish all the interfaces with '%s', '%s' and '%s'", - HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_EFFORT); - } // filter state interfaces that have at least one of the joint_states fields, // the rest will be ignored for this message for (const auto & name_ifv : name_if_value_mapping_) @@ -262,8 +263,8 @@ bool JointStateBroadcaster::init_joint_data() if (has_any_key(interfaces_and_values, {HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_EFFORT})) { if ( - !params_.use_urdf_to_filter || !params_.joints.empty() || !is_model_loaded || - model.getJoint(name_ifv.first)) + !params_.use_urdf_to_filter || !params_.joints.empty() || !is_model_loaded_ || + model_.getJoint(name_ifv.first)) { joint_names_.push_back(name_ifv.first); }