Add new joint handles to test_robot_hardware

.github/workflows/ci.yml

@@ -9,17 +9,17 @@ on:
     - cron: '17 8 * * *'
 
 jobs:
-  ci_main_release:
-    name: Industrial CI against release repository
-    strategy:
-      matrix:
-        env:
-          - {ROS_DISTRO: foxy, ROS_REPO: main}
-    runs-on: ubuntu-latest
-    steps:
-      - uses: actions/checkout@v1
-      - uses: 'ros-industrial/industrial_ci@master'
-        env: ${{matrix.env}}
+        #  ci_main_release:
+        #    name: Industrial CI against release repository
+        #    strategy:
+        #      matrix:
+        #        env:
+        #          - {ROS_DISTRO: foxy, ROS_REPO: main}
+        #    runs-on: ubuntu-latest
+        #    steps:
+        #      - uses: actions/checkout@v1
+        #      - uses: 'ros-industrial/industrial_ci@master'
+        #        env: ${{matrix.env}}
 
   ci_testing_release:
     name: Industrial CI against testing repository

test_robot_hardware/include/test_robot_hardware/test_robot_hardware.hpp

@@ -16,6 +16,7 @@
 #define TEST_ROBOT_HARDWARE__TEST_ROBOT_HARDWARE_HPP_
 
 #include <string>
+#include <vector>
 
 #include "hardware_interface/robot_hardware.hpp"
 #include "hardware_interface/types/hardware_interface_return_values.hpp"
@@ -68,6 +69,13 @@ class TestRobotHardware : public hardware_interface::RobotHardware
   double cmd2 = 2.2;
   double cmd3 = 3.3;
 
+  std::vector<std::string> actuator_names = {"actuator1", "actuator2", "actuator3"};
+  std::vector<std::string> joint_names = {"joint1", "joint2", "joint3"};
+
+  std::vector<double> pos_dflt_values = {1.1, 2.1, 3.1};
+  std::vector<double> vel_dflt_values = {1.2, 2.2, 3.2};
+  std::vector<double> eff_dflt_values = {1.3, 2.3, 3.3};
+
   bool read1 = false;
   bool read2 = false;
   bool write1 = false;

test_robot_hardware/src/test_robot_hardware.cpp

@@ -107,49 +107,22 @@ TestRobotHardware::init()
     return ret;
   }
 
-  //
-
-  register_actuator("actuator1", "position", 1.1);
-  register_actuator("actuator1", "velocity", 1.2);
-  register_actuator("actuator1", "effort", 1.3);
-  register_actuator("actuator1", "position_command", 1.1);
-  register_actuator("actuator1", "velocity_command", 1.2);
-  register_actuator("actuator1", "effort_command", 1.3);
-
-  register_actuator("actuator2", "position", 2.1);
-  register_actuator("actuator2", "velocity", 2.2);
-  register_actuator("actuator2", "effort", 2.3);
-  register_actuator("actuator2", "position_command", 2.1);
-  register_actuator("actuator2", "velocity_command", 2.2);
-  register_actuator("actuator2", "effort_command", 2.3);
-
-  register_actuator("actuator3", "position", 3.1);
-  register_actuator("actuator3", "velocity", 3.2);
-  register_actuator("actuator3", "effort", 3.3);
-  register_actuator("actuator3", "position_command", 3.1);
-  register_actuator("actuator3", "velocity_command", 3.2);
-  register_actuator("actuator3", "effort_command", 3.3);
-
-  register_joint("joint1", "position", 1.1);
-  register_joint("joint1", "velocity", 1.2);
-  register_joint("joint1", "effort", 1.3);
-  register_joint("joint1", "position_command", 1.1);
-  register_joint("joint1", "velocity_command", 1.2);
-  register_joint("joint1", "effort_command", 1.3);
-
-  register_joint("joint2", "position", 2.1);
-  register_joint("joint2", "velocity", 2.2);
-  register_joint("joint2", "effort", 2.3);
-  register_joint("joint2", "position_command", 2.1);
-  register_joint("joint2", "velocity_command", 2.2);
-  register_joint("joint2", "effort_command", 2.3);
-
-  register_joint("joint3", "position", 3.1);
-  register_joint("joint3", "velocity", 3.2);
-  register_joint("joint3", "effort", 3.3);
-  register_joint("joint3", "position_command", 3.1);
-  register_joint("joint3", "velocity_command", 3.2);
-  register_joint("joint3", "effort_command", 3.3);
+  // register actuators and joints
+  for (auto index = 0u; index < 3u; ++index) {
+    register_actuator(actuator_names[index], "position", pos_dflt_values[index]);
+    register_actuator(actuator_names[index], "velocity", vel_dflt_values[index]);
+    register_actuator(actuator_names[index], "effort", eff_dflt_values[index]);
+    register_actuator(actuator_names[index], "position_command", pos_dflt_values[index]);
+    register_actuator(actuator_names[index], "velocity_command", vel_dflt_values[index]);
+    register_actuator(actuator_names[index], "effort_command", eff_dflt_values[index]);
+
+    register_joint(joint_names[index], "position", pos_dflt_values[index]);
+    register_joint(joint_names[index], "velocity", vel_dflt_values[index]);
+    register_joint(joint_names[index], "effort", eff_dflt_values[index]);
+    register_joint(joint_names[index], "position_command", pos_dflt_values[index]);
+    register_joint(joint_names[index], "velocity_command", vel_dflt_values[index]);
+    register_joint(joint_names[index], "effort_command", eff_dflt_values[index]);
+  }
 
   return hardware_interface::return_type::OK;
 }
@@ -166,6 +139,34 @@ TestRobotHardware::write()
   pos1 = cmd1;
   pos2 = cmd2;
   pos3 = cmd3;
+
+  auto update_handle = [&](const std::string & joint_name, const std::string & interface_name)
+    {
+      auto get_handle = [&](const std::string & joint_name, const std::string & interface_name)
+        {
+          auto joint_handle = std::make_shared<hardware_interface::JointHandle>(
+            joint_name,
+            interface_name);
+          get_joint_handle(*joint_handle);
+          return joint_handle;
+        };
+
+      get_handle(
+        joint_name,
+        interface_name)->set_value(
+        get_handle(
+          joint_name,
+          interface_name + "_command")->get_value());
+    };
+
+  // update all the joint state handles with their respectives command values
+  const std::vector<std::string> interface_names = {"position", "velocity", "effort"};
+  for (const auto & joint_name : joint_names) {
+    for (const auto & interface_name : interface_names) {
+      update_handle(joint_name, interface_name);
+    }
+  }
+
   return hardware_interface::return_type::OK;
 }
 

test_robot_hardware/test/test_robot_hardware_interface.cpp

@@ -29,56 +29,55 @@ class TestRobotHardwareInterface : public ::testing::Test
 protected:
   void SetUp()
   {
-    joint_names = {robot.joint_name1, robot.joint_name2, robot.joint_name3};
-    joint_pos_values = {robot.pos1, robot.pos2, robot.pos3};
-    joint_vel_values = {robot.vel1, robot.vel2, robot.vel3};
-    joint_eff_values = {robot.eff1, robot.eff2, robot.eff3};
-    joint_cmd_values = {robot.cmd1, robot.cmd2, robot.cmd3};
+    joint_names_ = robot_.joint_names;
+
+    joint_pos_values_ = robot_.pos_dflt_values;
+    joint_vel_values_ = robot_.vel_dflt_values;
+    joint_eff_values_ = robot_.eff_dflt_values;
+
+    joint_pos_cmd_values_ = robot_.pos_dflt_values;
+    joint_vel_cmd_values_ = robot_.vel_dflt_values;
+    joint_eff_cmd_values_ = robot_.eff_dflt_values;
   }
 
-  test_robot_hardware::TestRobotHardware robot;
+  test_robot_hardware::TestRobotHardware robot_;
+
+  std::vector<std::string> joint_names_;
 
-  std::vector<std::string> joint_names;
-  std::vector<double> joint_pos_values;
-  std::vector<double> joint_vel_values;
-  std::vector<double> joint_eff_values;
-  std::vector<double> joint_cmd_values;
+  std::vector<double> joint_pos_values_;
+  std::vector<double> joint_vel_values_;
+  std::vector<double> joint_eff_values_;
+
+  std::vector<double> joint_pos_cmd_values_;
+  std::vector<double> joint_vel_cmd_values_;
+  std::vector<double> joint_eff_cmd_values_;
 };
 
 TEST_F(TestRobotHardwareInterface, initialize) {
-  EXPECT_EQ(hw_ret::OK, robot.init());
+  EXPECT_EQ(hw_ret::OK, robot_.init());
 }
 
 TEST_F(TestRobotHardwareInterface, get_registered_joint_handles) {
-  robot.init();
+  robot_.init();
 
   auto ret = hw_ret::ERROR;
   for (auto i = 0u; i < 3u; ++i) {
-    const hardware_interface::JointStateHandle * js_ptr = nullptr;
-    ret = robot.get_joint_state_handle(joint_names[i], &js_ptr);
-    EXPECT_EQ(hw_ret::OK, ret);
-    EXPECT_EQ(joint_pos_values[i], js_ptr->get_position());
-    EXPECT_EQ(joint_vel_values[i], js_ptr->get_velocity());
-    EXPECT_EQ(joint_eff_values[i], js_ptr->get_effort());
-    ret = hw_ret::ERROR;
-  }
+    auto get_handle = [&](const std::string joint_name, const std::string interface_name)
+      {
+        hardware_interface::JointHandle joint_handle(joint_name, interface_name);
+        ret = robot_.get_joint_handle(joint_handle);
+        return joint_handle;
+      };
 
-  auto registered_joint_handles = robot.get_registered_joint_state_handles();
-  EXPECT_EQ(3u, registered_joint_handles.size());
-}
-
-TEST_F(TestRobotHardwareInterface, get_registered_command_handles) {
-  robot.init();
+    EXPECT_EQ(get_handle(joint_names_[i], "position").get_value(), joint_pos_values_[i]);
+    EXPECT_EQ(get_handle(joint_names_[i], "velocity").get_value(), joint_vel_values_[i]);
+    EXPECT_EQ(get_handle(joint_names_[i], "effort").get_value(), joint_eff_values_[i]);
 
-  auto ret = hw_ret::ERROR;
-  for (auto i = 0u; i < 3u; ++i) {
-    hardware_interface::JointCommandHandle * jcmd_ptr = nullptr;
-    ret = robot.get_joint_command_handle(joint_names[i], &jcmd_ptr);
-    EXPECT_EQ(hw_ret::OK, ret);
-    EXPECT_EQ(joint_cmd_values[i], jcmd_ptr->get_cmd());
-    ret = hw_ret::ERROR;
+    EXPECT_EQ(get_handle(joint_names_[i], "position_command").get_value(), joint_pos_values_[i]);
+    EXPECT_EQ(get_handle(joint_names_[i], "velocity_command").get_value(), joint_vel_values_[i]);
+    EXPECT_EQ(get_handle(joint_names_[i], "effort_command").get_value(), joint_eff_values_[i]);
   }
 
-  auto registered_command_handles = robot.get_registered_joint_command_handles();
-  EXPECT_EQ(3u, registered_command_handles.size());
+  // 3 joints * 6 interfaces
+  EXPECT_EQ(robot_.get_registered_joints().size(), 3u * 6u);
 }