import test_robot_hardware

test_robot_hardware/CMakeLists.txt

@@ -0,0 +1,54 @@
+cmake_minimum_required(VERSION 3.5)
+project(test_robot_hardware)
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(hardware_interface REQUIRED)
+find_package(rcutils REQUIRED)
+
+add_library(test_robot_hardware SHARED src/test_robot_hardware.cpp)
+target_include_directories(test_robot_hardware PRIVATE include)
+ament_target_dependencies(
+  test_robot_hardware
+  hardware_interface
+  rcutils
+)
+
+# Causes the visibility macros to use dllexport rather than dllimport,
+# which is appropriate when building the dll but not consuming it.
+target_compile_definitions(test_robot_hardware PRIVATE "TEST_ROBOT_HARDWARE_BUILDING_DLL")
+
+install(DIRECTORY include/
+  DESTINATION include)
+
+install(TARGETS test_robot_hardware
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+
+  ament_add_gtest(test_robot_hardware_interface test/test_robot_hardware_interface.cpp)
+  if(TARGET test_robot_hardware_interface)
+    target_include_directories(test_robot_hardware_interface PRIVATE include)
+    target_link_libraries(
+      test_robot_hardware_interface
+      test_robot_hardware
+    )
+  endif()
+endif()
+
+ament_export_libraries(${PROJECT_NAME})
+ament_export_include_directories(include)
+ament_package()

test_robot_hardware/include/test_robot_hardware/test_robot_hardware.hpp

@@ -0,0 +1,90 @@
+// Copyright 2017 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef TEST_ROBOT_HARDWARE__TEST_ROBOT_HARDWARE_HPP_
+#define TEST_ROBOT_HARDWARE__TEST_ROBOT_HARDWARE_HPP_
+
+#include <string>
+
+#include "hardware_interface/robot_hardware.hpp"
+#include "hardware_interface/types/hardware_interface_return_values.hpp"
+
+#include "test_robot_hardware/visibility_control.h"
+
+namespace test_robot_hardware
+{
+// TODO(karsten1987): Maybe visibility macros on class level
+// as all members are publically exposed
+class TestRobotHardware : public hardware_interface::RobotHardware
+{
+public:
+  TEST_ROBOT_HARDWARE_PUBLIC
+  hardware_interface::hardware_interface_ret_t
+  init();
+
+  TEST_ROBOT_HARDWARE_PUBLIC
+  hardware_interface::hardware_interface_ret_t
+  read();
+
+  TEST_ROBOT_HARDWARE_PUBLIC
+  hardware_interface::hardware_interface_ret_t
+  write();
+
+  std::string joint_name1 = "joint1";
+  std::string joint_name2 = "joint2";
+  std::string joint_name3 = "joint3";
+
+  std::string read_op_handle_name1 = "read1";
+  std::string read_op_handle_name2 = "read2";
+  std::string write_op_handle_name1 = "write1";
+  std::string write_op_handle_name2 = "write2";
+
+  double pos1 = 1.1;
+  double pos2 = 2.2;
+  double pos3 = 3.3;
+
+  double vel1 = 1.1;
+  double vel2 = 2.2;
+  double vel3 = 3.3;
+
+  double eff1 = 1.1;
+  double eff2 = 2.2;
+  double eff3 = 3.3;
+
+  double cmd1 = 1.1;
+  double cmd2 = 2.2;
+  double cmd3 = 3.3;
+
+  bool read1 = false;
+  bool read2 = false;
+  bool write1 = false;
+  bool write2 = false;
+
+  hardware_interface::JointStateHandle js1;
+  hardware_interface::JointStateHandle js2;
+  hardware_interface::JointStateHandle js3;
+
+  hardware_interface::JointCommandHandle jcmd1;
+  hardware_interface::JointCommandHandle jcmd2;
+  hardware_interface::JointCommandHandle jcmd3;
+
+  hardware_interface::OperationModeHandle read_op_handle1;
+  hardware_interface::OperationModeHandle read_op_handle2;
+
+  hardware_interface::OperationModeHandle write_op_handle1;
+  hardware_interface::OperationModeHandle write_op_handle2;
+};
+
+}  // namespace test_robot_hardware
+#endif  // TEST_ROBOT_HARDWARE__TEST_ROBOT_HARDWARE_HPP_

test_robot_hardware/include/test_robot_hardware/visibility_control.h

@@ -0,0 +1,56 @@
+// Copyright 2017 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/* This header must be included by all rclcpp headers which declare symbols
+ * which are defined in the rclcpp library. When not building the rclcpp
+ * library, i.e. when using the headers in other package's code, the contents
+ * of this header change the visibility of certain symbols which the rclcpp
+ * library cannot have, but the consuming code must have inorder to link.
+ */
+
+#ifndef TEST_ROBOT_HARDWARE__VISIBILITY_CONTROL_H_
+#define TEST_ROBOT_HARDWARE__VISIBILITY_CONTROL_H_
+
+// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
+//     https://gcc.gnu.org/wiki/Visibility
+
+#if defined _WIN32 || defined __CYGWIN__
+  #ifdef __GNUC__
+    #define TEST_ROBOT_HARDWARE_EXPORT __attribute__ ((dllexport))
+    #define TEST_ROBOT_HARDWARE_IMPORT __attribute__ ((dllimport))
+  #else
+    #define TEST_ROBOT_HARDWARE_EXPORT __declspec(dllexport)
+    #define TEST_ROBOT_HARDWARE_IMPORT __declspec(dllimport)
+  #endif
+  #ifdef TEST_ROBOT_HARDWARE_BUILDING_DLL
+    #define TEST_ROBOT_HARDWARE_PUBLIC TEST_ROBOT_HARDWARE_EXPORT
+  #else
+    #define TEST_ROBOT_HARDWARE_PUBLIC TEST_ROBOT_HARDWARE_IMPORT
+  #endif
+  #define TEST_ROBOT_HARDWARE_PUBLIC_TYPE TEST_ROBOT_HARDWARE_PUBLIC
+  #define TEST_ROBOT_HARDWARE_LOCAL
+#else
+  #define TEST_ROBOT_HARDWARE_EXPORT __attribute__ ((visibility("default")))
+  #define TEST_ROBOT_HARDWARE_IMPORT
+  #if __GNUC__ >= 4
+    #define TEST_ROBOT_HARDWARE_PUBLIC __attribute__ ((visibility("default")))
+    #define TEST_ROBOT_HARDWARE_LOCAL  __attribute__ ((visibility("hidden")))
+  #else
+    #define TEST_ROBOT_HARDWARE_PUBLIC
+    #define TEST_ROBOT_HARDWARE_LOCAL
+  #endif
+  #define TEST_ROBOT_HARDWARE_PUBLIC_TYPE
+#endif
+
+#endif  // TEST_ROBOT_HARDWARE__VISIBILITY_CONTROL_H_

test_robot_hardware/package.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="2">
+  <name>test_robot_hardware</name>
+  <version>0.0.0</version>
+  <description>Description of test_robot_hardware</description>
+  <maintainer email="karsten@osrfoundation.org">Karsten Knese</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>hardware_interface</build_depend>
+  <build_depend>rcutils</build_depend>
+
+  <exec_depend>hardware_interface</exec_depend>
+  <exec_depend>rcutils</exec_depend>
+
+  <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

test_robot_hardware/src/test_robot_hardware.cpp

@@ -0,0 +1,125 @@
+// Copyright 2017 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "test_robot_hardware/test_robot_hardware.hpp"
+
+#include <string>
+#include <vector>
+
+#include "rcutils/logging_macros.h"
+
+namespace test_robot_hardware
+{
+
+hardware_interface::hardware_interface_ret_t
+TestRobotHardware::init()
+{
+  auto ret = hardware_interface::HW_RET_ERROR;
+
+  js1 = hardware_interface::JointStateHandle(
+    joint_name1, &pos1, &vel1, &eff1);
+  ret = register_joint_state_handle(&js1);
+  if (ret != hardware_interface::HW_RET_OK) {
+    RCUTILS_LOG_WARN("can't register joint state handle %s", joint_name1.c_str());
+    return ret;
+  }
+
+  js2 = hardware_interface::JointStateHandle(
+    joint_name2, &pos2, &vel2, &eff2);
+  ret = register_joint_state_handle(&js2);
+  if (ret != hardware_interface::HW_RET_OK) {
+    RCUTILS_LOG_WARN("can't register joint state handle %s", joint_name2.c_str());
+    return ret;
+  }
+
+  js3 = hardware_interface::JointStateHandle(
+    joint_name3, &pos3, &vel3, &eff3);
+  ret = register_joint_state_handle(&js3);
+  if (ret != hardware_interface::HW_RET_OK) {
+    RCUTILS_LOG_WARN("can't register joint state handle %s", joint_name3.c_str());
+    return ret;
+  }
+
+  jcmd1 = hardware_interface::JointCommandHandle(joint_name1, &cmd1);
+  ret = register_joint_command_handle(&jcmd1);
+  if (ret != hardware_interface::HW_RET_OK) {
+    RCUTILS_LOG_WARN("can't register joint command handle %s", joint_name1.c_str());
+    return ret;
+  }
+
+  jcmd2 = hardware_interface::JointCommandHandle(joint_name2, &cmd2);
+  ret = register_joint_command_handle(&jcmd2);
+  if (ret != hardware_interface::HW_RET_OK) {
+    RCUTILS_LOG_WARN("can't register joint command handle %s", joint_name2.c_str());
+    return ret;
+  }
+
+  jcmd3 = hardware_interface::JointCommandHandle(joint_name3, &cmd3);
+  ret = register_joint_command_handle(&jcmd3);
+  if (ret != hardware_interface::HW_RET_OK) {
+    RCUTILS_LOG_WARN("can't register joint command handle %s", joint_name3.c_str());
+    return ret;
+  }
+
+  read_op_handle1 = hardware_interface::OperationModeHandle(read_op_handle_name1,
+      reinterpret_cast<hardware_interface::OperationMode *>(&read1));
+  ret = register_operation_mode_handle(&read_op_handle1);
+  if (ret != hardware_interface::HW_RET_OK) {
+    RCUTILS_LOG_WARN("can't register operation mode handle %s", read_op_handle_name1.c_str());
+    return ret;
+  }
+
+  read_op_handle2 = hardware_interface::OperationModeHandle(read_op_handle_name2,
+      reinterpret_cast<hardware_interface::OperationMode *>(&read2));
+  ret = register_operation_mode_handle(&read_op_handle2);
+  if (ret != hardware_interface::HW_RET_OK) {
+    RCUTILS_LOG_WARN("can't register operation mode handle %s", read_op_handle_name2.c_str());
+    return ret;
+  }
+
+  write_op_handle1 = hardware_interface::OperationModeHandle(write_op_handle_name1,
+      reinterpret_cast<hardware_interface::OperationMode *>(&write1));
+  ret = register_operation_mode_handle(&write_op_handle1);
+  if (ret != hardware_interface::HW_RET_OK) {
+    RCUTILS_LOG_WARN("can't register operation mode handle %s", write_op_handle_name1.c_str());
+    return ret;
+  }
+
+  write_op_handle2 = hardware_interface::OperationModeHandle(write_op_handle_name2,
+      reinterpret_cast<hardware_interface::OperationMode *>(&write2));
+  ret = register_operation_mode_handle(&write_op_handle2);
+  if (ret != hardware_interface::HW_RET_OK) {
+    RCUTILS_LOG_WARN("can't register operation mode handle %s", write_op_handle_name2.c_str());
+    return ret;
+  }
+
+  return hardware_interface::HW_RET_OK;
+}
+
+hardware_interface::hardware_interface_ret_t
+TestRobotHardware::read()
+{
+  return hardware_interface::HW_RET_OK;
+}
+
+hardware_interface::hardware_interface_ret_t
+TestRobotHardware::write()
+{
+  pos1 = cmd1;
+  pos2 = cmd2;
+  pos3 = cmd3;
+  return hardware_interface::HW_RET_OK;
+}
+
+}  // namespace test_robot_hardware