Add mock robot (as ros2 controllers) (#7)

* add C++ mock robot

* add pytroller-based mock robot

hk1d_mock_component_controllers/hk1d_mock_pyrobot/CMakeLists.txt

@@ -0,0 +1,161 @@
+cmake_minimum_required(VERSION 3.16)
+project(hk1d_mock_pyrobot LANGUAGES CXX)
+
+if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
+  add_compile_options(-Wall -Wextra -Wpedantic -Wconversion)
+endif()
+
+set(THIS_PACKAGE_INCLUDE_DEPENDS
+  controller_interface
+  generate_parameter_library
+  hardware_interface
+  pluginlib
+  rclcpp
+  rclcpp_lifecycle
+  realtime_tools
+)
+
+find_package(ament_cmake REQUIRED)
+foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
+  find_package(${Dependency} REQUIRED)
+endforeach()
+
+find_package(PythonLibs REQUIRED)
+include_directories(${PYTHON_INCLUDE_DIRS})
+
+# Set the parameter header file name
+set(PARAM_INCLUDE_DIR ${CMAKE_CURRENT_BINARY_DIR}/hk1d_mock_pyrobot_parameters/include)
+set(PARAM_HEADER_FILE ${PARAM_INCLUDE_DIR}/hk1d_mock_pyrobot_parameters.hpp)
+
+# Make logic build directory
+set(LOGIC_DIR ${CMAKE_CURRENT_BINARY_DIR}/${LIB_NAME}/hk1d_mock_pyrobot_logic)
+set(LOGIC_INCLUDE_DIR ${LOGIC_DIR}/include/hk1d_mock_pyrobot)
+file(MAKE_DIRECTORY ${LOGIC_DIR})
+file(MAKE_DIRECTORY ${LOGIC_INCLUDE_DIR})
+
+file (REMOVE ${LOGIC_DIR}/hk1d_mock_pyrobot_logic.cpp)
+file (REMOVE ${LOGIC_DIR}/hk1d_mock_pyrobot_logic.h)
+file (REMOVE ${LOGIC_INCLUDE_DIR}/hk1d_mock_pyrobot_logic.h)
+
+add_custom_command(
+  OUTPUT ${LOGIC_DIR}/hk1d_mock_pyrobot_logic.cpp ${LOGIC_DIR}/hk1d_mock_pyrobot_logic.h
+  COMMAND cython3 -3 --cplus ${CMAKE_CURRENT_SOURCE_DIR}/src/hk1d_mock_pyrobot_logic.pyx
+    -o ${LOGIC_DIR}/hk1d_mock_pyrobot_logic.cpp -I ${PARAM_INCLUDE_DIR}
+  WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/src
+  DEPENDS
+    ${CMAKE_CURRENT_SOURCE_DIR}/src/hk1d_mock_pyrobot_logic.pyx
+    ${CMAKE_CURRENT_SOURCE_DIR}/script/hk1d_mock_pyrobot_logic_impl.py
+)
+
+# Copy the header file into the include directory
+add_custom_command(
+  OUTPUT ${LOGIC_INCLUDE_DIR}/hk1d_mock_pyrobot_logic.h
+  COMMAND ${CMAKE_COMMAND} -E copy
+    ${LOGIC_DIR}/hk1d_mock_pyrobot_logic.h ${LOGIC_INCLUDE_DIR}/hk1d_mock_pyrobot_logic.h
+  DEPENDS ${LOGIC_DIR}/hk1d_mock_pyrobot_logic.h
+)
+
+generate_parameter_library(
+  hk1d_mock_pyrobot_parameters
+  src/hk1d_mock_pyrobot_parameters.yaml
+)
+
+file (REMOVE ${PARAM_INCLUDE_DIR}/hk1d_mock_pyrobot_parameters.pxd)
+
+# Generate the pxd for the library
+add_custom_command(
+  OUTPUT ${PARAM_INCLUDE_DIR}/hk1d_mock_pyrobot_parameters.pxd
+  COMMAND ros2 run pytroller_tools generate_pxd ${PARAM_INCLUDE_DIR}/hk1d_mock_pyrobot_parameters.pxd ${PARAM_HEADER_FILE}
+  DEPENDS ${PARAM_HEADER_FILE}
+)
+
+add_library(hk1d_mock_pyrobot SHARED
+  src/hk1d_mock_pyrobot.cpp
+  ${LOGIC_DIR}/hk1d_mock_pyrobot_logic.cpp
+  ${LOGIC_INCLUDE_DIR}/hk1d_mock_pyrobot_logic.h
+  ${PARAM_INCLUDE_DIR}/hk1d_mock_pyrobot_parameters.pxd
+)
+target_compile_features(hk1d_mock_pyrobot PUBLIC cxx_std_17)
+target_include_directories(hk1d_mock_pyrobot PUBLIC
+  $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include/hk1d_mock_pyrobot>
+)
+target_include_directories(hk1d_mock_pyrobot PUBLIC
+  $<BUILD_INTERFACE:${LOGIC_DIR}/include>
+  $<INSTALL_INTERFACE:include/hk1d_mock_pyrobot>
+)
+target_link_libraries(hk1d_mock_pyrobot PUBLIC
+  hk1d_mock_pyrobot_parameters
+  ${PYTHON_LIBRARIES}
+)
+ament_target_dependencies(hk1d_mock_pyrobot PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS})
+# Causes the visibility macros to use dllexport rather than dllimport,
+# which is appropriate when building the dll but not consuming it.
+target_compile_definitions(hk1d_mock_pyrobot PRIVATE "PYTROLLER_BUILDING_DLL")
+pluginlib_export_plugin_description_file(controller_interface controller_plugin.xml)
+
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  find_package(ament_cmake_gmock REQUIRED)
+  find_package(controller_manager REQUIRED)
+  find_package(hardware_interface REQUIRED)
+  find_package(ros2_control_test_assets REQUIRED)
+
+  ament_lint_auto_find_test_dependencies()
+
+  # Load test
+  add_rostest_with_parameters_gmock(
+    test_load_hk1d_mock_pyrobot
+    test/test_load_hk1d_mock_pyrobot.cpp
+    ${CMAKE_CURRENT_SOURCE_DIR}/test/test_params.yaml
+  )
+  target_link_libraries(test_load_hk1d_mock_pyrobot
+    hk1d_mock_pyrobot
+  )
+  ament_target_dependencies(test_load_hk1d_mock_pyrobot
+    controller_manager
+    hardware_interface
+    ros2_control_test_assets
+  )
+
+  # Controller test
+  add_rostest_with_parameters_gmock(
+    test_hk1d_mock_pyrobot
+    test/test_hk1d_mock_pyrobot.cpp
+    ${CMAKE_CURRENT_SOURCE_DIR}/test/test_params.yaml
+  )
+
+  target_link_libraries(test_hk1d_mock_pyrobot
+    hk1d_mock_pyrobot
+  )
+
+  ament_target_dependencies(test_load_hk1d_mock_pyrobot
+    controller_manager
+    hardware_interface
+  )
+
+endif()
+
+install(
+  DIRECTORY ${LOGIC_DIR}/include
+  DESTINATION include/hk1d_mock_pyrobot
+)
+install(
+  DIRECTORY include/
+  DESTINATION include/hk1d_mock_pyrobot
+)
+install(
+  TARGETS
+    hk1d_mock_pyrobot
+    hk1d_mock_pyrobot_parameters
+  EXPORT export_hk1d_mock_pyrobot
+  RUNTIME DESTINATION bin
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  INCLUDES DESTINATION include
+)
+
+ament_export_targets(export_hk1d_mock_pyrobot HAS_LIBRARY_TARGET)
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+ament_package()

hk1d_mock_component_controllers/hk1d_mock_pyrobot/controller_plugin.xml

@@ -0,0 +1,9 @@
+<library path="hk1d_mock_pyrobot">
+  <class name="hk1d_mock_pyrobot/Hk1DMockPyrobot"
+        type="hk1d_mock_pyrobot::Hk1DMockPyrobot"
+        base_class_type="controller_interface::ControllerInterface">
+  <description>
+    Python controller for ros2_control
+  </description>
+  </class>
+</library>

hk1d_mock_component_controllers/hk1d_mock_pyrobot/include/hk1d_mock_pyrobot/hk1d_mock_pyrobot.hpp

@@ -0,0 +1,92 @@
+// Copyright 2023 ICube-Robotics
+//
+// 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.
+//
+// author: Maciej Bednarczyk
+
+#ifndef HK1D_MOCK_PYROBOT__HK1D_MOCK_PYROBOT_HPP_
+#define HK1D_MOCK_PYROBOT__HK1D_MOCK_PYROBOT_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+#include <unordered_map>
+
+#include "controller_interface/controller_interface.hpp"
+#include "hk1d_mock_pyrobot/visibility_control.h"
+#include "rclcpp/serialized_message.hpp"
+#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
+#include "rclcpp_lifecycle/state.hpp"
+#include "realtime_tools/realtime_buffer.h"
+#include "hk1d_mock_pyrobot_parameters.hpp"
+
+namespace hk1d_mock_pyrobot
+{
+/**
+ * \brief Python controller for a set of joints and interfaces.
+ *
+ * Subscribes to:
+ * - \b commands : The commands to apply.
+ */
+class Hk1DMockPyrobot : public controller_interface::ControllerInterface
+{
+public:
+  HK1D_MOCK_PYROBOT_PUBLIC
+  Hk1DMockPyrobot();
+
+  HK1D_MOCK_PYROBOT_PUBLIC
+  ~Hk1DMockPyrobot() = default;
+
+  HK1D_MOCK_PYROBOT_PUBLIC
+  controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+
+  HK1D_MOCK_PYROBOT_PUBLIC
+  controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+
+  HK1D_MOCK_PYROBOT_PUBLIC
+  controller_interface::CallbackReturn on_init() override;
+
+  HK1D_MOCK_PYROBOT_PUBLIC
+  controller_interface::CallbackReturn on_configure(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  HK1D_MOCK_PYROBOT_PUBLIC
+  controller_interface::CallbackReturn on_activate(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  HK1D_MOCK_PYROBOT_PUBLIC
+  controller_interface::CallbackReturn on_deactivate(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  HK1D_MOCK_PYROBOT_PUBLIC
+  controller_interface::return_type update(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+protected:
+  void declare_parameters();
+  controller_interface::CallbackReturn read_parameters();
+
+  std::shared_ptr<ParamListener> param_listener_;
+  Params params_;
+
+  std::vector<std::string> command_interface_types_;
+  std::unordered_map<std::string, double> states_;
+  std::unordered_map<std::string, double> commands_;
+
+  realtime_tools::RealtimeBuffer<std::shared_ptr<rclcpp::SerializedMessage>> rt_command_ptr_;
+  rclcpp::GenericSubscription::SharedPtr command_subscriber_;
+};
+
+}  // namespace hk1d_mock_pyrobot
+
+#endif  // HK1D_MOCK_PYROBOT__FORWARD_CONTROLLERS_BASE_HPP_

hk1d_mock_component_controllers/hk1d_mock_pyrobot/include/hk1d_mock_pyrobot/visibility_control.h

@@ -0,0 +1,56 @@
+// Copyright 2020 PAL Robotics S.L.
+//
+// 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  HK1D_MOCK_PYROBOT__VISIBILITY_CONTROL_H_
+#define  HK1D_MOCK_PYROBOT__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  HK1D_MOCK_PYROBOT_EXPORT __attribute__((dllexport))
+#define  HK1D_MOCK_PYROBOT_IMPORT __attribute__((dllimport))
+#else
+#define  HK1D_MOCK_PYROBOT_EXPORT __declspec(dllexport)
+#define  HK1D_MOCK_PYROBOT_IMPORT __declspec(dllimport)
+#endif
+#ifdef  HK1D_MOCK_PYROBOT_BUILDING_DLL
+#define  HK1D_MOCK_PYROBOT_PUBLIC  HK1D_MOCK_PYROBOT_EXPORT
+#else
+#define  HK1D_MOCK_PYROBOT_PUBLIC  HK1D_MOCK_PYROBOT_IMPORT
+#endif
+#define  HK1D_MOCK_PYROBOT_PUBLIC_TYPE  HK1D_MOCK_PYROBOT_PUBLIC
+#define  HK1D_MOCK_PYROBOT_LOCAL
+#else
+#define  HK1D_MOCK_PYROBOT_EXPORT __attribute__((visibility("default")))
+#define  HK1D_MOCK_PYROBOT_IMPORT
+#if __GNUC__ >= 4
+#define  HK1D_MOCK_PYROBOT_PUBLIC __attribute__((visibility("default")))
+#define  HK1D_MOCK_PYROBOT_LOCAL __attribute__((visibility("hidden")))
+#else
+#define  HK1D_MOCK_PYROBOT_PUBLIC
+#define  HK1D_MOCK_PYROBOT_LOCAL
+#endif
+#define  HK1D_MOCK_PYROBOT_PUBLIC_TYPE
+#endif
+
+#endif  //  HK1D_MOCK_PYROBOT__VISIBILITY_CONTROL_H_

hk1d_mock_component_controllers/hk1d_mock_pyrobot/package.xml

@@ -0,0 +1,30 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>hk1d_mock_pyrobot</name>
+  <version>0.0.1</version>
+  <description>Python controller for ros2_control.</description>
+  <maintainer email="[email protected]">maintainer</maintainer>
+
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>controller_interface</depend>
+  <depend>generate_parameter_library</depend>
+  <depend>hardware_interface</depend>
+  <depend>pluginlib</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_lifecycle</depend>
+  <depend>realtime_tools</depend>
+  <depend>cython3</depend>
+
+  <build_depend>pytroller_tools</build_depend>
+
+  <test_depend>ament_cmake_gmock</test_depend>
+  <test_depend>controller_manager</test_depend>
+  <test_depend>ros2_control_test_assets</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>