diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 128fddbaea..3653d82af5 100644 --- a/.github/workflows/ci.yml +++ b/.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 diff --git a/test_robot_hardware/include/test_robot_hardware/test_robot_hardware.hpp b/test_robot_hardware/include/test_robot_hardware/test_robot_hardware.hpp index 89294c717c..a71bfe5329 100644 --- a/test_robot_hardware/include/test_robot_hardware/test_robot_hardware.hpp +++ b/test_robot_hardware/include/test_robot_hardware/test_robot_hardware.hpp @@ -16,6 +16,7 @@ #define TEST_ROBOT_HARDWARE__TEST_ROBOT_HARDWARE_HPP_ #include +#include #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 actuator_names = {"actuator1", "actuator2", "actuator3"}; + std::vector joint_names = {"joint1", "joint2", "joint3"}; + + std::vector pos_dflt_values = {1.1, 2.1, 3.1}; + std::vector vel_dflt_values = {1.2, 2.2, 3.2}; + std::vector eff_dflt_values = {1.3, 2.3, 3.3}; + bool read1 = false; bool read2 = false; bool write1 = false; diff --git a/test_robot_hardware/src/test_robot_hardware.cpp b/test_robot_hardware/src/test_robot_hardware.cpp index 0a5cba711d..ff08859d60 100644 --- a/test_robot_hardware/src/test_robot_hardware.cpp +++ b/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( + 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 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; } diff --git a/test_robot_hardware/test/test_robot_hardware_interface.cpp b/test_robot_hardware/test/test_robot_hardware_interface.cpp index 12a7beea32..c3f9983d62 100644 --- a/test_robot_hardware/test/test_robot_hardware_interface.cpp +++ b/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 joint_names_; - std::vector joint_names; - std::vector joint_pos_values; - std::vector joint_vel_values; - std::vector joint_eff_values; - std::vector joint_cmd_values; + std::vector joint_pos_values_; + std::vector joint_vel_values_; + std::vector joint_eff_values_; + + std::vector joint_pos_cmd_values_; + std::vector joint_vel_cmd_values_; + std::vector 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); }