From a47e0fa3d6320ccda19563ffa779ba4819f68be0 Mon Sep 17 00:00:00 2001
From: ShotaAk <s.aoki@rt-net.jp>
Date: Thu, 4 Jan 2024 19:28:55 +0900
Subject: [PATCH] Adding read_data method

---
 rt_manipulators_lib/include/hardware.hpp   | 29 +++++++++++++++++
 rt_manipulators_lib/test/test_hardware.cpp | 37 ++++++++++++++++++++++
 2 files changed, 66 insertions(+)

diff --git a/rt_manipulators_lib/include/hardware.hpp b/rt_manipulators_lib/include/hardware.hpp
index 4542381..ffcf18c 100644
--- a/rt_manipulators_lib/include/hardware.hpp
+++ b/rt_manipulators_lib/include/hardware.hpp
@@ -108,6 +108,35 @@ class Hardware {
     return false;
   }
 
+  template <typename IdentifyT, typename DataT>
+  bool read_data(const IdentifyT & identify, const uint16_t & addr, DataT & data)
+  {
+    if (!joints_.has_joint(identify)) {
+      std::cerr << "Joint: " << identify << " is not registered." << std::endl;
+      return false;
+    }
+
+    bool result = false;
+    if (std::is_same<DataT, uint8_t>::value) {
+      uint8_t tmp_data = 0x00;
+      result = comm_->read_byte_data(joints_.joint(identify)->id(), addr, tmp_data);
+      data = tmp_data;
+    }
+    if (std::is_same<DataT, uint16_t>::value) {
+      uint16_t tmp_data = 0x00;
+      result = comm_->read_word_data(joints_.joint(identify)->id(), addr, tmp_data);
+      data = tmp_data;
+    }
+    if (std::is_same<DataT, uint32_t>::value) {
+      uint32_t tmp_data = 0x00;
+      result = comm_->read_double_word_data(joints_.joint(identify)->id(), addr, tmp_data);
+      data = tmp_data;
+    }
+
+    return result;
+  }
+
+
 
  protected:
   std::shared_ptr<hardware_communicator::Communicator> comm_;
diff --git a/rt_manipulators_lib/test/test_hardware.cpp b/rt_manipulators_lib/test/test_hardware.cpp
index 7e5570c..61bf9da 100644
--- a/rt_manipulators_lib/test/test_hardware.cpp
+++ b/rt_manipulators_lib/test/test_hardware.cpp
@@ -117,5 +117,42 @@ TEST(HardwareTest, write_data) {
   Verify(Method(mock, write_byte_data)).Never();
   Verify(Method(mock, write_word_data)).Never();
   Verify(Method(mock, write_double_word_data)).Never();
+}
+
+TEST(HardwareTest, read_data) {
+  auto mock = create_comm_mock();
+  rt_manipulators_cpp::Hardware hardware(
+    std::unique_ptr<hardware_communicator::Communicator>(&mock.get()));
 
+  EXPECT_TRUE(hardware.load_config_file("../config/ok_has_dynamixel_name.yaml"));
+
+  uint8_t byte_data = 0x00;
+  uint16_t word_data = 0x00;
+  uint32_t double_word_data = 0x00;
+  // Return false when joint name or id is not found
+  mock.ClearInvocationHistory();
+  EXPECT_FALSE(hardware.read_data("joint0", 0x00, byte_data));
+  EXPECT_FALSE(hardware.read_data(0, 0x00, byte_data));
+  Verify(Method(mock, read_byte_data)).Never();
+  Verify(Method(mock, read_word_data)).Never();
+  Verify(Method(mock, read_double_word_data)).Never();
+
+  // Identify joint via joint name
+  EXPECT_FALSE(hardware.read_data("joint1", 0x00, byte_data));
+  Verify(Method(mock, read_byte_data)).Once();
+
+  // Identify joint via joint id
+  EXPECT_FALSE(hardware.read_data(2, 0x00, word_data));
+  Verify(Method(mock, read_word_data)).Once();
+
+  EXPECT_FALSE(hardware.read_data("joint3", 0x00, double_word_data));
+  Verify(Method(mock, read_double_word_data)).Once();
+
+  // Return false when data type is not matched
+  double invalid_type_data = 0.0;
+  mock.ClearInvocationHistory();
+  EXPECT_FALSE(hardware.read_data("joint1", 0x00, invalid_type_data));
+  Verify(Method(mock, read_byte_data)).Never();
+  Verify(Method(mock, read_word_data)).Never();
+  Verify(Method(mock, read_double_word_data)).Never();
 }