Merge pull request #37 from AGH-CEAI/bugfix/wait_activation

Wait for ready state before starting communication

Author: sivral

robotiq_hande_driver/CHANGELOG.md

@@ -14,6 +14,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 ### Fixed
 
+* [PR-37](https://github.com/AGH-CEAI/robotiq_hande_driver/pull/37) - Fixed missing readiness check during activation.
 * [PR-34](https://github.com/AGH-CEAI/robotiq_hande_driver/pull/34) - Fixed check for dropping request to set the same gripper position.
 * [PR-34](https://github.com/AGH-CEAI/robotiq_hande_driver/pull/34) - Fixed non-deterministic control node's frequency hicups by removing call to the modbus write in time-critical code block.
 * [PR-33](https://github.com/AGH-CEAI/robotiq_hande_driver/pull/33) - Fixed missing ros2_control_test_assets dependency in CMakeLists.txt that causes build failures after ROS2 Humble package updates.

robotiq_hande_driver/hardware/src/hande_hardware_interface.cpp

@@ -186,6 +186,12 @@ HWI::CallbackReturn RobotiqHandeHardwareInterface::on_activate(
             th_comm_enabled_.store(true, std::memory_order_relaxed);
             th_comm_.emplace(&RobotiqHandeHardwareInterface::gripper_communication, this);
         }
+
+        RCLCPP_INFO(
+            get_logger(), "%sWaiting for Hand-E complete activation%s", color::BCYAN, color::RESET);
+        while(!gripper_driver_.get_status().is_ready) {
+            std::this_thread::sleep_for(th_sleep_rate_);
+        }
     } catch(const std::exception& e) {
         RCLCPP_ERROR(
             get_logger(),