Skip to content

Commit

Permalink
[JSB] Move the initialize of urdf::Model from on_activate to on_confi…
Browse files Browse the repository at this point in the history
…gure to improve real-time performance (#1269)
  • Loading branch information
TakashiSato authored Aug 24, 2024
1 parent 20f6f0b commit 1c4d58e
Show file tree
Hide file tree
Showing 2 changed files with 18 additions and 13 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand Down Expand Up @@ -111,6 +112,9 @@ class JointStateBroadcaster : public controller_interface::ControllerInterface
dynamic_joint_state_publisher_;
std::shared_ptr<realtime_tools::RealtimePublisher<control_msgs::msg::DynamicJointState>>
realtime_dynamic_joint_state_publisher_;

urdf::Model model_;
bool is_model_loaded_ = false;
};

} // namespace joint_state_broadcaster
Expand Down
27 changes: 14 additions & 13 deletions joint_state_broadcaster/src/joint_state_broadcaster.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}

Expand Down Expand Up @@ -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_)
Expand All @@ -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);
}
Expand Down

0 comments on commit 1c4d58e

Please sign in to comment.