init catch

lbr-stack · May 9, 2024 · dd17a0e · dd17a0e
1 parent ff8582b
commit dd17a0e
Showing 1 changed file with 4 additions and 0 deletions.
diff --git a/lbr_ros2_control/src/system_interface.cpp b/lbr_ros2_control/src/system_interface.cpp
@@ -292,6 +292,10 @@ SystemInterface::on_deactivate(const rclcpp_lifecycle::State &) {
 
 hardware_interface::return_type SystemInterface::read(const rclcpp::Time & /*time*/,
                                                       const rclcpp::Duration &period) {
+  if (!async_client_ptr_->get_state_interface().is_initialized()) {
+    return hardware_interface::return_type::OK;
+  }
+
   hw_lbr_state_ = async_client_ptr_->get_state_interface().get_state();
 
   if (period.seconds() - hw_lbr_state_.sample_time * 0.2 > hw_lbr_state_.sample_time) {