Merge pull request #11 from AGH-CEAI/feature/connection_error_handling

ADD: reconnecting to modbus on configure

Author: macmacal

robotiq_hande_driver/CHANGELOG.md

@@ -9,6 +9,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 ### Added
 
+* [PR-11](https://github.com/AGH-CEAI/robotiq_hande_driver/pull/11) - Added automatic reconnection to Modbus during configuration.
 * [PR-7](https://github.com/AGH-CEAI/robotiq_hande_driver/pull/7) - Integration of ModbusRTU communication with ros2_control Hardware Interface
 * [PR-2](https://github.com/AGH-CEAI/robotiq_hande_driver/pull/2) - ModbusRTU communication
 * [PR-2](https://github.com/AGH-CEAI/robotiq_hande_driver/pull/2) - Protocol abstraction
@@ -22,4 +23,6 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 ### Fixed
 
+* [PR-11](https://github.com/AGH-CEAI/robotiq_hande_driver/pull/11) - Fixed an issue where the Modbus connection check always failed.
+
 ### Security

robotiq_hande_driver/hardware/include/robotiq_hande_driver/communication.hpp

@@ -136,7 +136,7 @@ class Communication {
      * @note The status should be checked to verify successful execution. An exception is thrown if
      * communication issues occur.
      */
-    void read_write_registers() {
+    int read_write_registers() {
         /**
          * @brief Read and write modbus registers at once
          *
@@ -147,18 +147,22 @@ class Communication {
          * @param int read_addr
          * @param int read_nb
          * @param uint16_t *dest
-         * @return int >0 on success
+         * @return int FAILURE_MODBUS on failure, nonnegative value for success
          * @note The status should be checked to verify successful execution. An exception is thrown
          * if communication issues occur.
          */
-        modbus_write_and_read_registers(
+        int result;
+
+        result = modbus_write_and_read_registers(
             mb_,
             GRIPPER_INPUT_FIRST_REG,
             OUTPUT_REGISTER_WORD_LENGTH,
             reinterpret_cast<uint16_t*>(output_bytes_),
             GRIPPER_OUTPUT_FIRST_REG,
             INPUT_REGISTER_WORD_LENGTH,
             reinterpret_cast<uint16_t*>(input_bytes_));
+
+        return result;
     };
 
     /**

robotiq_hande_driver/hardware/src/communication.cpp

@@ -7,12 +7,11 @@ namespace robotiq_hande_driver {
 Communication::Communication() : input_bytes_{}, output_bytes_{} {}
 
 int Communication::connect() {
-    uint16_t activation_status[1] = {0x0000};
     int result;
 
     modbus_connect(mb_);
 
-    result = modbus_read_registers(mb_, GRIPPER_OUTPUT_FIRST_REG, 1, activation_status);
+    result = read_write_registers();
 
     return result;
 }

robotiq_hande_driver/hardware/src/hande_hardware_interface.cpp

@@ -7,6 +7,7 @@ namespace robotiq_hande_driver {
 
 static constexpr auto THROTTLE_1000_MS = 1000;
 static constexpr auto ACTIVATION_MAX_ITER = 100;
+static constexpr auto RECONNECT_MAX_ITER = 10;
 inline void wait_100ms() {
     usleep(100 * 1000);
 }
@@ -70,24 +71,37 @@ HWI::CallbackReturn RobotiqHandeHardwareInterface::on_init(const HWI::HardwareIn
 
 HWI::CallbackReturn RobotiqHandeHardwareInterface::on_configure(
     const rlccp_lc::State& /*previous_state*/) {
-    int result;
+    int result = FAILURE_MODBUS;
 
-    RCLCPP_INFO(
+    RCLCPP_INFO(get_logger(), "Connecting to ModbusRTU");
+    RCLCPP_DEBUG(
         get_logger(),
         "Connecting to tty:%s, baudrate:%d, parity:%c, data_bits:%d, stop_bit:%d",
         tty_port_.c_str(),
         baudrate_,
         parity_,
         data_bits_,
         stop_bit_);
+
     result = gripper_driver_.configure();
 
-    // TODO(issue#10) Modbus connection check always fails
-    // if(result == FAILURE_MODBUS) {
-    //     RCLCPP_INFO(get_logger(), "Failed to configure Hand-E Gripper");
-    //     return HWI::CallbackReturn::FAILURE;
-    // }
-    RCLCPP_INFO(get_logger(), "Configured Hand-E Gripper: %d", result);
+    for(int iter = 0; result == FAILURE_MODBUS && iter < RECONNECT_MAX_ITER; iter++) {
+        RCLCPP_DEBUG(
+            get_logger(), "Reconfiguring Hand-E Gripper attempt: %d; result: %d", iter, result);
+
+        wait_100ms();
+        wait_100ms();
+
+        gripper_driver_.cleanup();
+        result = gripper_driver_.configure();
+    }
+
+    if(result == FAILURE_MODBUS) {
+        RCLCPP_INFO(get_logger(), "Failed to configure Hand-E Gripper");
+        return HWI::CallbackReturn::FAILURE;
+    }
+
+    RCLCPP_INFO(get_logger(), "Connected");
     return HWI::CallbackReturn::SUCCESS;
 }