Commit f8cf03bf authored by Paul Rouanet's avatar Paul Rouanet
Browse files

Confusion between ros2_control_bolt and ros2_hardware_interface_bolt

parent 986e58f9
# 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()
<!-- 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>
# 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_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/).
// 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() override;