ADD: read write registers in a thread

Author: slimasziom

robotiq_hande_driver/CMakeLists.txt

@@ -19,7 +19,6 @@ set(HW_IF_INCLUDE_DEPENDS
     rcpputils)
 
 find_package(ament_cmake REQUIRED)
-find_package(behaviortree_cpp REQUIRED)
 
 foreach(dependency IN ITEMS ${HW_IF_INCLUDE_DEPENDS})
   find_package(${dependency} REQUIRED)

robotiq_hande_driver/hardware/include/robotiq_hande_driver/communication.hpp

@@ -2,8 +2,11 @@
 #define ROBOTIQ_HANDE_DRIVER__COMMUNICATION_HPP_
 
 #include <modbus/modbus.h>
+#include <atomic>
 #include <cstring>
+#include <optional>
 #include <string>
+#include <thread>
 
 namespace robotiq_hande_driver {
 
@@ -74,6 +77,56 @@ class Communication {
         slave_id_ = slave_id;
     };
 
+    /**
+     * @brief Reads and writes input/output registers at once.
+     *
+     * @param none
+     * @return None.
+     * @note The status should be checked to verify successful execution. An exception is thrown if
+     * communication issues occur.
+     */
+    void read_write_registers() {
+        /**
+         * @brief Read and write modbus registers at once
+         *
+         * @param modbus_t *ctx
+         * @param int write_addr
+         * @param int write_nb
+         * @param uint16_t *src
+         * @param int read_addr
+         * @param int read_nb
+         * @param uint16_t *dest
+         * @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.
+         */
+        // int result;
+
+        while(running) {
+            // sleep for 100ms
+            std::this_thread::sleep_for(std::chrono::milliseconds(100));
+
+            for(size_t i = 0; i < static_cast<size_t>(OutputBytes::BYTES_MAX); ++i) {
+                output_bytes_modbus_[i] = output_bytes_[i].load(std::memory_order_relaxed);
+            }
+
+            modbus_write_and_read_registers(
+                mb_,
+                GRIPPER_INPUT_FIRST_REG,
+                OUTPUT_REGISTER_WORD_LENGTH,
+                reinterpret_cast<uint16_t*>(output_bytes_modbus_),
+                GRIPPER_OUTPUT_FIRST_REG,
+                INPUT_REGISTER_WORD_LENGTH,
+                reinterpret_cast<uint16_t*>(input_bytes_modbus_));
+
+            for(size_t i = 0; i < static_cast<size_t>(InputBytes::BYTES_MAX); ++i) {
+                input_bytes_[i].store(input_bytes_modbus_[i], std::memory_order_relaxed);
+            }
+        }
+
+        // return result;
+    };
+
     /**
      * @brief Initializes communication layer.
      *
@@ -90,6 +143,8 @@ class Communication {
         modbus_set_debug(mb_, DEBUG_MODBUS);
         result = connect();
 
+        my_thread.emplace(&Communication::read_write_registers, this);
+
         return result;
     };
 
@@ -102,6 +157,10 @@ class Communication {
      * communication issues occur.
      */
     void cleanup() {
+        if(my_thread && my_thread->joinable()) {
+            my_thread->join();
+        }
+
         disconnect();
         modbus_free(mb_);
     };
@@ -128,43 +187,6 @@ class Communication {
         modbus_close(mb_);
     };
 
-    /**
-     * @brief Reads and writes input/output registers at once.
-     *
-     * @param none
-     * @return None.
-     * @note The status should be checked to verify successful execution. An exception is thrown if
-     * communication issues occur.
-     */
-    int read_write_registers() {
-        /**
-         * @brief Read and write modbus registers at once
-         *
-         * @param modbus_t *ctx
-         * @param int write_addr
-         * @param int write_nb
-         * @param uint16_t *src
-         * @param int read_addr
-         * @param int read_nb
-         * @param uint16_t *dest
-         * @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.
-         */
-        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;
-    };
-
     /**
      * @brief Sets output bytes to zeros.
      *
@@ -174,7 +196,10 @@ class Communication {
      * communication issues occur.
      */
     void clear_output_bytes() {
-        memset(output_bytes_, 0, sizeof(output_bytes_));
+        // memset(output_bytes_, 0, sizeof(output_bytes_));
+        for(size_t i = 0; i < static_cast<size_t>(OutputBytes::BYTES_MAX); ++i) {
+            output_bytes_[i].store(0, std::memory_order_relaxed);
+        }
     };
 
     /**
@@ -186,7 +211,7 @@ class Communication {
      * communication issues occur.
      */
     uint8_t get_input_byte(InputBytes index) {
-        return input_bytes_[static_cast<uint>(index)];
+        return input_bytes_[static_cast<uint>(index)].load(std::memory_order_relaxed);
     };
 
     /**
@@ -199,7 +224,7 @@ class Communication {
      * communication issues occur.
      */
     void set_output_byte(OutputBytes index, uint8_t value) {
-        output_bytes_[static_cast<uint>(index)] = value;
+        output_bytes_[static_cast<uint>(index)].store(value, std::memory_order_relaxed);
     };
 
     /**
@@ -210,8 +235,13 @@ class Communication {
      * @return None.
      */
     void write_action_bit(uint8_t position_bit, bool value) {
-        output_bytes_[static_cast<uint>(OutputBytes::ACTION_REQUEST)] = bit_set_to(
-            output_bytes_[static_cast<uint>(OutputBytes::ACTION_REQUEST)], position_bit, value);
+        output_bytes_[static_cast<uint>(OutputBytes::ACTION_REQUEST)].store(
+            bit_set_to(
+                output_bytes_[static_cast<uint>(OutputBytes::ACTION_REQUEST)].load(
+                    std::memory_order_relaxed),
+                position_bit,
+                value),
+            std::memory_order_relaxed);
     };
 
     /**
@@ -238,8 +268,14 @@ class Communication {
 
     modbus_t* mb_;
 
-    uint8_t input_bytes_[static_cast<size_t>(InputBytes::BYTES_MAX)];
-    uint8_t output_bytes_[static_cast<size_t>(OutputBytes::BYTES_MAX)];
+    uint8_t input_bytes_modbus_[static_cast<size_t>(InputBytes::BYTES_MAX)];
+    uint8_t output_bytes_modbus_[static_cast<size_t>(OutputBytes::BYTES_MAX)];
+
+    std::atomic<uint8_t> input_bytes_[static_cast<size_t>(InputBytes::BYTES_MAX)];
+    std::atomic<uint8_t> output_bytes_[static_cast<size_t>(OutputBytes::BYTES_MAX)];
+
+    std::atomic<bool> running{true};
+    std::optional<std::thread> my_thread;
 };
 }  // namespace robotiq_hande_driver
 #endif  // ROBOTIQ_HANDE_DRIVER__COMMUNICATION_HPP_

robotiq_hande_driver/hardware/include/robotiq_hande_driver/protocol_logic.hpp

@@ -146,7 +146,7 @@ class ProtocolLogic {
         communication_.write_action_bit(
             static_cast<uint>(ActionRequestPositionBit::ACTIVATE),
             static_cast<bool>(Activate::DEACTIVATE_GRIPPER));
-        communication_.read_write_registers();
+        // communication_.read_write_registers();
     };
 
     /**
@@ -162,7 +162,7 @@ class ProtocolLogic {
         communication_.write_action_bit(
             static_cast<uint>(ActionRequestPositionBit::ACTIVATE),
             static_cast<bool>(Activate::ACTIVATE_GRIPPER));
-        communication_.read_write_registers();
+        // communication_.read_write_registers();
     };
 
     /**
@@ -180,7 +180,7 @@ class ProtocolLogic {
         communication_.write_action_bit(
             static_cast<uint>(ActionRequestPositionBit::AUTOMATIC_RELEASE_DIRECTION),
             static_cast<bool>(AutoReleaseDirection::OPENING));
-        communication_.read_write_registers();
+        // communication_.read_write_registers();
     };
 
     /**
@@ -225,7 +225,7 @@ class ProtocolLogic {
         communication_.set_output_byte(OutputBytes::POSITION_REQUEST, position);
         communication_.set_output_byte(OutputBytes::SPEED, velocity);
         communication_.set_output_byte(OutputBytes::FORCE, force);
-        communication_.read_write_registers();
+        // communication_.read_write_registers();
     };
 
     /**
@@ -238,7 +238,7 @@ class ProtocolLogic {
     void stop() {
         communication_.write_action_bit(
             static_cast<uint>(ActionRequestPositionBit::GO_TO), static_cast<bool>(GoTo::STOP));
-        communication_.read_write_registers();
+        // communication_.read_write_registers();
     };
 
     /**

robotiq_hande_driver/hardware/src/application.cpp

@@ -13,6 +13,19 @@ GripperApplication::GripperApplication()
       gripper_postion_step_() {}
 
 void GripperApplication::read() {
+    // static int divider = 0;
+
+    // if(protocol_logic_.is_ready()){
+    //     divider++;
+    //     if(divider==1000){
+    //         divider=0;
+    //         protocol_logic_.refresh_registers();
+    //     }
+    // }
+    // else{
+    //    protocol_logic_.refresh_registers();
+    // }
+
     protocol_logic_.refresh_registers();
 
     status_.is_reset = protocol_logic_.is_reset();

robotiq_hande_driver/hardware/src/communication.cpp

@@ -4,15 +4,16 @@
 
 namespace robotiq_hande_driver {
 
-Communication::Communication() : input_bytes_{}, output_bytes_{} {}
+Communication::Communication()
+    : input_bytes_modbus_{}, output_bytes_modbus_{}, input_bytes_{}, output_bytes_{} {}
 
 int Communication::connect() {
-    int result;
+    // int result;
 
     modbus_connect(mb_);
 
-    result = read_write_registers();
+    // read_write_registers();
 
-    return result;
+    return 0;
 }
 }  // namespace robotiq_hande_driver

robotiq_hande_driver/hardware/src/protocol_logic.cpp

@@ -16,7 +16,7 @@ ProtocolLogic::ProtocolLogic()
       current_() {}
 
 void ProtocolLogic::refresh_registers() {
-    communication_.read_write_registers();
+    // communication_.read_write_registers();
 
     status_ = communication_.get_input_byte(InputBytes::GRIPPER_STATUS);