Skip to content

Commit

Permalink
First empy shell of the ROS 2 controller
Browse files Browse the repository at this point in the history
  • Loading branch information
Maximilien Naveau committed Nov 4, 2024
1 parent 936a0cf commit 7d0ce01
Show file tree
Hide file tree
Showing 11 changed files with 959 additions and 26 deletions.
2 changes: 1 addition & 1 deletion .pre-commit-config.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -32,4 +32,4 @@ repos:
rev: v0.6.10
hooks:
- id: cmake-format
- id: cmake-lint
# - id: cmake-lint
27 changes: 19 additions & 8 deletions CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,20 @@ find_package(eigen3_cmake_module REQUIRED)
find_package(Eigen3)
find_package(pinocchio REQUIRED)

#
# Generate the ROS2 parameters interface automatically.
#
include(cmake/sec_generate_parameter_library.cmake)
# Main node params
sec_generate_parameter_library_markdown(${PROJECT_NAME}_parameters_doc
src/linear_feedback_controller.yaml)
sec_generate_parameter_library(
generated_parameters # Lib name
${PROJECT_NAME}_parameters # CMake target name for the parameter library.
src/linear_feedback_controller.yaml # Path to input yaml file
)
add_dependencies(${PROJECT_NAME}_parameters ${PROJECT_NAME}_parameters_doc)

#
# Main Library
#
Expand All @@ -31,27 +45,24 @@ set(${PROJECT_NAME}_headers
include/${PROJECT_NAME}/averaging_filter.hxx
include/${PROJECT_NAME}/contact_detector.hpp
include/${PROJECT_NAME}/lf_controller.hpp
include/${PROJECT_NAME}/controller.hpp
include/${PROJECT_NAME}/linear_feedback_controller.hpp
include/${PROJECT_NAME}/linear_feedback_controller_ros.hpp
include/${PROJECT_NAME}/min_jerk.hpp
include/${PROJECT_NAME}/pd_controller.hpp
include/${PROJECT_NAME}/robot_model_builder.hpp)
set(${PROJECT_NAME}_sources
src/contact_detector.cpp #
src/lf_controller.cpp #
src/controller.cpp #
src/linear_feedback_controller.cpp #
src/linear_feedback_controller_ros.cpp #
src/min_jerk.cpp #
src/pd_controller.cpp #
src/robot_model_builder.cpp)
ament_auto_add_library(${PROJECT_NAME} ${${PROJECT_NAME}_sources}
${${PROJECT_NAME}_headers})
ament_target_dependencies(${PROJECT_NAME} Eigen3)
ament_target_dependencies(${PROJECT_NAME} pinocchio)

#
# Plugins
#
# ament_auto_add_library(${PROJECT_NAME}_plugins src/plugins.cpp)
# target_link_libraries(${PROJECT_NAME}_plugins ${PROJECT_NAME})
target_link_libraries(${PROJECT_NAME} ${PROJECT_NAME}_parameters)

#
# Unit tests
Expand Down
174 changes: 174 additions & 0 deletions cmake/sec_generate_parameter_library.cmake
Original file line number Diff line number Diff line change
@@ -0,0 +1,174 @@
# Copyright 2022 PickNik Inc.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
# * Redistributions of source code must retain the above copyright notice, this
# list of conditions and the following disclaimer.
#
# * Redistributions in binary form must reproduce the above copyright notice,
# this list of conditions and the following disclaimer in the documentation
# and/or other materials provided with the distribution.
#
# * Neither the name of the PickNik Inc. nor the names of its contributors may
# be used to endorse or promote products derived from this software without
# specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

function(sec_generate_parameter_library LIB_NAME TARGET_NAME YAML_FILE)
find_program(generate_parameter_library_cpp_BIN
NAMES "generate_parameter_library_cpp")
if(NOT generate_parameter_library_cpp_BIN)
message(
FATAL_ERROR
"sec_generate_parameter_library() variable 'generate_parameter_library_cpp_BIN' must not be empty"
)
endif()

# Make the include directory
set(LIB_INCLUDE_DIR ${CMAKE_CURRENT_BINARY_DIR}/include/${PROJECT_NAME})
file(MAKE_DIRECTORY ${LIB_INCLUDE_DIR})

# Optional 4rd parameter for the user defined validation header
if(${ARGC} EQUAL 4)
cmake_path(SET IN_VALIDATE_HEADER ${CMAKE_CURRENT_SOURCE_DIR})
cmake_path(APPEND IN_VALIDATE_HEADER ${ARGV3})

cmake_path(GET IN_VALIDATE_HEADER FILENAME VALIDATE_HEADER_FILENAME)
cmake_path(SET VALIDATE_HEADER ${LIB_INCLUDE_DIR})
cmake_path(APPEND VALIDATE_HEADER ${VALIDATE_HEADER_FILENAME})

# Copy the header file into the include directory
add_custom_command(
OUTPUT ${VALIDATE_HEADER}
COMMAND ${CMAKE_COMMAND} -E copy ${IN_VALIDATE_HEADER} ${VALIDATE_HEADER}
DEPENDS ${IN_VALIDATE_HEADER}
COMMENT
"Running `${CMAKE_COMMAND} -E copy ${IN_VALIDATE_HEADER} ${VALIDATE_HEADER}`"
VERBATIM)
endif()

# Set the yaml file parameter to be relative to the current source dir
set(YAML_FILE ${CMAKE_CURRENT_SOURCE_DIR}/${YAML_FILE})

# Set the output parameter header file name
set(PARAM_HEADER_FILE ${LIB_INCLUDE_DIR}/${LIB_NAME}.hpp)

# Generate the header for the library
add_custom_command(
OUTPUT ${PARAM_HEADER_FILE}
COMMAND ${generate_parameter_library_cpp_BIN} ${PARAM_HEADER_FILE}
${YAML_FILE} ${VALIDATE_HEADER_FILENAME}
DEPENDS ${YAML_FILE} ${VALIDATE_HEADER}
COMMENT
"Running `${generate_parameter_library_cpp_BIN} ${PARAM_HEADER_FILE} ${YAML_FILE} ${VALIDATE_HEADER_FILENAME}`"
VERBATIM)

# Create the library target
add_library(${TARGET_NAME} INTERFACE ${PARAM_HEADER_FILE} ${VALIDATE_HEADER})
target_include_directories(
${TARGET_NAME}
INTERFACE $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/include/>
$<INSTALL_INTERFACE:include>)
set_target_properties(${TARGET_NAME} PROPERTIES LINKER_LANGUAGE CXX)
target_link_libraries(
${TARGET_NAME}
INTERFACE fmt::fmt
parameter_traits::parameter_traits
rclcpp::rclcpp
rclcpp_lifecycle::rclcpp_lifecycle
rsl::rsl
tcb_span::tcb_span
tl_expected::tl_expected)
install(DIRECTORY ${LIB_INCLUDE_DIR} DESTINATION include)
endfunction()

#
# Generate parameters documentation
#
function(sec_generate_parameter_library_markdown TARGET_NAME YAML_FILE)
find_program(generate_parameter_library_markdown_BIN
NAMES "generate_parameter_library_markdown")
if(NOT generate_parameter_library_markdown_BIN)
message(
FATAL_ERROR
"sec_generate_parameter_library_markdown() variable 'generate_parameter_library_markdown_BIN' must not be empty"
)
endif()

# Set the yaml file parameter to be relative to the current source dir
set(YAML_FILE ${CMAKE_CURRENT_SOURCE_DIR}/${YAML_FILE})

# Set the output parameter header file name
get_filename_component(MARKDOWN_FILE ${YAML_FILE} NAME_WE)
set(MARKDOWN_FILE
${CMAKE_CURRENT_BINARY_DIR}/doc/${MARKDOWN_FILE}/${MARKDOWN_FILE}.md)

# Generate the header for the library
add_custom_target(
${TARGET_NAME}
BYPRODUCTS ${MARKDOWN_FILE}
COMMAND ${generate_parameter_library_markdown_BIN} --input_yaml ${YAML_FILE}
--output_markdown_file ${MARKDOWN_FILE}
SOURCES ${YAML_FILE}
COMMENT
"Running `${generate_parameter_library_markdown_BIN} --input_yaml ${YAML_FILE} --output_markdown_file ${MARKDOWN_FILE}`"
VERBATIM)
endfunction()

# create custom test function to pass yaml file into test main
function(sec_add_rostest_with_parameters_gtest TARGET SOURCES YAML_FILE)
add_executable(${TARGET} ${SOURCES})
_ament_cmake_gtest_find_gtest()
target_include_directories(${TARGET} PUBLIC "${GTEST_INCLUDE_DIRS}")
target_link_libraries(${TARGET} ${GTEST_LIBRARIES})
set(executable "$<TARGET_FILE:${TARGET}>")
set(result_file
"${AMENT_TEST_RESULTS_DIR}/${PROJECT_NAME}/${TARGET}.gtest.xml")
ament_add_test(
${TARGET}
COMMAND
${executable}
--ros-args
--params-file
${YAML_FILE}
--
--gtest_output=xml:${result_file}
OUTPUT_FILE
${AMENT_TEST_RESULTS_DIR}/${PROJECT_NAME}/${TARGET}.txt
RESULT_FILE
${result_file})
endfunction()

function(sec_add_rostest_with_parameters_gmock TARGET SOURCES YAML_FILE)
add_executable(${TARGET} ${SOURCES})
_ament_cmake_gmock_find_gmock()
target_include_directories(${TARGET} PUBLIC "${GMOCK_INCLUDE_DIRS}")
target_link_libraries(${TARGET} ${GMOCK_LIBRARIES})
set(executable "$<TARGET_FILE:${TARGET}>")
set(result_file
"${AMENT_TEST_RESULTS_DIR}/${PROJECT_NAME}/${TARGET}.gtest.xml")
ament_add_test(
${TARGET}
COMMAND
${executable}
--ros-args
--params-file
${YAML_FILE}
--
--gtest_output=xml:${result_file}
OUTPUT_FILE
${AMENT_TEST_RESULTS_DIR}/${PROJECT_NAME}/${TARGET}.txt
RESULT_FILE
${result_file})
endfunction()
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,6 @@
#include "Eigen/Core"

// local include
#include "linear_feedback_controller/averaging_filter.hpp"
#include "linear_feedback_controller/contact_detector.hpp"
#include "linear_feedback_controller/lf_controller.hpp"
#include "linear_feedback_controller/min_jerk.hpp"
Expand Down Expand Up @@ -51,15 +50,15 @@ struct ControllerParameters {
* Hence it's the \f$ x_0 \f$ of the optimal control problem.
*
*/
class Controller {
class LinearFeedbackController {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

using Sensor = linear_feedback_controller_msgs::Eigen::Sensor;
using Control = linear_feedback_controller_msgs::Eigen::Control;

Controller();
~Controller();
LinearFeedbackController();
~LinearFeedbackController();

bool load(const ControllerParameters& params);

Expand All @@ -77,6 +76,8 @@ class Controller {
const Eigen::VectorXd& compute_control(TimePoint time, Sensor sensor,
Control control);

RobotModelBuilder::ConstSharedPtr getRobotModel() const;

private:
ControllerParameters params_; /*! @brief Parameters of the controller. */
/// @brief Control to be sent to the low-level controller.
Expand Down
Loading

0 comments on commit 7d0ce01

Please sign in to comment.