Confusion between ros2_control_bolt and ros2_hardware_interface_bolt

ros2_control_bolt/CMakeLists.txt

-# Update CMakeLists.txt from odri_control_interface to adapt it to Bolt
+cmake_minimum_required(VERSION 3.5)
+project(ros2_control_bolt)
 
-#
-# Copyright (c) 2020, New York University and Max Planck Gesellschaft.
-#
-# License BSD-3 clause
-#
-
-cmake_minimum_required(VERSION 3.1)
-
-# ----------------------------------------------------
-# --- CXX FLAGS --------------------------------------
-# ----------------------------------------------------
-
-set(CXX_DISABLE_WERROR True)
-set(CMAKE_VERBOSE_MAKEFILE True)
-
-# These variables have to be defined before running SETUP_PROJECT
-set(PROJECT_NAME ros2_control_bolt)
-set(PROJECT_DESCRIPTION
-    "Common interface for controlling robots build with the odri master board. Here it's for Bolt")
-set(PROJECT_URL
-    https://github.com/PaulRouanet/ros2_control_bolt.git)
-set(PROJECT_USE_CMAKE_EXPORT TRUE)
-
-# Check if the submodule cmake have been initialized
-if(NOT EXISTS "${CMAKE_SOURCE_DIR}/cmake/base.cmake")
-  message(
-    FATAL_ERROR
-      "\nPlease run the following command first:\ngit submodule update --init\n"
-  )
-endif()
-
-# --- OPTIONS ----------------------------------------
-option(BUILD_PYTHON_INTERFACE "Build the python binding" ON)
-option(PYTHON_STANDARD_LAYOUT "Enable standard Python package layout" ON)
-option(PYTHON_DEB_LAYOUT "Enable Debian-style Python package layout" OFF)
-
-include(cmake/base.cmake)
-include(cmake/boost.cmake)
-include(cmake/python.cmake)
-include(cmake/ide.cmake)
-
-compute_project_args(PROJECT_ARGS LANGUAGES CXX)
-project(${PROJECT_NAME} ${PROJECT_ARGS})
-check_minimal_cxx_standard(11 ENFORCE)
-
-# ----------------------------------------------------
-# --- DEPENDENCIES -----------------------------------
-# ----------------------------------------------------
-
-
-cmake_policy(SET CMP0057 NEW)
 find_package(ament_cmake REQUIRED)
-find_package(rclcpp REQUIRED)
-find_package(hardware_interface REQUIRED)
-find_package(odri_control_interface REQUIRED)
-find_package(controller_interface REQUIRED)
-find_package(master_board_sdk REQUIRED)
-
-
-
-
-
-
-#find_package(ros2_control REQUIRED)
-#find_package(ros2_control_demos REQUIRED)
-
-
-add_project_dependency(yaml-cpp CONFIG REQUIRED)
-add_project_dependency(Eigen3 REQUIRED PKG_CONFIG_REQUIRES "eigen3 >= 3.0.5")
-add_project_dependency(master_board_sdk REQUIRED)
-add_project_dependency(odri_control_interface REQUIRED)
-add_project_dependency(controller_interface REQUIRED)
-
-# Usefull for system_bolt
-#add_project_dependency(hardware_interface REQUIRED)
-#add_project_dependency(ament_cmake REQUIRED)
-#add_project_dependency(pluginlib REQUIRED)
-#add_project_dependency(rclcpp REQUIRED)
-#add_project_dependency(ros2_control REQUIRED)
-#add_project_dependency(ros2_control_demos REQUIRED)
-
-# Set component to fetch from boost Get the python interface for the bindings
-if(BUILD_PYTHON_INTERFACE)
-  findpython(REQUIRED)
-  search_for_boost_python(REQUIRED)
-  add_project_dependency(eigenpy 2.5.0 REQUIRED PKG_CONFIG_REQUIRES
-                         "eigenpy >= 2.5.0")
-endif(BUILD_PYTHON_INTERFACE)
-
-#ament_target_dependencies(rclcpp)
-
-
-#pluginlib_export_plugin_description_file(hardware_interface ros2_control_demo_hardware.xml)
-
-#ament_export_dependencies(
-#  hardware_interface
-#  pluginlib
-#  rclcpp
-#)
-#ament_package()
-ament_export_dependencies(odri_control_interface)
-
-
-
-
-# ----------------------------------------------------
-# --- INCLUDE ----------------------------------------
-# ----------------------------------------------------
-
-# --- MAIN LIBRARY -------------------------------------------------------------
-set(ROS2_CONTROL_BOLT_SRC src/system_bolt.cpp)
-add_library(${PROJECT_NAME} SHARED ${ROS2_CONTROL_BOLT_SRC})
-target_link_libraries(${PROJECT_NAME} ${YAML_CPP_LIBRARIES})
-target_link_libraries(${PROJECT_NAME} master_board_sdk::master_board_sdk)
-target_link_libraries(${PROJECT_NAME} Eigen3::Eigen)
-target_link_libraries(${PROJECT_NAME} rclcpp::rclcpp)
-target_link_libraries(${PROJECT_NAME} odri_control_interface::odri_control_interface)
-
-ament_target_dependencies(
-  ${PROJECT_NAME}
-  controller_interface
-  hardware_interface
-  pluginlib
-  rclcpp
-)
-
-target_include_directories(
-  ${PROJECT_NAME} PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-                         $<INSTALL_INTERFACE:include>)
-install(
-  TARGETS ${PROJECT_NAME}
-  EXPORT ${TARGETS_EXPORT_NAME}
-  DESTINATION lib)
-install(DIRECTORY include/ DESTINATION include)
-
-# --- BINDINGS -----------------------------------------------------------------
-
-if(BUILD_PYTHON_INTERFACE)
-  # --- Setup the wrapper name and source files --- #
-#  set(PYWRAP ${PROJECT_NAME}_pywrap)
-#  set(${PYWRAP}_HEADERS srcpy/bindings.h)
-#  set(${PYWRAP}_SOURCES srcpy/bindings.cpp)
-
-  # --- Build the wrapper library --- #
-  set(${PYWRAP}_INSTALL_DIR ${PYTHON_SITELIB})
-#  add_library(${PYWRAP} SHARED ${${PYWRAP}_SOURCES} ${${PYWRAP}_HEADERS})
-#  target_include_directories(${PYWRAP} SYSTEM PRIVATE ${PYTHON_INCLUDE_DIR})
-  target_link_libraries(${PYWRAP} ${PROJECT_NAME} eigenpy::eigenpy)
-#  target_link_boost_python(${PYWRAP})
-  set_target_properties(${PYWRAP} PROPERTIES INSTALL_RPATH
-                                             "${CMAKE_INSTALL_PREFIX}/lib")
-  install(TARGETS ${PYWRAP} DESTINATION ${${PYWRAP}_INSTALL_DIR})
-
-  # --- Allow to do: make python --- #
-  add_custom_target(python)
-  set_target_properties(python PROPERTIES EXCLUDE_FROM_DEFAULT_BUILD True)
-#  add_dependencies(python ${PYWRAP})
-
-endif(BUILD_PYTHON_INTERFACE)
-
-# --- CHECK DEMOS -----------------------------------------------------------
-macro(create_bolt_demo source)
-
-  set(demo_name ${PROJECT_NAME}_${source})
-  add_executable(${demo_name} test/${source}.cpp)
-  target_link_libraries(${demo_name} ${PROJECT_NAME})
-  target_compile_definitions(
-    ${demo_name}
-    PUBLIC CONFIG_BOLT_YAML="${PROJECT_SOURCE_DIR}/test/config_bolt.yaml")
-  ament_target_dependencies(
-  	${demo_name}
-    controller_interface
-  	hardware_interface
-  	pluginlib
-  	rclcpp
-  	odri_control_interface
-    master_board_sdk
-	)	
-  install(TARGETS ${demo_name} DESTINATION bin)
-
-endmacro(create_bolt_demo source)
-
-create_bolt_demo(demo_create_bolt_robot)
-create_bolt_demo(demo_bolt_from_yaml)
-create_bolt_demo(demo_bolt_sensor_reading)
-create_bolt_demo(demo_bolt_calibration)
-create_bolt_demo(demo_bolt_write_commands)
+ament_package()

ros2_control_bolt/package.xml

-<!-- Adaptation of package.xml from odri_control_interface -->
-
 <?xml version="1.0"?>
-<?xml-model
-   href="http://download.ros.org/schema/package_format3.xsd"
-   schematypens="http://www.w3.org/2001/XMLSchema"?>
-
-
 <package format="3">
+  <name>ros2_control_demos</name>
+  <version>0.0.0</version>
+  <description>Metapackage for `ros2_control` demos packages</description>
 
-    <name>ros2_control_bolt</name>
-    <version>1.0.0</version>
-
-    <description>
-	Common interface for controlling robots build with the odri master board.
-	Here its a test version for Bolt.
-    </description>
-
-    <maintainer email="prouanet@laas.fr">Paul Rouanet</maintainer>
-
-    <license>BSD 3-clause</license>
+  <author email="prouanet@laas.fr">Paul Rouanet</author>
 
-    <buildtool_depend>cmake</buildtool_depend>
-    <buildtool_depend>mpi_cmake_modules</buildtool_depend>
-    
-    <buildtool_depend>ament_cmake</buildtool_depend>
+  <maintainer email="prouanet@laas.fr">Paul Rouanet</maintainer>
 
-    <!-- CMake module containing helper to find the OS, boost, python, etc. -->
-    <depend>pybind11</depend>
-    <depend>Threads</depend>
-    <depend>master_board_sdk</depend>
-    <depend>Boost</depend>
-    <depend>Eigen3</depend>
-    <depend>eigenpy</depend>
-    
-    
-    <!-- Modules usefull for rrbot_system_bolt -->
-    <depend>hardware_interface</depend>
-    <depend>pluginlib</depend>
-    <depend>rclcpp</depend>
+  <license>Apache-2.0</license>
 
-    
-    
-    <test_depend>ament_cmake_gtest</test_depend>
+  <buildtool_depend>ament_cmake</buildtool_depend>
 
-    <export>
-      <build_type>ament_cmake</build_type>
-    </export>
+  <exec_depend>ros2_control_bolt_bringup</exec_depend>
+  <exec_depend>diffbot_description</exec_depend>
+  <exec_depend>rrbot_description</exec_depend>
+  <exec_depend>ros2_hardware_interface_bolt</exec_depend>
+  <exec_depend>ros2_control_test_nodes</exec_depend>
 
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
 </package>

ros2_hardware_interface_bolt/CMakeLists.txt

+# Update CMakeLists.txt from odri_control_interface to adapt it to Bolt
+
+#
+# Copyright (c) 2020, New York University and Max Planck Gesellschaft.
+#
+# License BSD-3 clause
+#
+
+cmake_minimum_required(VERSION 3.1)
+
+# ----------------------------------------------------
+# --- CXX FLAGS --------------------------------------
+# ----------------------------------------------------
+
+set(CXX_DISABLE_WERROR True)
+set(CMAKE_VERBOSE_MAKEFILE True)
+
+# These variables have to be defined before running SETUP_PROJECT
+set(PROJECT_NAME ros2_hardware_interface_bolt)
+set(PROJECT_DESCRIPTION
+    "Common interface for controlling robots build with the odri master board. Here it's for Bolt")
+set(PROJECT_URL
+    https://github.com/PaulRouanet/ros2_hardware_interface_bolt.git)
+set(PROJECT_USE_CMAKE_EXPORT TRUE)
+
+# Check if the submodule cmake have been initialized
+if(NOT EXISTS "${CMAKE_SOURCE_DIR}/cmake/base.cmake")
+  message(
+    FATAL_ERROR
+      "\nPlease run the following command first:\ngit submodule update --init\n"
+  )
+endif()
+
+# --- OPTIONS ----------------------------------------
+option(BUILD_PYTHON_INTERFACE "Build the python binding" ON)
+option(PYTHON_STANDARD_LAYOUT "Enable standard Python package layout" ON)
+option(PYTHON_DEB_LAYOUT "Enable Debian-style Python package layout" OFF)
+
+include(cmake/base.cmake)
+include(cmake/boost.cmake)
+include(cmake/python.cmake)
+include(cmake/ide.cmake)
+
+compute_project_args(PROJECT_ARGS LANGUAGES CXX)
+project(${PROJECT_NAME} ${PROJECT_ARGS})
+check_minimal_cxx_standard(11 ENFORCE)
+
+# ----------------------------------------------------
+# --- DEPENDENCIES -----------------------------------
+# ----------------------------------------------------
+
+
+cmake_policy(SET CMP0057 NEW)
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(hardware_interface REQUIRED)
+find_package(odri_control_interface REQUIRED)
+find_package(controller_interface REQUIRED)
+find_package(master_board_sdk REQUIRED)
+
+
+
+
+
+
+#find_package(ros2_control REQUIRED)
+#find_package(ros2_control_demos REQUIRED)
+
+
+add_project_dependency(yaml-cpp CONFIG REQUIRED)
+add_project_dependency(Eigen3 REQUIRED PKG_CONFIG_REQUIRES "eigen3 >= 3.0.5")
+add_project_dependency(master_board_sdk REQUIRED)
+add_project_dependency(odri_control_interface REQUIRED)
+add_project_dependency(controller_interface REQUIRED)
+
+# Usefull for system_bolt
+#add_project_dependency(hardware_interface REQUIRED)
+#add_project_dependency(ament_cmake REQUIRED)
+#add_project_dependency(pluginlib REQUIRED)
+#add_project_dependency(rclcpp REQUIRED)
+#add_project_dependency(ros2_control REQUIRED)
+#add_project_dependency(ros2_control_demos REQUIRED)
+
+# Set component to fetch from boost Get the python interface for the bindings
+if(BUILD_PYTHON_INTERFACE)
+  findpython(REQUIRED)
+  search_for_boost_python(REQUIRED)
+  add_project_dependency(eigenpy 2.5.0 REQUIRED PKG_CONFIG_REQUIRES
+                         "eigenpy >= 2.5.0")
+endif(BUILD_PYTHON_INTERFACE)
+
+#ament_target_dependencies(rclcpp)
+
+
+#pluginlib_export_plugin_description_file(hardware_interface ros2_control_demo_hardware.xml)
+
+#ament_export_dependencies(
+#  hardware_interface
+#  pluginlib
+#  rclcpp
+#)
+#ament_package()
+ament_export_dependencies(odri_control_interface)
+
+
+
+
+# ----------------------------------------------------
+# --- INCLUDE ----------------------------------------
+# ----------------------------------------------------
+
+# --- MAIN LIBRARY -------------------------------------------------------------
+set(ROS2_HARDWARE_INTERFACE_BOLT_SRC src/system_bolt.cpp)
+add_library(${PROJECT_NAME} SHARED ${ROS2_HARDWARE_INTERFACE_BOLT_SRC})
+target_link_libraries(${PROJECT_NAME} ${YAML_CPP_LIBRARIES})
+target_link_libraries(${PROJECT_NAME} master_board_sdk::master_board_sdk)
+target_link_libraries(${PROJECT_NAME} Eigen3::Eigen)
+target_link_libraries(${PROJECT_NAME} rclcpp::rclcpp)
+target_link_libraries(${PROJECT_NAME} odri_control_interface::odri_control_interface)
+
+ament_target_dependencies(
+  ${PROJECT_NAME}
+  controller_interface
+  hardware_interface
+  pluginlib
+  rclcpp
+)
+
+target_include_directories(
+  ${PROJECT_NAME} PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+                         $<INSTALL_INTERFACE:include>)
+install(
+  TARGETS ${PROJECT_NAME}
+  EXPORT ${TARGETS_EXPORT_NAME}
+  DESTINATION lib)
+install(DIRECTORY include/ DESTINATION include)
+
+# --- BINDINGS -----------------------------------------------------------------
+
+if(BUILD_PYTHON_INTERFACE)
+  # --- Setup the wrapper name and source files --- #
+#  set(PYWRAP ${PROJECT_NAME}_pywrap)
+#  set(${PYWRAP}_HEADERS srcpy/bindings.h)
+#  set(${PYWRAP}_SOURCES srcpy/bindings.cpp)
+
+  # --- Build the wrapper library --- #
+  set(${PYWRAP}_INSTALL_DIR ${PYTHON_SITELIB})
+#  add_library(${PYWRAP} SHARED ${${PYWRAP}_SOURCES} ${${PYWRAP}_HEADERS})
+#  target_include_directories(${PYWRAP} SYSTEM PRIVATE ${PYTHON_INCLUDE_DIR})
+  target_link_libraries(${PYWRAP} ${PROJECT_NAME} eigenpy::eigenpy)
+#  target_link_boost_python(${PYWRAP})
+  set_target_properties(${PYWRAP} PROPERTIES INSTALL_RPATH
+                                             "${CMAKE_INSTALL_PREFIX}/lib")
+  install(TARGETS ${PYWRAP} DESTINATION ${${PYWRAP}_INSTALL_DIR})
+
+  # --- Allow to do: make python --- #
+  add_custom_target(python)
+  set_target_properties(python PROPERTIES EXCLUDE_FROM_DEFAULT_BUILD True)
+#  add_dependencies(python ${PYWRAP})
+
+endif(BUILD_PYTHON_INTERFACE)
+
+# --- CHECK DEMOS -----------------------------------------------------------
+macro(create_bolt_demo source)
+
+  set(demo_name ${PROJECT_NAME}_${source})
+  add_executable(${demo_name} test/${source}.cpp)
+  target_link_libraries(${demo_name} ${PROJECT_NAME})
+  target_compile_definitions(
+    ${demo_name}
+    PUBLIC CONFIG_BOLT_YAML="${PROJECT_SOURCE_DIR}/test/config_bolt.yaml")
+  ament_target_dependencies(
+  	${demo_name}
+    controller_interface
+  	hardware_interface
+  	pluginlib
+  	rclcpp
+  	odri_control_interface
+    master_board_sdk
+	)	
+  install(TARGETS ${demo_name} DESTINATION bin)
+
+endmacro(create_bolt_demo source)
+
+create_bolt_demo(demo_create_bolt_robot)
+create_bolt_demo(demo_bolt_from_yaml)
+create_bolt_demo(demo_bolt_sensor_reading)
+create_bolt_demo(demo_bolt_calibration)
+create_bolt_demo(demo_bolt_write_commands)

ros2_hardware_interface_bolt/README.md

+# ros2_control_bolt
+----------------------
+
+### What it is
+
+A personal version of odri_control_interface demos adapt to Bolt ==> /test/.
+
+And an adaptation of Olivier Stasse's codes : rrbot_system_quadruped.hpp and .cpp in rrbot_system_bolt.hpp and .cpp (in /include/ and /src/).

ros2_hardware_interface_bolt/include/system_bolt.hpp

+// Copyright 2021 ros2_control Development Team
+//
+// 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 ROS2_CONTROL_BOLT__SYSTEM_BOLT_HPP_
+#define ROS2_CONTROL_BOLT__SYSTEM_BOLT_HPP_
+
+/*Connection to ODRI for read sensors and write commands*/
+#include "odri_control_interface/calibration.hpp"
+#include "odri_control_interface/robot.hpp"
+#include "odri_control_interface/imu.hpp"
+#include "master_board_sdk/master_board_interface.h"
+
+
+
+#include <memory>
+#include <set>
+#include <string>
+#include <vector>
+#include <map>
+#include "rclcpp/macros.hpp"
+
+#include "hardware_interface/base_interface.hpp"
+#include "hardware_interface/system_interface.hpp"
+#include "hardware_interface/handle.hpp"
+#include "hardware_interface/hardware_info.hpp"
+#include "hardware_interface/types/hardware_interface_return_values.hpp"
+#include "hardware_interface/types/hardware_interface_status_values.hpp"
+#include "visibility_control.h"
+#include "semantic_components/imu_sensor.hpp"
+
+
+
+using hardware_interface::return_type;
+
+#define rt_printf printf
+
+/**
+ * @brief Usefull tool for the demos and programs in order to print data in
+ * real time.
+ *
+ * @param v_name  is a string defining the data to print.
+ * @param v the vector to print.
+ */
+void print_vector(std::string v_name, const Eigen::Ref<const Eigen::VectorXd> v)
+{
+    v_name += ": [";
+    rt_printf("%s", v_name.c_str());
+    for (int i = 0; i < v.size(); ++i)
+    {
+        rt_printf("%0.3f, ", v(i));
+    }
+    rt_printf("]\n");
+}
+
+
+namespace Eigen
+{
+/** @brief Eigen shortcut for vector of size 6. */
+typedef Matrix<double, 6, 1> Vector6d;
+typedef Matrix<bool, 6, 1> Vector6b;
+typedef Matrix<long, 6, 1> Vector6l;
+typedef Matrix<int, 6, 1> Vector6i;
+/** @brief Eigen shortcut for vector of size 3. */
+typedef Matrix<long, 3, 1> Vector3l;
+/** @brief Eigen shortcut for vector of size 4. */
+typedef Matrix<long, 4, 1> Vector4l;
+
+}  // namespace Eigen
+
+
+namespace ros2_control_bolt
+{
+
+
+struct PosVelEffortGains
+{
+  double position;
+  double velocity;
+  double effort;
+  double Kp;
+  double Kd;
+};
+
+struct GyroAccLineEulerQuater
+{
+  Eigen::Vector3d gyro;
+  Eigen::Vector3d accelero;
+  Eigen::Vector3d line_acc;
+  Eigen::Vector3d euler;
+  Eigen::Vector4d quater;
+};
+
+constexpr const auto HW_IF_GAIN_KP = "gain_kp";
+constexpr const auto HW_IF_GAIN_KD = "gain_kd";
+
+std::set<std::string> bolt_list_of_cmd_inter {
+  "position",
+  "velocity",
+  "effort",
+  "gain_kp",
+  "gain_kd"
+};
+
+std::set<std::string> bolt_list_of_state_inter {
+  "position",
+  "velocity",
+  "effort",
+  "gain_kp",
+  "gain_kd"
+};
+
+enum control_mode_t {
+  POSITION,
+  VELOCITY,
+  EFFORT,
+  POS_VEL_EFF_GAINS,
+  NO_VALID_MODE
+};
+
+class SystemBoltHardware : public
+  hardware_interface::BaseInterface<hardware_interface::SystemInterface>
+{
+public:
+
+  RCLCPP_SHARED_PTR_DEFINITIONS(SystemBoltHardware)
+
+  ROS2_CONTROL_BOLT_PUBLIC
+  return_type configure();
+
+  ROS2_CONTROL_BOLT_PUBLIC
+  return_type init_robot();
+
+  ROS2_CONTROL_BOLT_PUBLIC
+  std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
+
+  ROS2_CONTROL_BOLT_PUBLIC
+  std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
+
+  ROS2_CONTROL_BOLT_PUBLIC
+  return_type prepare_command_mode_switch(
+    const std::vector<std::string> & start_interfaces,
+    const std::vector<std::string> & stop_interfaces) override;
+  
+  ROS2_CONTROL_BOLT_PUBLIC
+  return_type calibration();
+  
+  ROS2_CONTROL_BOLT_PUBLIC
+  return_type start()