Add write_data method

rt-net · Jan 5, 2024 · 00c6d34 · 00c6d34
1 parent 00fe068
commit 00c6d34
Show file tree

Hide file tree

Showing 2 changed files with 58 additions and 0 deletions.
diff --git a/rt_manipulators_lib/include/hardware.hpp b/rt_manipulators_lib/include/hardware.hpp
@@ -20,6 +20,7 @@
 #include <memory>
 #include <string>
 #include <thread>
+#include <type_traits>
 #include <vector>
 
 #include "joint.hpp"
@@ -86,6 +87,28 @@ class Hardware {
   bool write_velocity_pi_gain_to_group(const std::string& group_name, const uint16_t p,
                                        const uint16_t i);
 
+  template <typename IdentifyT, typename DataT>
+  bool write_data(const IdentifyT & identify, const uint16_t & addr, const DataT & data)
+  {
+    if (!joints_.has_joint(identify)) {
+      std::cerr << "Joint: " << identify << " is not registered." << std::endl;
+      return false;
+    }
+
+    if (std::is_same<DataT, uint8_t>::value) {
+      return comm_->write_byte_data(joints_.joint(identify)->id(), addr, data);
+    }
+    if (std::is_same<DataT, uint16_t>::value) {
+      return comm_->write_word_data(joints_.joint(identify)->id(), addr, data);
+    }
+    if (std::is_same<DataT, uint32_t>::value) {
+      return comm_->write_double_word_data(joints_.joint(identify)->id(), addr, data);
+    }
+
+    return false;
+  }
+
+
  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
@@ -84,3 +84,38 @@ TEST(HardwareTest, disconnect) {
   hardware.disconnect();
   Verify(Method(mock, disconnect)).Once();
 }
+
+TEST(HardwareTest, write_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"));
+
+  // Return false when joint name or id is not found
+  mock.ClearInvocationHistory();
+  EXPECT_FALSE(hardware.write_data("joint0", 0x00, static_cast<uint8_t>(0x00)));
+  EXPECT_FALSE(hardware.write_data(0, 0x00, static_cast<uint8_t>(0x00)));
+  Verify(Method(mock, write_byte_data)).Never();
+  Verify(Method(mock, write_word_data)).Never();
+  Verify(Method(mock, write_double_word_data)).Never();
+
+  // Identify joint via joint name
+  EXPECT_TRUE(hardware.write_data("joint1", 0x00, static_cast<uint8_t>(0x00)));
+  Verify(Method(mock, write_byte_data)).Once();
+
+  // Identify joint via joint id
+  EXPECT_TRUE(hardware.write_data(2, 0x00, static_cast<uint16_t>(0x00)));
+  Verify(Method(mock, write_word_data)).Once();
+
+  EXPECT_TRUE(hardware.write_data("joint3", 0x00, static_cast<uint32_t>(0x00)));
+  Verify(Method(mock, write_double_word_data)).Once();
+
+  // Return false when data type is not matched
+  mock.ClearInvocationHistory();
+  EXPECT_FALSE(hardware.write_data("joint1", 0x00, 0x00));
+  Verify(Method(mock, write_byte_data)).Never();
+  Verify(Method(mock, write_word_data)).Never();
+  Verify(Method(mock, write_double_word_data)).Never();
+
+}