From 41e08cf86e4551f14ddfa86b414b9de4e8aed8c1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Sun, 19 Jun 2022 11:10:50 +0200 Subject: [PATCH] Extend controller templates, update hardware templates and update scripts to be easier to use (#48) * Update templates for controller * Added template for chained controllers. * Update hardware templates and make nice output to the hardware generation script. --- .github/workflows/ci-format.yml | 4 +- .pre-commit-config.yaml | 2 +- scripts/_RosTeamWs_Defines.bash | 7 +- scripts/_Team_Defines.bash | 2 +- scripts/create-new-package.bash | 40 +- .../setup-controller-package.bash | 79 +++- .../setup-hardware-interface-package.bash | 72 +-- scripts/setup-repository-ci.bash | 2 +- ...thub_reusable-industrial-ci-with-cache.yml | 12 +- .../package/CI-github_semi-binary-build.yml | 1 + .../controller/dummy_chainable_controller.cpp | 274 +++++++++++ .../dummy_chainable_controller_pluginlib.xml | 8 + .../controller/dummy_controller.cpp | 132 ++++-- .../dummy_chainable_controller.hpp | 112 +++++ .../dummy_controller.hpp | 55 ++- .../visibility_control.h | 49 ++ .../test_dummy_chainable_controller.cpp | 435 ++++++++++++++++++ .../test_dummy_chainable_controller.hpp | 294 ++++++++++++ .../controller/test_dummy_controller.cpp | 193 +++++++- .../controller/test_dummy_controller.hpp | 137 ++++-- .../robot_hardware_interface.hpp | 31 +- .../visibility_control.h | 49 ++ .../hardware/robot_hardware_interface.cpp | 15 +- .../hardware/visibility_control.h | 49 -- 24 files changed, 1782 insertions(+), 272 deletions(-) create mode 100644 templates/ros2_control/controller/dummy_chainable_controller.cpp create mode 100644 templates/ros2_control/controller/dummy_chainable_controller_pluginlib.xml create mode 100644 templates/ros2_control/controller/dummy_package_namespace/dummy_chainable_controller.hpp create mode 100644 templates/ros2_control/controller/dummy_package_namespace/visibility_control.h create mode 100644 templates/ros2_control/controller/test_dummy_chainable_controller.cpp create mode 100644 templates/ros2_control/controller/test_dummy_chainable_controller.hpp create mode 100644 templates/ros2_control/hardware/dummy_package_namespace/visibility_control.h delete mode 100644 templates/ros2_control/hardware/visibility_control.h diff --git a/.github/workflows/ci-format.yml b/.github/workflows/ci-format.yml index dee6f4b1..a5d175a5 100644 --- a/.github/workflows/ci-format.yml +++ b/.github/workflows/ci-format.yml @@ -18,8 +18,8 @@ jobs: with: python-version: '3.9.7' cache: 'pip' - - name: Install clang-format-10 - run: sudo apt-get install clang-format-11 + - name: Install clang-format-12 + run: sudo apt-get install clang-format-12 - uses: pre-commit/action@v2.0.3 with: extra_args: --all-files --hook-stage manual diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index e9b474c5..7fa53196 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -80,7 +80,7 @@ repos: - id: clang-format name: clang-format description: Format files with ClangFormat. - entry: clang-format-10 + entry: clang-format-12 language: system files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$ args: ['-fallback-style=none', '-i'] diff --git a/scripts/_RosTeamWs_Defines.bash b/scripts/_RosTeamWs_Defines.bash index 46731a3c..eda41332 100755 --- a/scripts/_RosTeamWs_Defines.bash +++ b/scripts/_RosTeamWs_Defines.bash @@ -55,6 +55,9 @@ function RosTeamWS_setup_exports { export RAW_TERMINAL_COLOR_LIGHT_GRAY=$'\e[0;37m' export RAW_TERMINAL_COLOR_WHITE=$'\e[1;37m' + export TERMINAL_COLOR_USER_NOTICE=${TERMINAL_COLOR_YELLOW} + export TERMINAL_COLOR_USER_INPUT_DECISION=${TERMINAL_COLOR_PURPLE} + export TERMINAL_COLOR_USER_CONFIRMATION=${TERMINAL_COLOR_BLUE} } # TODO(denis): add this into setup.bash @@ -247,8 +250,8 @@ function framework_default_paths { PACKAGE_TEMPLATES="$FRAMEWORK_PACKAGE_PATH/templates/package" ROBOT_DESCRIPTION_TEMPLATES="$FRAMEWORK_PACKAGE_PATH/templates/robot_description" ROS2_CONTROL_TEMPLATES="$FRAMEWORK_PACKAGE_PATH/templates/ros2_control" - ROS2_CONTROL_HW_ITF_TEMPLATES="$FRAMEWORK_PACKAGE_PATH/templates/ros2_control/hardware" - ROS2_CONTROL_CONTROLLER_TEMPLATES="$FRAMEWORK_PACKAGE_PATH/templates/ros2_control/controller" + ROS2_CONTROL_HW_ITF_TEMPLATES="$ROS2_CONTROL_TEMPLATES/hardware" + ROS2_CONTROL_CONTROLLER_TEMPLATES="$ROS2_CONTROL_TEMPLATES/controller" LICENSE_TEMPLATES="$FRAMEWORK_PACKAGE_PATH/templates/licenses" } diff --git a/scripts/_Team_Defines.bash b/scripts/_Team_Defines.bash index 6e5a6fc0..ee23f114 100644 --- a/scripts/_Team_Defines.bash +++ b/scripts/_Team_Defines.bash @@ -20,7 +20,7 @@ DEFAULT_ROS_VERSION=2 # TODO(denis): We have two example teams. On is working with industrial and other with mobile robots # TEAM_TEAM_NAMES=("Industrial" "Mobile") -TEAM_LICENSE="Apache License 2.0" +TEAM_LICENSE="Apache-2.0" TEAM_REPOSITORY_SERVER="https://github.com" diff --git a/scripts/create-new-package.bash b/scripts/create-new-package.bash index 6003d8c2..a9f082b9 100755 --- a/scripts/create-new-package.bash +++ b/scripts/create-new-package.bash @@ -9,26 +9,28 @@ check_ros_distro ${ROS_DISTRO} PKG_NAME=$1 if [ -z "$1" ]; then - print_and_exit "Package name is not defined. Nothing to do, exiting..." "$usage" + print_and_exit "Package name is not defined. Nothing to do 😯" "$usage" fi PKG_DESCRIPTION=$2 if [ -z "$2" ]; then - print_and_exit "Package description is not defined. Nothing to do, exiting..." "$usage" + print_and_exit "Package description is not defined. Nothing to do 😯" "$usage" fi echo "" -echo "Your path is `pwd`. Is this your workspace source folder?" -read -p "If so press otherwise +C and start the script again from your source folder." +echo -e "${TERMINAL_COLOR_USER_NOTICE}Your path is `pwd`. Is this your workspace source folder?${TERMINAL_COLOR_NC}" +echo -e "${TERMINAL_COLOR_USER_CONFIRMATION}If so press otherwise +C and start the script again from your source folder.${TERMINAL_COLOR_NC}" +read -read -p "What type of package you want to create? (0 - standard, 1 - metapackage, 2 - subpackage) [0]:" META +echo -n -e "${TERMINAL_COLOR_USER_INPUT_DECISION}What type of package you want to create? (0 - standard, 1 - metapackage, 2 - subpackage) [0]:${TERMINAL_COLOR_NC}" +read META META=${META:=0} CREATE_PARAMS="" case "$META" in "1") - echo "Meta-package '$PKG_NAME' will be created!" + echo -e "${TERMINAL_COLOR_USER_NOTICE}Meta-package '$PKG_NAME' will be created!${TERMINAL_COLOR_NC}" CREATE_PARAMS="--meta" mkdir $PKG_NAME cd $PKG_NAME @@ -42,16 +44,17 @@ case "$META" in if [[ ! -d $META_NAME ]]; then echo "ERROR: metapackage with the name '$META_NAME' does not exist! Exiting..." fi - echo "Subpackage '$PKG_NAME' will be created in the metapackage '$META_NAME'!" + echo -e "${TERMINAL_COLOR_USER_NOTICE}Subpackage '$PKG_NAME' will be created in the metapackage '$META_NAME'!${TERMINAL_COLOR_NC}" cd $META_NAME # TODO: read licence of the meta-package ;; *) - echo "Package '$PKG_NAME' will be created!" + echo -e "${TERMINAL_COLOR_USER_NOTICE}Package '$PKG_NAME' will be created!${TERMINAL_COLOR_NC}" esac -read -p "Do you want to enter name and email address of the maintainer? If not, data from git configuration will be used. (y/n) [n]: " choice +echo -n -e "${TERMINAL_COLOR_USER_INPUT_DECISION}Do you want to enter name and email address of the maintainer? If not, data from git configuration will be used. (y/n) [n]: ${TERMINAL_COLOR_NC}" +read choice choice=${choice:="n"} case "$choice" in @@ -73,7 +76,8 @@ case "$choice" in esac # License -read -p "How do you want to licence your package? ['$TEAM_LICENSE']: " LICENSE +echo -n -e "${TERMINAL_COLOR_USER_INPUT_DECISION}How do you want to licence your package? ['$TEAM_LICENSE']: ${TERMINAL_COLOR_NC}" +read LICENSE LICENSE=${LICENSE:=$TEAM_LICENSE} if [[ $ros_version == 1 ]]; then @@ -81,7 +85,8 @@ if [[ $ros_version == 1 ]]; then elif [[ $ros_version == 2 ]]; then # Build type - read -p "Please choose your package build type (1 - ament_cmake, 2 - ament_python, 3 - cmake) [1]:" choice + echo -n -e "${TERMINAL_COLOR_USER_INPUT_DECISION}Please choose your package build type (1 - ament_cmake, 2 - ament_python, 3 - cmake) [1]:${TERMINAL_COLOR_NC}" + read choice case "$choice" in "2") @@ -96,9 +101,9 @@ elif [[ $ros_version == 2 ]]; then fi echo "" -echo "ATTENTION: Creating package '$PKG_NAME' in '`pwd`' with description '$PKG_DESCRIPTION', licence '$LICENSE', build type '$BUILD_TYPE' and maintainer '$MAINTAINER_NAME <$MAINTAINER_EMAIL>'" -read -p "If correct press , otherwise +C and start the script again from your source folder." - +echo -e "${TERMINAL_COLOR_USER_INPUT_DECISION}ATTENTION: Creating package '$PKG_NAME' in '`pwd`' with description '$PKG_DESCRIPTION', licence '$LICENSE', build type '$BUILD_TYPE' and maintainer '$MAINTAINER_NAME <$MAINTAINER_EMAIL>'${TERMINAL_COLOR_NC}" +echo -e "${TERMINAL_COLOR_USER_CONFIRMATION}If correct press , otherwise +C and start the script again from your source folder.${TERMINAL_COLOR_NC}" +read if [[ $ros_version == 1 ]]; then catkin_create_pkg $CREATE_PARAMS $PKG_NAME -V 0.0.1 -l "$LICENSE" -D "$PKG_DESCRIPTION" @@ -121,8 +126,9 @@ elif [[ $ros_version == 2 ]]; then fi fi -read -p "Do you want to setup/update repository with the new package configuration? (y/n) [y]: " choice -choice=${choice:="y"} +echo -n -e "${TERMINAL_COLOR_USER_INPUT_DECISION}Do you want to setup/update repository with the new package configuration? (y/n) [n]: ${TERMINAL_COLOR_NC}" +read choice +choice=${choice:="n"} case "$choice" in "y") @@ -187,5 +193,5 @@ case "$choice" in fi ;; "n") - echo "Repository configuration is _not_ updated!" + echo -e "${TERMINAL_COLOR_USER_NOTICE}Repository configuration is _not_ updated!${TERMINAL_COLOR_NC}" esac diff --git a/scripts/ros2_control/setup-controller-package.bash b/scripts/ros2_control/setup-controller-package.bash index 6ac0ea0c..eb573731 100755 --- a/scripts/ros2_control/setup-controller-package.bash +++ b/scripts/ros2_control/setup-controller-package.bash @@ -27,12 +27,14 @@ check_ros_distro ${ROS_DISTRO} FILE_NAME=$1 if [ -z "$1" ]; then - print_and_exit "ERROR: You should provide the file name!" "$usage" + print_and_exit "You should provide the file name! Nothing to do 😯" "$usage" fi if [ -f src/$FILE_NAME.cpp ]; then - print_and_exit "ERROR:The file '$FILE_NAME' already exist!" "$usage" + print_and_exit "ERROR:The file '$FILE_NAME' already exist! 😱!" "$usage" fi +echo "" # Adds empty line + CLASS_NAME=$2 if [ -z "$2" ]; then delimiter='_' @@ -43,25 +45,28 @@ if [ -z "$2" ]; then s=${s#*"$delimiter"} CLASS_NAME="$CLASS_NAME${part^}" done - echo "ClassName guessed from the '$FILE_NAME': '$CLASS_NAME'. Is this correct? If not, provide it as the second parameter." + echo -e "${TERMINAL_COLOR_USER_CONFIRMATION}ClassName guessed from the '$FILE_NAME': '$CLASS_NAME'. Is this correct? If not, provide it as the second parameter.${TERMINAL_COLOR_NC}" fi PKG_NAME=$3 if [ -z "$3" ]; then current=$(pwd) PKG_NAME=$(basename "$current") - echo "Package name guessed from the current path is '$PKG_NAME'. Is this correct? If not provide it as the third parameter." + echo -e "${TERMINAL_COLOR_USER_CONFIRMATION}Package name guessed from the current path is '$PKG_NAME'. Is this correct? If not provide it as the third parameter.${TERMINAL_COLOR_NC}" fi +read -echo "Which license-header do you want to use? [1]" +echo -e "${TERMINAL_COLOR_USER_INPUT_DECISION}Which license-header do you want to use? [1]" echo "(0) None" -echo "(1) Apache 2.0 License" +echo "(1) Apache-2.0" echo "(2) Propiatery" +echo -n -e "${TERMINAL_COLOR_NC}" read choice choice=${choice:="1"} if [ "$choice" != 0 ]; then - read -p "Insert your company or personal name (copyright): " NAME_ON_LICENSE + echo -n -e "${TERMINAL_COLOR_USER_INPUT_DECISION}Insert your company or personal name (copyright): ${TERMINAL_COLOR_NC}" + read NAME_ON_LICENSE NAME_ON_LICENSE=${NAME_ON_LICENSE=""} YEAR_ON_LICENSE=$(date +%Y) fi @@ -75,13 +80,31 @@ case "$choice" in LICENSE_HEADER="$LICENSE_TEMPLATES/propriatery_company_cpp.txt" esac -read -p "Is package already configured (is in there a working controller already)? (yes/no) [no] " package_configured +echo -n -e "${TERMINAL_COLOR_USER_INPUT_DECISION}Is package already configured (is in there a working controller already)? (yes/no) [no] ${TERMINAL_COLOR_NC}" +read package_configured package_configured=${package_configured:="no"} +echo -e "${TERMINAL_COLOR_USER_INPUT_DECISION}Do you want to setup a 'normal' or 'chainable' controller? [1]" +echo "(1) normal (single-level control)" +echo "(2) chainable" +echo -n -e "${TERMINAL_COLOR_NC}" +read choice +choice=${choice:="1"} + +CONTROLLER_TYPE="" +case "$choice" in +"1") + CONTROLLER_TYPE="normal (single-level control)" + ;; +"2") + CONTROLLER_TYPE="chainable" +esac + echo "" -echo "ATTENTION: Setting up ros2_control controller files with following parameters: file name '$FILE_NAME', class '$CLASS_NAME', package/namespace '$PKG_NAME'. Those will be placed in folder '`pwd`'." +echo -e "${TERMINAL_COLOR_USER_NOTICE}ATTENTION: Setting up ros2_control controller files with following parameters: file name '$FILE_NAME', class '$CLASS_NAME', package/namespace '$PKG_NAME', type '$CONTROLLER_TYPE'. Those will be placed in folder '`pwd`'.${TERMINAL_COLOR_NC}" echo "" -read -p "If correct press , otherwise +C and start the script again from the package folder and/or with correct controller name." +echo -e "${TERMINAL_COLOR_USER_CONFIRMATION}If correct press , otherwise +C and start the script again from the package folder and/or with correct controller name.${TERMINAL_COLOR_NC}" +read # Add folders if deleted ADD_FOLDERS=("include/$PKG_NAME" "src" "test") @@ -101,16 +124,26 @@ TEST_HPP="test/test_$FILE_NAME.hpp" # Copy files if [[ ! -f "$VC_H" ]]; then - cp -n $ROS2_CONTROL_HW_ITF_TEMPLATES/visibility_control.h $VC_H + cp -n $ROS2_CONTROL_CONTROLLER_TEMPLATES/dummy_package_namespace/visibility_control.h $VC_H fi -cat $ROS2_CONTROL_CONTROLLER_TEMPLATES/dummy_controller_pluginlib.xml >> $PLUGIN_XML -cp -n $ROS2_CONTROL_CONTROLLER_TEMPLATES/dummy_package_namespace/dummy_controller.hpp $CTRL_HPP -cp -n $ROS2_CONTROL_CONTROLLER_TEMPLATES/dummy_controller.cpp $CTRL_CPP + +if [[ "$CONTROLLER_TYPE" == "chainable" ]]; then + cp -n $ROS2_CONTROL_CONTROLLER_TEMPLATES/dummy_package_namespace/dummy_chainable_controller.hpp $CTRL_HPP + cp -n $ROS2_CONTROL_CONTROLLER_TEMPLATES/dummy_chainable_controller.cpp $CTRL_CPP + cp -n $ROS2_CONTROL_CONTROLLER_TEMPLATES/test_dummy_chainable_controller.cpp $TEST_CPP + cp -n $ROS2_CONTROL_CONTROLLER_TEMPLATES/test_dummy_chainable_controller.hpp $TEST_HPP + cat $ROS2_CONTROL_CONTROLLER_TEMPLATES/dummy_chainable_controller_pluginlib.xml >> $PLUGIN_XML +else + cp -n $ROS2_CONTROL_CONTROLLER_TEMPLATES/dummy_package_namespace/dummy_controller.hpp $CTRL_HPP + cp -n $ROS2_CONTROL_CONTROLLER_TEMPLATES/dummy_controller.cpp $CTRL_CPP + cp -n $ROS2_CONTROL_CONTROLLER_TEMPLATES/test_dummy_controller.cpp $TEST_CPP + cp -n $ROS2_CONTROL_CONTROLLER_TEMPLATES/test_dummy_controller.hpp $TEST_HPP + cat $ROS2_CONTROL_CONTROLLER_TEMPLATES/dummy_controller_pluginlib.xml >> $PLUGIN_XML +fi + cp -n $ROS2_CONTROL_CONTROLLER_TEMPLATES/test_load_dummy_controller.cpp $LOAD_TEST_CPP -cp -n $ROS2_CONTROL_CONTROLLER_TEMPLATES/test_dummy_controller.cpp $TEST_CPP -cp -n $ROS2_CONTROL_CONTROLLER_TEMPLATES/test_dummy_controller.hpp $TEST_HPP -echo "Template files copied." +echo -e "${TERMINAL_COLOR_USER_NOTICE}Template files copied.${TERMINAL_COLOR_NC}" # TODO(anyone): fuse this with hardware interface package creating. @@ -139,11 +172,11 @@ FILES_TO_SED+=("$PLUGIN_XML") for SED_FILE in "${FILES_TO_SED[@]}"; do sed -i "s/TEMPLATES__ROS2_CONTROL__CONTROLLER__DUMMY_PACKAGE_NAMESPACE/${PKG_NAME^^}/g" $SED_FILE # package name for include guard - sed -i "s/TEMPLATES__ROS2_CONTROL__CONTROLLER/${PKG_NAME^^}/g" $SED_FILE # package name for include guard - sed -i "s/TEMPLATES__ROS2_CONTROL__HARDWARE/${PKG_NAME^^}/g" $SED_FILE # package name for include guard from hardware sed -i "s/dummy_package_namespace/${PKG_NAME}/g" $SED_FILE # package name for includes sed -i "s/dummy_controller/${FILE_NAME}/g" $SED_FILE # file name + sed -i "s/dummy_chainable_controller/${FILE_NAME}/g" $SED_FILE # file name sed -i "s/DUMMY_CONTROLLER/${FILE_NAME^^}/g" $SED_FILE # file name for include guard + sed -i "s/DUMMY_CHAINABLE_CONTROLLER/${FILE_NAME^^}/g" $SED_FILE # file name for include guard sed -i "s/DummyClassName/${CLASS_NAME}/g" $SED_FILE # class name sed -i "s/dummy_interface_type/${INTERFACE_TYPE}/g" $SED_FILE # interface type for includes sed -i "s/Dummy_Interface_Type/${INTERFACE_TYPE^}/g" $SED_FILE # Interface type in namespace resolution @@ -188,6 +221,7 @@ echo " pluginlib" >> $TMP_FILE echo " rclcpp" >> $TMP_FILE echo " rclcpp_lifecycle" >> $TMP_FILE echo " realtime_tools" >> $TMP_FILE +echo " std_srvs" >> $TMP_FILE echo ")" >> $TMP_FILE if [[ "$package_configured" == "no" ]]; then @@ -270,6 +304,7 @@ if [[ "$package_configured" == "no" ]]; then echo " rclcpp" >> $TMP_FILE echo " rclcpp_lifecycle" >> $TMP_FILE echo " realtime_tools" >> $TMP_FILE + echo " std_srvs" >> $TMP_FILE echo ")" >> $TMP_FILE fi @@ -281,7 +316,7 @@ tail -n +$TEST_LINE CMakeLists.txt | tail -n +$CUT_LINE >> $TMP_FILE mv $TMP_FILE CMakeLists.txt # CMakeLists.txt & package.xml: Add dependencies if they not exist -DEP_PKGS=("realtime_tools" "rclcpp_lifecycle" "rclcpp" "pluginlib" "hardware_interface" "controller_interface" "control_msgs") +DEP_PKGS=("std_srvs" "realtime_tools" "rclcpp_lifecycle" "rclcpp" "pluginlib" "hardware_interface" "controller_interface" "control_msgs") for DEP_PKG in "${DEP_PKGS[@]}"; do @@ -339,7 +374,7 @@ if [ -f README.md ]; then fi -echo "Template files are adjusted." +echo -e "${TERMINAL_COLOR_USER_NOTICE}Template files were adjusted.${TERMINAL_COLOR_NC}" git add . # git commit -m "RosTeamWS: ros2_control skeleton files for $ROBOT_NAME generated." @@ -350,4 +385,4 @@ git add . compile_and_source_package $PKG_NAME "yes" echo "" -echo "FINISHED: Your package is set and the tests should be finished without any errors. (linter errors possible!)" +echo -e "${TERMINAL_COLOR_USER_NOTICE}FINISHED: Your package is set and the tests should be finished without any errors. (linter errors possible!)${TERMINAL_COLOR_NC}" diff --git a/scripts/ros2_control/setup-hardware-interface-package.bash b/scripts/ros2_control/setup-hardware-interface-package.bash index 09781f54..0d2f4881 100755 --- a/scripts/ros2_control/setup-hardware-interface-package.bash +++ b/scripts/ros2_control/setup-hardware-interface-package.bash @@ -27,12 +27,14 @@ check_ros_distro ${ROS_DISTRO} FILE_NAME=$1 if [ -z "$1" ]; then - print_and_exit "ERROR: You should provide the file name!" "$usage" + print_and_exit "You should provide the file name! Nothing to do 😯" "$usage" fi if [ -f src/$FILE_NAME.cpp ]; then - print_and_exit "ERROR:The file '$FILE_NAME' already exist!" "$usage" + print_and_exit "ERROR:The file '$FILE_NAME' already exist! 😱!" "$usage" fi +echo "" # Adds empty line + CLASS_NAME=$2 if [ -z "$2" ]; then delimiter='_' @@ -43,41 +45,27 @@ if [ -z "$2" ]; then s=${s#*"$delimiter"} CLASS_NAME="$CLASS_NAME${part^}" done - echo "ClassName guessed from the '$FILE_NAME': '$CLASS_NAME'. Is this correct? If not provide it as the second parameter." + echo -e "${TERMINAL_COLOR_USER_CONFIRMATION}ClassName guessed from the '$FILE_NAME': '$CLASS_NAME'. Is this correct? If not provide it as the second parameter.${TERMINAL_COLOR_NC}" fi PKG_NAME=$3 if [ -z "$3" ]; then current=`pwd` PKG_NAME=$(basename "$current") - echo "Package name guessed from the current path is '$PKG_NAME'. Is this correct? If not provide it as the third parameter." + echo -e "${TERMINAL_COLOR_USER_CONFIRMATION}Package name guessed from the current path is '$PKG_NAME'. Is this correct? If not provide it as the third parameter.${TERMINAL_COLOR_NC}" fi -echo "Which type of ros2_control hardware interface you want to extend? [0]" -echo "(0) system" -echo "(1) sensor" -echo "(2) actuator" -read choice -choice=${choice="0"} - -INTERFACE_TYPE="system" -case "$choice" in -"1") - INTERFACE_TYPE="sensor" - ;; -"2") - INTERFACE_TYPE="actuator" -esac - -echo "Which license-header do you want to use? [1]" +echo -e "${TERMINAL_COLOR_USER_INPUT_DECISION}Which license-header do you want to use? [1]" echo "(0) None" echo "(1) Apache 2.0 License" echo "(2) Propiatery" +echo -n -e "${TERMINAL_COLOR_NC}" read choice choice=${choice:="1"} if [ "$choice" != 0 ]; then - read -p "Insert your company or personal name (copyright): " NAME_ON_LICENSE + echo -n -e "${TERMINAL_COLOR_USER_INPUT_DECISION}Insert your company or personal name (copyright): ${TERMINAL_COLOR_NC}" + read NAME_ON_LICENSE NAME_ON_LICENSE=${NAME_ON_LICENSE=""} YEAR_ON_LICENSE=`date +%Y` fi @@ -91,10 +79,28 @@ case "$choice" in LICENSE_HEADER="$LICENSE_TEMPLATES/propriatery_company_cpp.txt" esac +echo -e "${TERMINAL_COLOR_USER_INPUT_DECISION}Which type of ros2_control hardware interface you want to extend? [0]" +echo "(0) system" +echo "(1) sensor" +echo "(2) actuator" +echo -n -e "${TERMINAL_COLOR_NC}" +read choice +choice=${choice="0"} + +INTERFACE_TYPE="system" +case "$choice" in +"1") + INTERFACE_TYPE="sensor" + ;; +"2") + INTERFACE_TYPE="actuator" +esac + echo "" -echo "ATTENTION: Setting up ros2_control hardware interface files with following parameters: file name '$FILE_NAME', class '$CLASS_NAME', package/namespace '$PKG_NAME' for interface type '$INTERFACE_TYPE'. Those will be placed in folder '`pwd`'." +echo -e "${TERMINAL_COLOR_USER_NOTICE}ATTENTION: Setting up ros2_control hardware interface files with following parameters: file name '$FILE_NAME', class '$CLASS_NAME', package/namespace '$PKG_NAME' for interface type '$INTERFACE_TYPE'. Those will be placed in folder '`pwd`'.${TERMINAL_COLOR_NC}" echo "" -read -p "If correct press , otherwise +C and start the script again from the package folder and/or with correct robot name." +echo -e "${TERMINAL_COLOR_USER_CONFIRMATION}If correct press , otherwise +C and start the script again from the package folder and/or with correct robot name.${TERMINAL_COLOR_NC}" +read # Add folders if deleted ADD_FOLDERS=("include/$PKG_NAME" "src" "test") @@ -111,13 +117,15 @@ PLUGIN_XML="$PKG_NAME.xml" TEST_CPP="test/test_$FILE_NAME.cpp" # Copy files -cp -n $ROS2_CONTROL_HW_ITF_TEMPLATES/visibility_control.h $VC_H +if [[ ! -f "$VC_H" ]]; then + cp -n $ROS2_CONTROL_HW_ITF_TEMPLATES/dummy_package_namespace/visibility_control.h $VC_H +fi cp -n $ROS2_CONTROL_HW_ITF_TEMPLATES/dummy_package_namespace/robot_hardware_interface.hpp $HW_ITF_HPP cp -n $ROS2_CONTROL_HW_ITF_TEMPLATES/robot_hardware_interface.cpp $HW_ITF_CPP cp -n $ROS2_CONTROL_HW_ITF_TEMPLATES/robot_pluginlib.xml $PLUGIN_XML cp -n $ROS2_CONTROL_HW_ITF_TEMPLATES/test_robot_hardware_interface.cpp $TEST_CPP -echo "Template files copied." +echo -e "${TERMINAL_COLOR_USER_NOTICE}Template files copied.${TERMINAL_COLOR_NC}" # Add license header to the files # TODO: When Propiatery then add the following before ament_lint_auto_find_test_dependencies() @@ -159,18 +167,18 @@ done # If type is "sensor" remove write and command_interfaces methods if [[ "$INTERFACE_TYPE" == "sensor" ]]; then - METHODS_TO_DELETE=("write()" "export_command_interfaces()") + METHODS_TO_DELETE=("write(" "export_command_interfaces()") # Clean HPP for DEL_METHOD in "${METHODS_TO_DELETE[@]}"; do line_nr=`grep -n "${DEL_METHOD}" $HW_ITF_HPP | awk -F ":" '{print $1;}'` let start_line=${line_nr}-1 - let end_line=${line_nr}+1 + let end_line=${line_nr}+2 sed -i "${start_line},${end_line}d" $HW_ITF_HPP done # Clean CPP - line_nr=`grep -n "write()" $HW_ITF_CPP | awk -F ":" '{print $1;}'` + line_nr=`grep -n "::write(" $HW_ITF_CPP | awk -F ":" '{print $1;}'` let start_line=${line_nr}-1 - let end_line=${line_nr}+5 + let end_line=${line_nr}+6 sed -i "${start_line},${end_line}d" $HW_ITF_CPP line_nr=`grep -n "export_command_interfaces()" $HW_ITF_CPP | awk -F ":" '{print $1;}'` @@ -339,7 +347,7 @@ if [ -f README.md ]; then fi -echo "Template files are adjusted." +echo -e "${TERMINAL_COLOR_USER_NOTICE}Template files were adjusted.${TERMINAL_COLOR_NC}" git add . # git commit -m "RosTeamWS: ros2_control skeleton files for $ROBOT_NAME generated." @@ -348,4 +356,4 @@ git add . compile_and_source_package $PKG_NAME "yes" echo "" -echo "FINISHED: Your package is set and the tests should be finished without any errors. (linter errors possible!)" +echo -e "${TERMINAL_COLOR_USER_NOTICE}FINISHED: Your package is set and the tests should be finished without any errors. (linter errors possible!)${TERMINAL_COLOR_NC}" diff --git a/scripts/setup-repository-ci.bash b/scripts/setup-repository-ci.bash index 7b161bcc..be752a72 100755 --- a/scripts/setup-repository-ci.bash +++ b/scripts/setup-repository-ci.bash @@ -1,6 +1,6 @@ #!/bin/bash # -# Copyright 2021 Stogl Denis Stogl (Stogl Robotics Consulting) +# Copyright 2021 Denis Stogl (Stogl Robotics Consulting) # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/templates/package/CI-github_reusable-industrial-ci-with-cache.yml b/templates/package/CI-github_reusable-industrial-ci-with-cache.yml index 9d272209..1a9818f3 100644 --- a/templates/package/CI-github_reusable-industrial-ci-with-cache.yml +++ b/templates/package/CI-github_reusable-industrial-ci-with-cache.yml @@ -20,8 +20,8 @@ on: required: true type: string ros_repo: - description: 'ROS_REPO to run for industrial_ci. Possible values: "all", "main", "testing"' - default: 'all' + description: 'ROS_REPO to run for industrial_ci. Possible values: "main", "testing"' + default: 'main' required: false type: string os_code_name: @@ -49,11 +49,7 @@ on: jobs: reusable_industrial_ci_with_cache: - name: ${{ matrix.ROS_REPO }} ${{ inputs.ros_distro }} ${{ inputs.os_code_name }} - strategy: - fail-fast: false - matrix: - ROS_REPO: [ main, testing ] + name: ${{ inputs.ros_distro }} ${{ inputs.ros_repo }} ${{ inputs.os_code_name }} runs-on: ubuntu-latest env: CCACHE_DIR: ${{ github.workspace }}/${{ inputs.ccache_dir }} @@ -90,7 +86,7 @@ jobs: env: UPSTREAM_WORKSPACE: ${{ inputs.upstream_workspace }} ROS_DISTRO: ${{ inputs.ros_distro }} - ROS_REPO: ${{ matrix.ROS_REPO }} + ROS_REPO: ${{ inputs.ros_repo }} OS_CODE_NAME: ${{ inputs.os_code_name }} BEFORE_INSTALL_UPSTREAM_DEPENDENCIES: ${{ inputs.before_install_upstream_dependencies }} - name: prepare target_ws for cache diff --git a/templates/package/CI-github_semi-binary-build.yml b/templates/package/CI-github_semi-binary-build.yml index 0fba3bce..bfcbdcdf 100644 --- a/templates/package/CI-github_semi-binary-build.yml +++ b/templates/package/CI-github_semi-binary-build.yml @@ -17,5 +17,6 @@ jobs: uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml with: ros_distro: $ROS_DISTRO$ + ros_repo: $ROS_REPO$ upstream_workspace: $NAME$.$ROS_DISTRO$.repos ref_for_scheduled_build: $branch$ diff --git a/templates/ros2_control/controller/dummy_chainable_controller.cpp b/templates/ros2_control/controller/dummy_chainable_controller.cpp new file mode 100644 index 00000000..73a3a0f7 --- /dev/null +++ b/templates/ros2_control/controller/dummy_chainable_controller.cpp @@ -0,0 +1,274 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// 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 "dummy_package_namespace/dummy_chainable_controller.hpp" + +#include +#include +#include +#include + +#include "controller_interface/helpers.hpp" + +namespace +{ // utility + +// TODO(destogl): remove this when merged upstream +// Changed services history QoS to keep all so we don't lose any client service calls +static constexpr rmw_qos_profile_t rmw_qos_profile_services_hist_keep_all = { + RMW_QOS_POLICY_HISTORY_KEEP_ALL, + 1, // message queue depth + RMW_QOS_POLICY_RELIABILITY_RELIABLE, + RMW_QOS_POLICY_DURABILITY_VOLATILE, + RMW_QOS_DEADLINE_DEFAULT, + RMW_QOS_LIFESPAN_DEFAULT, + RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, + RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, + false}; + +using ControllerCommandMsg = dummy_package_namespace::DummyClassName::ControllerCommandMsg; + +// called from RT control loop +void reset_controller_command_msg( + const std::shared_ptr & msg, const std::vector & joint_names) +{ + msg->joint_names = joint_names; + msg->displacements.resize(joint_names.size(), std::numeric_limits::quiet_NaN()); + msg->velocities.resize(joint_names.size(), std::numeric_limits::quiet_NaN()); + msg->duration = std::numeric_limits::quiet_NaN(); +} + +} // namespace + +namespace dummy_package_namespace +{ +DummyClassName::DummyClassName() : controller_interface::ChainableControllerInterface() {} + +controller_interface::CallbackReturn DummyClassName::on_init() +{ + control_mode_.initRT(control_mode_type::FAST); + + try { + get_node()->declare_parameter>("joints", std::vector({})); + get_node()->declare_parameter>( + "state_joints", std::vector({})); + get_node()->declare_parameter("interface_name", ""); + } catch (const std::exception & e) { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn DummyClassName::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + auto error_if_empty = [&](const auto & parameter, const char * parameter_name) { + if (parameter.empty()) { + RCLCPP_ERROR(get_node()->get_logger(), "'%s' parameter was empty", parameter_name); + return true; + } + return false; + }; + + auto get_string_array_param_and_error_if_empty = + [&](std::vector & parameter, const char * parameter_name) { + parameter = get_node()->get_parameter(parameter_name).as_string_array(); + return error_if_empty(parameter, parameter_name); + }; + + auto get_string_param_and_error_if_empty = + [&](std::string & parameter, const char * parameter_name) { + parameter = get_node()->get_parameter(parameter_name).as_string(); + return error_if_empty(parameter, parameter_name); + }; + + if ( + get_string_array_param_and_error_if_empty(joint_names_, "joints") || + get_string_array_param_and_error_if_empty(state_joint_names_, "state_joints") || + get_string_param_and_error_if_empty(interface_name_, "interface_name")) { + return controller_interface::CallbackReturn::ERROR; + } + + // Command Subscriber and callbacks + auto callback_cmd = [&](const std::shared_ptr msg) -> void { + if (msg->joint_names.size() == joint_names_.size()) { + input_cmd_.writeFromNonRT(msg); + } else { + RCLCPP_ERROR( + get_node()->get_logger(), + "Received %zu , but expected %zu joints in command. Ignoring message.", + msg->joint_names.size(), joint_names_.size()); + } + }; + cmd_subscriber_ = get_node()->create_subscription( + "~/commands", rclcpp::SystemDefaultsQoS(), callback_cmd); + + std::shared_ptr msg = std::make_shared(); + reset_controller_command_msg(msg, joint_names_); + input_cmd_.writeFromNonRT(msg); + + auto set_slow_mode_service_callback = + [&]( + const std::shared_ptr request, + std::shared_ptr response) { + if (request->data) { + control_mode_.writeFromNonRT(control_mode_type::SLOW); + } else { + control_mode_.writeFromNonRT(control_mode_type::FAST); + } + response->success = true; + }; + + set_slow_control_mode_service_ = get_node()->create_service( + "~/set_slow_control_mode", set_slow_mode_service_callback, + rmw_qos_profile_services_hist_keep_all); + + try { + // State publisher + s_publisher_ = + get_node()->create_publisher("~/state", rclcpp::SystemDefaultsQoS()); + state_publisher_ = std::make_unique(s_publisher_); + } catch (const std::exception & e) { + fprintf( + stderr, "Exception thrown during publisher creation at configure stage with message : %s \n", + e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + // TODO(anyone): Reserve memory in state publisher depending on the message type + state_publisher_->lock(); + state_publisher_->msg_.header.frame_id = joint_names_[0]; + state_publisher_->unlock(); + + RCLCPP_INFO(get_node()->get_logger(), "configure successful"); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration DummyClassName::command_interface_configuration() const +{ + controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + command_interfaces_config.names.reserve(joint_names_.size()); + for (const auto & joint : joint_names_) { + command_interfaces_config.names.push_back(joint + "/" + interface_name_); + } + + return command_interfaces_config; +} + +controller_interface::InterfaceConfiguration DummyClassName::state_interface_configuration() const +{ + controller_interface::InterfaceConfiguration state_interfaces_config; + state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + state_interfaces_config.names.reserve(state_joint_names_.size()); + for (const auto & joint : state_joint_names_) { + state_interfaces_config.names.push_back(joint + "/" + interface_name_); + } + + return state_interfaces_config; +} + +std::vector DummyClassName::on_export_reference_interfaces() +{ + reference_interfaces_.resize(joint_names_.size(), std::numeric_limits::quiet_NaN()); + + std::vector reference_interfaces; + reference_interfaces.reserve(reference_interfaces_.size()); + + for (size_t i = 0; i < reference_interfaces_.size(); ++i) { + reference_interfaces.push_back(hardware_interface::CommandInterface( + get_node()->get_name(), joint_names_[i] + "/" + interface_name_, &reference_interfaces_[i])); + } + + return reference_interfaces; +} + +bool DummyClassName::on_set_chained_mode(bool chained_mode) +{ + // Always accept switch to/from chained mode + return true || chained_mode; +} + +controller_interface::CallbackReturn DummyClassName::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // Set default value in command + reset_controller_command_msg(*(input_cmd_.readFromRT()), joint_names_); + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn DummyClassName::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // TODO(anyone): depending on number of interfaces, use definitions, e.g., `CMD_MY_ITFS`, + // instead of a loop + for (size_t i = 0; i < command_interfaces_.size(); ++i) { + command_interfaces_[i].set_value(std::numeric_limits::quiet_NaN()); + } + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type DummyClassName::update_reference_from_subscribers() +{ + auto current_cmd = input_cmd_.readFromRT(); + + // TODO(anyone): depending on number of interfaces, use definitions, e.g., `CMD_MY_ITFS`, + // instead of a loop + for (size_t i = 0; i < reference_interfaces_.size(); ++i) { + if (!std::isnan((*current_cmd)->displacements[i])) { + reference_interfaces_[i] = (*current_cmd)->displacements[i]; + + (*current_cmd)->displacements[i] = std::numeric_limits::quiet_NaN(); + } + } + return controller_interface::return_type::OK; +} + +controller_interface::return_type DummyClassName::update_and_write_commands( + const rclcpp::Time & time, const rclcpp::Duration & /*period*/) +{ + // TODO(anyone): depending on number of interfaces, use definitions, e.g., `CMD_MY_ITFS`, + // instead of a loop + for (size_t i = 0; i < command_interfaces_.size(); ++i) { + if (!std::isnan(reference_interfaces_[i])) { + if (*(control_mode_.readFromRT()) == control_mode_type::SLOW) { + reference_interfaces_[i] /= 2; + } + command_interfaces_[i].set_value(reference_interfaces_[i]); + + reference_interfaces_[i] = std::numeric_limits::quiet_NaN(); + } + } + + if (state_publisher_ && state_publisher_->trylock()) { + state_publisher_->msg_.header.stamp = time; + state_publisher_->msg_.set_point = command_interfaces_[CMD_MY_ITFS].get_value(); + + state_publisher_->unlockAndPublish(); + } + + return controller_interface::return_type::OK; +} + +} // namespace dummy_package_namespace + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + dummy_package_namespace::DummyClassName, controller_interface::ChainableControllerInterface) diff --git a/templates/ros2_control/controller/dummy_chainable_controller_pluginlib.xml b/templates/ros2_control/controller/dummy_chainable_controller_pluginlib.xml new file mode 100644 index 00000000..6a355f80 --- /dev/null +++ b/templates/ros2_control/controller/dummy_chainable_controller_pluginlib.xml @@ -0,0 +1,8 @@ + + + + DummyClassName ros2_control controller. + + + diff --git a/templates/ros2_control/controller/dummy_controller.cpp b/templates/ros2_control/controller/dummy_controller.cpp index 28f7b60b..2e383b5e 100644 --- a/templates/ros2_control/controller/dummy_controller.cpp +++ b/templates/ros2_control/controller/dummy_controller.cpp @@ -21,24 +21,59 @@ #include "controller_interface/helpers.hpp" +namespace +{ // utility + +// TODO(destogl): remove this when merged upstream +// Changed services history QoS to keep all so we don't lose any client service calls +static constexpr rmw_qos_profile_t rmw_qos_profile_services_hist_keep_all = { + RMW_QOS_POLICY_HISTORY_KEEP_ALL, + 1, // message queue depth + RMW_QOS_POLICY_RELIABILITY_RELIABLE, + RMW_QOS_POLICY_DURABILITY_VOLATILE, + RMW_QOS_DEADLINE_DEFAULT, + RMW_QOS_LIFESPAN_DEFAULT, + RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, + RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, + false}; + +using ControllerCommandMsg = dummy_package_namespace::DummyClassName::ControllerCommandMsg; + +// called from RT control loop +void reset_controller_command_msg( + std::shared_ptr & msg, const std::vector & joint_names) +{ + msg->joint_names = joint_names; + msg->displacements.resize(joint_names.size(), std::numeric_limits::quiet_NaN()); + msg->velocities.resize(joint_names.size(), std::numeric_limits::quiet_NaN()); + msg->duration = std::numeric_limits::quiet_NaN(); +} + +} // namespace + namespace dummy_package_namespace { DummyClassName::DummyClassName() : controller_interface::ControllerInterface() {} -CallbackReturn DummyClassName::on_init() +controller_interface::CallbackReturn DummyClassName::on_init() { + control_mode_.initRT(control_mode_type::FAST); + try { get_node()->declare_parameter>("joints", std::vector({})); + get_node()->declare_parameter>( + "state_joints", std::vector({})); get_node()->declare_parameter("interface_name", ""); } catch (const std::exception & e) { fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); - return CallbackReturn::ERROR; + return controller_interface::CallbackReturn::ERROR; } - return CallbackReturn::SUCCESS; + return controller_interface::CallbackReturn::SUCCESS; } -CallbackReturn DummyClassName::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) +controller_interface::CallbackReturn DummyClassName::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) { auto error_if_empty = [&](const auto & parameter, const char * parameter_name) { if (parameter.empty()) { @@ -62,14 +97,15 @@ CallbackReturn DummyClassName::on_configure(const rclcpp_lifecycle::State & /*pr if ( get_string_array_param_and_error_if_empty(joint_names_, "joints") || + get_string_array_param_and_error_if_empty(state_joint_names_, "state_joints") || get_string_param_and_error_if_empty(interface_name_, "interface_name")) { - return CallbackReturn::ERROR; + return controller_interface::CallbackReturn::ERROR; } // Command Subscriber and callbacks - auto callback_command = [&](const std::shared_ptr msg) -> void { + auto callback_cmd = [&](const std::shared_ptr msg) -> void { if (msg->joint_names.size() == joint_names_.size()) { - input_command_.writeFromNonRT(msg); + input_cmd_.writeFromNonRT(msg); } else { RCLCPP_ERROR( get_node()->get_logger(), @@ -77,21 +113,48 @@ CallbackReturn DummyClassName::on_configure(const rclcpp_lifecycle::State & /*pr msg->joint_names.size(), joint_names_.size()); } }; - command_subscriber_ = get_node()->create_subscription( - "~/commands", rclcpp::SystemDefaultsQoS(), callback_command); + cmd_subscriber_ = get_node()->create_subscription( + "~/commands", rclcpp::SystemDefaultsQoS(), callback_cmd); - // State publisher - s_publisher_ = - get_node()->create_publisher("~/state", rclcpp::SystemDefaultsQoS()); - state_publisher_ = std::make_unique(s_publisher_); + std::shared_ptr msg = std::make_shared(); + reset_controller_command_msg(msg, joint_names_); + input_cmd_.writeFromNonRT(msg); + + auto set_slow_mode_service_callback = + [&]( + const std::shared_ptr request, + std::shared_ptr response) { + if (request->data) { + control_mode_.writeFromNonRT(control_mode_type::SLOW); + } else { + control_mode_.writeFromNonRT(control_mode_type::FAST); + } + response->success = true; + }; + + set_slow_control_mode_service_ = get_node()->create_service( + "~/set_slow_control_mode", set_slow_mode_service_callback, + rmw_qos_profile_services_hist_keep_all); + + try { + // State publisher + s_publisher_ = + get_node()->create_publisher("~/state", rclcpp::SystemDefaultsQoS()); + state_publisher_ = std::make_unique(s_publisher_); + } catch (const std::exception & e) { + fprintf( + stderr, "Exception thrown during publisher creation at configure stage with message : %s \n", + e.what()); + return controller_interface::CallbackReturn::ERROR; + } // TODO(anyone): Reserve memory in state publisher depending on the message type state_publisher_->lock(); state_publisher_->msg_.header.frame_id = joint_names_[0]; state_publisher_->unlock(); - RCLCPP_INFO_STREAM(get_node()->get_logger(), "configure successful"); - return CallbackReturn::SUCCESS; + RCLCPP_INFO(get_node()->get_logger(), "configure successful"); + return controller_interface::CallbackReturn::SUCCESS; } controller_interface::InterfaceConfiguration DummyClassName::command_interface_configuration() const @@ -112,44 +175,55 @@ controller_interface::InterfaceConfiguration DummyClassName::state_interface_con controller_interface::InterfaceConfiguration state_interfaces_config; state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - state_interfaces_config.names.reserve(joint_names_.size()); - for (const auto & joint : joint_names_) { + state_interfaces_config.names.reserve(state_joint_names_.size()); + for (const auto & joint : state_joint_names_) { state_interfaces_config.names.push_back(joint + "/" + interface_name_); } return state_interfaces_config; } -CallbackReturn DummyClassName::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) +controller_interface::CallbackReturn DummyClassName::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) { // Set default value in command - std::shared_ptr msg = std::make_shared(); - msg->joint_names = joint_names_; - msg->displacements.resize(joint_names_.size(), std::numeric_limits::quiet_NaN()); - input_command_.writeFromNonRT(msg); + reset_controller_command_msg(*(input_cmd_.readFromRT)(), joint_names_); - return CallbackReturn::SUCCESS; + return controller_interface::CallbackReturn::SUCCESS; } -CallbackReturn DummyClassName::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) +controller_interface::CallbackReturn DummyClassName::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) { - return CallbackReturn::SUCCESS; + // TODO(anyone): depending on number of interfaces, use definitions, e.g., `CMD_MY_ITFS`, + // instead of a loop + for (size_t i = 0; i < command_interfaces_.size(); ++i) { + command_interfaces_[i].set_value(std::numeric_limits::quiet_NaN()); + } + return controller_interface::CallbackReturn::SUCCESS; } controller_interface::return_type DummyClassName::update( const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { - auto current_command = input_command_.readFromRT(); + auto current_cmd = input_cmd_.readFromRT(); + // TODO(anyone): depending on number of interfaces, use definitions, e.g., `CMD_MY_ITFS`, + // instead of a loop for (size_t i = 0; i < command_interfaces_.size(); ++i) { - if (!std::isnan((*current_command)->displacements[i])) { - command_interfaces_[i].set_value((*current_command)->displacements[i]); + if (!std::isnan((*current_cmd)->displacements[i])) { + if (*(control_mode_.readFromRT()) == control_mode_type::SLOW) { + (*current_cmd)->displacements[i] /= 2; + } + command_interfaces_[i].set_value((*current_cmd)->displacements[i]); + + (*current_cmd)->displacements[i] = std::numeric_limits::quiet_NaN(); } } if (state_publisher_ && state_publisher_->trylock()) { state_publisher_->msg_.header.stamp = time; - state_publisher_->msg_.set_point = command_interfaces_[0].get_value(); + state_publisher_->msg_.set_point = command_interfaces_[CMD_MY_ITFS].get_value(); state_publisher_->unlockAndPublish(); } diff --git a/templates/ros2_control/controller/dummy_package_namespace/dummy_chainable_controller.hpp b/templates/ros2_control/controller/dummy_package_namespace/dummy_chainable_controller.hpp new file mode 100644 index 00000000..179b3d78 --- /dev/null +++ b/templates/ros2_control/controller/dummy_package_namespace/dummy_chainable_controller.hpp @@ -0,0 +1,112 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// 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 TEMPLATES__ROS2_CONTROL__CONTROLLER__DUMMY_PACKAGE_NAMESPACE__DUMMY_CHAINABLE_CONTROLLER_HPP_ +#define TEMPLATES__ROS2_CONTROL__CONTROLLER__DUMMY_PACKAGE_NAMESPACE__DUMMY_CHAINABLE_CONTROLLER_HPP_ + +#include +#include +#include + +#include "controller_interface/chainable_controller_interface.hpp" +#include "dummy_package_namespace/visibility_control.h" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_publisher.h" +#include "std_srvs/srv/set_bool.hpp" + +// TODO(anyone): Replace with controller specific messages +#include "control_msgs/msg/joint_controller_state.hpp" +#include "control_msgs/msg/joint_jog.hpp" + +namespace dummy_package_namespace +{ +// name constants for state interfaces +static constexpr size_t STATE_MY_ITFS = 0; + +// name constants for command interfaces +static constexpr size_t CMD_MY_ITFS = 0; + +// TODO(anyone: xample setup for control mode (usually you will use some enums defined in messages) +enum class control_mode_type : std::uint8_t { + FAST = 0, + SLOW = 1, +}; + +class DummyClassName : public controller_interface::ChainableControllerInterface +{ +public: + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + DummyClassName(); + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_init() override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + controller_interface::return_type update_reference_from_subscribers() override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + controller_interface::return_type update_and_write_commands( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + // TODO(anyone): replace the state and command message types + using ControllerCommandMsg = control_msgs::msg::JointJog; + using ControllerModeSrvType = std_srvs::srv::SetBool; + using ControllerStateMsg = control_msgs::msg::JointControllerState; + +protected: + std::vector joint_names_; + std::vector state_joint_names_; + std::string interface_name_; + + // Command subscribers and Controller State publisher + rclcpp::Subscription::SharedPtr cmd_subscriber_ = nullptr; + realtime_tools::RealtimeBuffer> input_cmd_; + + rclcpp::Service::SharedPtr set_slow_control_mode_service_; + realtime_tools::RealtimeBuffer control_mode_; + + using ControllerStatePublisher = realtime_tools::RealtimePublisher; + + rclcpp::Publisher::SharedPtr s_publisher_; + std::unique_ptr state_publisher_; + + // override methods from ChainableControllerInterface + std::vector on_export_reference_interfaces() override; + + bool on_set_chained_mode(bool chained_mode) override; +}; + +} // namespace dummy_package_namespace + +#endif // TEMPLATES__ROS2_CONTROL__CONTROLLER__DUMMY_PACKAGE_NAMESPACE__DUMMY_CHAINABLE_CONTROLLER_HPP_ diff --git a/templates/ros2_control/controller/dummy_package_namespace/dummy_controller.hpp b/templates/ros2_control/controller/dummy_package_namespace/dummy_controller.hpp index f91f90b1..5231193b 100644 --- a/templates/ros2_control/controller/dummy_package_namespace/dummy_controller.hpp +++ b/templates/ros2_control/controller/dummy_package_namespace/dummy_controller.hpp @@ -25,6 +25,7 @@ #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_buffer.h" #include "realtime_tools/realtime_publisher.h" +#include "std_srvs/srv/set_bool.hpp" // TODO(anyone): Replace with controller specific messages #include "control_msgs/msg/joint_controller_state.hpp" @@ -32,48 +33,66 @@ namespace dummy_package_namespace { -using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; +// name constants for state interfaces +static constexpr size_t STATE_MY_ITFS = 0; + +// name constants for command interfaces +static constexpr size_t CMD_MY_ITFS = 0; + +// TODO(anyone: xample setup for control mode (usually you will use some enums defined in messages) +enum class control_mode_type : std::uint8_t { + FAST = 0, + SLOW = 1, +}; class DummyClassName : public controller_interface::ControllerInterface { public: - TEMPLATES__ROS2_CONTROL__CONTROLLER_PUBLIC + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC DummyClassName(); - TEMPLATES__ROS2_CONTROL__CONTROLLER_PUBLIC - CallbackReturn on_init() override; + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_init() override; - TEMPLATES__ROS2_CONTROL__CONTROLLER_PUBLIC + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC controller_interface::InterfaceConfiguration command_interface_configuration() const override; - TEMPLATES__ROS2_CONTROL__CONTROLLER_PUBLIC + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC controller_interface::InterfaceConfiguration state_interface_configuration() const override; - TEMPLATES__ROS2_CONTROL__CONTROLLER_PUBLIC - CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; - TEMPLATES__ROS2_CONTROL__CONTROLLER_PUBLIC - CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; - TEMPLATES__ROS2_CONTROL__CONTROLLER_PUBLIC - CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; - TEMPLATES__ROS2_CONTROL__CONTROLLER_PUBLIC + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC controller_interface::return_type update( const rclcpp::Time & time, const rclcpp::Duration & period) override; + // TODO(anyone): replace the state and command message types + using ControllerCommandMsg = control_msgs::msg::JointJog; + using ControllerModeSrvType = std_srvs::srv::SetBool; + using ControllerStateMsg = control_msgs::msg::JointControllerState; + protected: std::vector joint_names_; + std::vector state_joint_names_; std::string interface_name_; - // TODO(anyone): replace the state and command message types // Command subscribers and Controller State publisher - using ControllerCommandMsg = control_msgs::msg::JointJog; + rclcpp::Subscription::SharedPtr cmd_subscriber_ = nullptr; + realtime_tools::RealtimeBuffer> input_cmd_; - rclcpp::Subscription::SharedPtr command_subscriber_ = nullptr; - realtime_tools::RealtimeBuffer> input_command_; + rclcpp::Service::SharedPtr set_slow_control_mode_service_; + realtime_tools::RealtimeBuffer control_mode_; - using ControllerStateMsg = control_msgs::msg::JointControllerState; using ControllerStatePublisher = realtime_tools::RealtimePublisher; rclcpp::Publisher::SharedPtr s_publisher_; diff --git a/templates/ros2_control/controller/dummy_package_namespace/visibility_control.h b/templates/ros2_control/controller/dummy_package_namespace/visibility_control.h new file mode 100644 index 00000000..ab72b560 --- /dev/null +++ b/templates/ros2_control/controller/dummy_package_namespace/visibility_control.h @@ -0,0 +1,49 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// 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 TEMPLATES__ROS2_CONTROL__CONTROLLER__DUMMY_PACKAGE_NAMESPACE__VISIBILITY_CONTROL_H_ +#define TEMPLATES__ROS2_CONTROL__CONTROLLER__DUMMY_PACKAGE_NAMESPACE__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 TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __attribute__((dllexport)) +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT __attribute__((dllimport)) +#else +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __declspec(dllexport) +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT __declspec(dllimport) +#endif +#ifdef TEMPLATES__ROS2_CONTROL__VISIBILITY_BUILDING_DLL +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT +#else +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT +#endif +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC_TYPE TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL +#else +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __attribute__((visibility("default"))) +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT +#if __GNUC__ >= 4 +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC __attribute__((visibility("default"))) +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) +#else +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL +#endif +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC_TYPE +#endif + +#endif // TEMPLATES__ROS2_CONTROL__CONTROLLER__DUMMY_PACKAGE_NAMESPACE__VISIBILITY_CONTROL_H_ diff --git a/templates/ros2_control/controller/test_dummy_chainable_controller.cpp b/templates/ros2_control/controller/test_dummy_chainable_controller.cpp new file mode 100644 index 00000000..4f5a3d52 --- /dev/null +++ b/templates/ros2_control/controller/test_dummy_chainable_controller.cpp @@ -0,0 +1,435 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// 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_dummy_chainable_controller.hpp" + +#include +#include +#include +#include +#include + +using dummy_package_namespace::CMD_MY_ITFS; +using dummy_package_namespace::control_mode_type; +using dummy_package_namespace::STATE_MY_ITFS; + +class DummyClassNameTest : public DummyClassNameFixture +{ +}; + +// When there are many mandatory parameters, set all by default and remove one by one in a +// parameterized test +TEST_P(DummyClassNameTestParameterizedParameters, one_parameter_is_missing) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); +} + +// TODO(anyone): the new gtest version after 1.8.0 uses INSTANTIATE_TEST_SUITE_P +INSTANTIATE_TEST_SUITE_P( + MissingMandatoryParameterDuringConfiguration, DummyClassNameTestParameterizedParameters, + ::testing::Values( + std::make_tuple(std::string("joints"), rclcpp::ParameterValue(std::vector({}))), + std::make_tuple( + std::string("state_joints"), rclcpp::ParameterValue(std::vector({}))), + std::make_tuple(std::string("interface_name"), rclcpp::ParameterValue("")))); + +TEST_F(DummyClassNameTest, joint_names_parameter_not_set) +{ + SetUpController(false); + + ASSERT_TRUE(controller_->joint_names_.empty()); + ASSERT_TRUE(controller_->state_joint_names_.empty()); + ASSERT_TRUE(controller_->interface_name_.empty()); + + controller_->get_node()->set_parameter({"state_joints", state_joint_names_}); + controller_->get_node()->set_parameter({"interface_name", interface_name_}); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); + + ASSERT_TRUE(controller_->joint_names_.empty()); + ASSERT_TRUE(controller_->state_joint_names_.empty()); + ASSERT_TRUE(controller_->interface_name_.empty()); +} + +TEST_F(DummyClassNameTest, state_joint_names_parameter_not_set) +{ + SetUpController(false); + + ASSERT_TRUE(controller_->joint_names_.empty()); + ASSERT_TRUE(controller_->state_joint_names_.empty()); + ASSERT_TRUE(controller_->interface_name_.empty()); + + controller_->get_node()->set_parameter({"joints", joint_names_}); + controller_->get_node()->set_parameter({"interface_name", interface_name_}); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); + + ASSERT_THAT(controller_->joint_names_, testing::ElementsAreArray(joint_names_)); + ASSERT_TRUE(controller_->state_joint_names_.empty()); + ASSERT_TRUE(controller_->interface_name_.empty()); +} + +TEST_F(DummyClassNameTest, interface_parameter_not_set) +{ + SetUpController(false); + + ASSERT_TRUE(controller_->joint_names_.empty()); + ASSERT_TRUE(controller_->state_joint_names_.empty()); + ASSERT_TRUE(controller_->interface_name_.empty()); + + controller_->get_node()->set_parameter({"joints", joint_names_}); + controller_->get_node()->set_parameter({"state_joints", state_joint_names_}); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); + + ASSERT_THAT(controller_->joint_names_, testing::ElementsAreArray(joint_names_)); + ASSERT_THAT(controller_->state_joint_names_, testing::ElementsAreArray(state_joint_names_)); + ASSERT_TRUE(controller_->interface_name_.empty()); +} + +TEST_F(DummyClassNameTest, all_parameters_set_configure_success) +{ + SetUpController(); + + ASSERT_TRUE(controller_->joint_names_.empty()); + ASSERT_TRUE(controller_->state_joint_names_.empty()); + ASSERT_TRUE(controller_->interface_name_.empty()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_THAT(controller_->joint_names_, testing::ElementsAreArray(joint_names_)); + ASSERT_THAT(controller_->state_joint_names_, testing::ElementsAreArray(state_joint_names_)); + ASSERT_EQ(controller_->interface_name_, interface_name_); +} + +TEST_F(DummyClassNameTest, check_exported_intefaces) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto command_intefaces = controller_->command_interface_configuration(); + ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + for (size_t i = 0; i < command_intefaces.names.size(); ++i) { + EXPECT_EQ(command_intefaces.names[i], joint_names_[i] + "/" + interface_name_); + } + + auto state_intefaces = controller_->state_interface_configuration(); + ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); + for (size_t i = 0; i < state_intefaces.names.size(); ++i) { + EXPECT_EQ(state_intefaces.names[i], state_joint_names_[i] + "/" + interface_name_); + } + + // check ref itfs + auto reference_interfaces = controller_->export_reference_interfaces(); + ASSERT_EQ(reference_interfaces.size(), joint_names_.size()); + for (size_t i = 0; i < joint_names_.size(); ++i) { + EXPECT_EQ(reference_interfaces[i].get_name(), controller_->get_node()->get_name()); + EXPECT_EQ( + reference_interfaces[i].get_interface_name(), joint_names_[i] + "/" + interface_name_); + } +} + +TEST_F(DummyClassNameTest, activate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check that the message is reset + auto msg = controller_->input_cmd_.readFromNonRT(); + EXPECT_EQ((*msg)->displacements.size(), joint_names_.size()); + for (const auto & cmd : (*msg)->displacements) { + EXPECT_TRUE(std::isnan(cmd)); + } + EXPECT_EQ((*msg)->velocities.size(), joint_names_.size()); + for (const auto & cmd : (*msg)->velocities) { + EXPECT_TRUE(std::isnan(cmd)); + } + + ASSERT_TRUE(std::isnan((*msg)->duration)); + + EXPECT_EQ(controller_->reference_interfaces_.size(), joint_names_.size()); + for (const auto & interface : controller_->reference_interfaces_) { + EXPECT_TRUE(std::isnan(interface)); + } +} + +TEST_F(DummyClassNameTest, update_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(DummyClassNameTest, deactivate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); +} + +TEST_F(DummyClassNameTest, reactivate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->command_interfaces_[CMD_MY_ITFS].get_value(), 101.101); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[CMD_MY_ITFS].get_value())); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[CMD_MY_ITFS].get_value())); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(DummyClassNameTest, test_setting_slow_mode_service) +{ + SetUpController(); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + // initially set to false + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // should stay false + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); + + // set to true + ASSERT_NO_THROW(call_service(true, executor)); + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::SLOW); + + // set back to false + ASSERT_NO_THROW(call_service(false, executor)); + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); +} + +TEST_F(DummyClassNameTest, test_update_logic_fast) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(false); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_FALSE(controller_->is_in_chained_mode()); + + // set command statically + static constexpr double TEST_DISPLACEMENT = 23.24; + std::shared_ptr msg = std::make_shared(); + msg->joint_names = joint_names_; + msg->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT); + msg->velocities.resize(joint_names_.size(), std::numeric_limits::quiet_NaN()); + msg->duration = std::numeric_limits::quiet_NaN(); + controller_->input_cmd_.writeFromNonRT(msg); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); + EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT); + EXPECT_TRUE(std::isnan((*(controller_->input_cmd_.readFromRT()))->displacements[0])); + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); + EXPECT_EQ(controller_->reference_interfaces_.size(), joint_names_.size()); + for (const auto & interface : controller_->reference_interfaces_) { + EXPECT_TRUE(std::isnan(interface)); + } +} + +TEST_F(DummyClassNameTest, test_update_logic_slow) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(false); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_FALSE(controller_->is_in_chained_mode()); + + // set command statically + static constexpr double TEST_DISPLACEMENT = 23.24; + std::shared_ptr msg = std::make_shared(); + // When slow mode is enabled command ends up being half of the value + msg->joint_names = joint_names_; + msg->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT); + msg->velocities.resize(joint_names_.size(), std::numeric_limits::quiet_NaN()); + msg->duration = std::numeric_limits::quiet_NaN(); + controller_->input_cmd_.writeFromNonRT(msg); + controller_->control_mode_.writeFromNonRT(control_mode_type::SLOW); + + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::SLOW); + ASSERT_EQ((*(controller_->input_cmd_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT / 2); + EXPECT_TRUE(std::isnan((*(controller_->input_cmd_.readFromRT()))->displacements[0])); + EXPECT_EQ(controller_->reference_interfaces_.size(), joint_names_.size()); + for (const auto & interface : controller_->reference_interfaces_) { + EXPECT_TRUE(std::isnan(interface)); + } +} + +TEST_F(DummyClassNameTest, test_update_logic_chainable_fast) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(true); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(controller_->is_in_chained_mode()); + + // set command statically + static constexpr double TEST_DISPLACEMENT = 23.24; + std::shared_ptr msg = std::make_shared(); + msg->joint_names = joint_names_; + msg->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT); + msg->velocities.resize(joint_names_.size(), std::numeric_limits::quiet_NaN()); + msg->duration = std::numeric_limits::quiet_NaN(); + controller_->input_cmd_.writeFromNonRT(msg); + // this is input source in chained mode + controller_->reference_interfaces_[STATE_MY_ITFS] = TEST_DISPLACEMENT * 2; + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); + // reference_interfaces is directly applied + EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT * 2); + // message is not touched in chained mode + EXPECT_EQ((*(controller_->input_cmd_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); + EXPECT_EQ(controller_->reference_interfaces_.size(), joint_names_.size()); + for (const auto & interface : controller_->reference_interfaces_) { + EXPECT_TRUE(std::isnan(interface)); + } +} + +TEST_F(DummyClassNameTest, test_update_logic_chainable_slow) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(true); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(controller_->is_in_chained_mode()); + + // set command statically + static constexpr double TEST_DISPLACEMENT = 23.24; + std::shared_ptr msg = std::make_shared(); + // When slow mode is enabled command ends up being half of the value + msg->joint_names = joint_names_; + msg->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT); + msg->velocities.resize(joint_names_.size(), std::numeric_limits::quiet_NaN()); + msg->duration = std::numeric_limits::quiet_NaN(); + controller_->input_cmd_.writeFromNonRT(msg); + controller_->control_mode_.writeFromNonRT(control_mode_type::SLOW); + // this is input source in chained mode + controller_->reference_interfaces_[STATE_MY_ITFS] = TEST_DISPLACEMENT * 4; + + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::SLOW); + ASSERT_EQ((*(controller_->input_cmd_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // reference_interfaces is directly applied + EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT * 2); + // message is not touched in chained mode + EXPECT_EQ((*(controller_->input_cmd_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); + EXPECT_EQ(controller_->reference_interfaces_.size(), joint_names_.size()); + for (const auto & interface : controller_->reference_interfaces_) { + EXPECT_TRUE(std::isnan(interface)); + } +} + +TEST_F(DummyClassNameTest, publish_status_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ControllerStateMsg msg; + subscribe_and_get_messages(msg); + + ASSERT_EQ(msg.set_point, 101.101); +} + +TEST_F(DummyClassNameTest, receive_message_and_publish_updated_status) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ControllerStateMsg msg; + subscribe_and_get_messages(msg); + + ASSERT_EQ(msg.set_point, 101.101); + + publish_commands(); + ASSERT_TRUE(controller_->wait_for_commands(executor)); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_EQ(joint_command_values_[CMD_MY_ITFS], 0.45); + + subscribe_and_get_messages(msg); + + ASSERT_EQ(msg.set_point, 0.45); +} diff --git a/templates/ros2_control/controller/test_dummy_chainable_controller.hpp b/templates/ros2_control/controller/test_dummy_chainable_controller.hpp new file mode 100644 index 00000000..225d9f54 --- /dev/null +++ b/templates/ros2_control/controller/test_dummy_chainable_controller.hpp @@ -0,0 +1,294 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// 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 TEMPLATES__ROS2_CONTROL__CONTROLLER__TEST_DUMMY_CHAINABLE_CONTROLLER_HPP_ +#define TEMPLATES__ROS2_CONTROL__CONTROLLER__TEST_DUMMY_CHAINABLE_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "dummy_package_namespace/dummy_chainable_controller.hpp" +#include "gmock/gmock.h" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/parameter_value.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" + +// TODO(anyone): replace the state and command message types +using ControllerStateMsg = dummy_package_namespace::DummyClassName::ControllerStateMsg; +using ControllerCommandMsg = dummy_package_namespace::DummyClassName::ControllerCommandMsg; +using ControllerModeSrvType = dummy_package_namespace::DummyClassName::ControllerModeSrvType; + +namespace +{ +constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; +constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; +} // namespace +// namespace + +// subclassing and friending so we can access member variables +class TestableDummyClassName : public dummy_package_namespace::DummyClassName +{ + FRIEND_TEST(DummyClassNameTest, joint_names_parameter_not_set); + FRIEND_TEST(DummyClassNameTest, state_joint_names_parameter_not_set); + FRIEND_TEST(DummyClassNameTest, interface_parameter_not_set); + FRIEND_TEST(DummyClassNameTest, all_parameters_set_configure_success); + FRIEND_TEST(DummyClassNameTest, activate_success); + FRIEND_TEST(DummyClassNameTest, reactivate_success); + FRIEND_TEST(DummyClassNameTest, test_setting_slow_mode_service); + FRIEND_TEST(DummyClassNameTest, test_update_logic_fast); + FRIEND_TEST(DummyClassNameTest, test_update_logic_slow); + FRIEND_TEST(DummyClassNameTest, test_update_logic_chainable_fast); + FRIEND_TEST(DummyClassNameTest, test_update_logic_chainable_slow); + +public: + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override + { + auto ret = dummy_package_namespace::DummyClassName::on_configure(previous_state); + // Only if on_configure is successful create subscription + if (ret == CallbackReturn::SUCCESS) { + cmd_subscriber_wait_set_.add_subscription(cmd_subscriber_); + } + return ret; + } + + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override + { + auto ref_itfs = on_export_reference_interfaces(); + return dummy_package_namespace::DummyClassName::on_activate(previous_state); + } + + /** + * @brief wait_for_command blocks until a new ControllerCommandMsg is received. + * Requires that the executor is not spinned elsewhere between the + * message publication and the call to this function. + * + * @return true if new ControllerCommandMsg msg was received, false if timeout. + */ + bool wait_for_command( + rclcpp::Executor & executor, rclcpp::WaitSet & subscriber_wait_set, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + bool success = subscriber_wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; + if (success) { + executor.spin_some(); + } + return success; + } + + bool wait_for_commands( + rclcpp::Executor & executor, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + return wait_for_command(executor, cmd_subscriber_wait_set_, timeout); + } + + // TODO(anyone): add implementation of any methods of your controller is needed + +private: + rclcpp::WaitSet cmd_subscriber_wait_set_; +}; + +// We are using template class here for easier reuse of Fixture in specializations of controllers +template +class DummyClassNameFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() { rclcpp::init(0, nullptr); } + + void SetUp() + { + // initialize controller + controller_ = std::make_unique(); + + command_publisher_node_ = std::make_shared("command_publisher"); + command_publisher_ = command_publisher_node_->create_publisher( + "/test_dummy_package_namespace/commands", rclcpp::SystemDefaultsQoS()); + + service_caller_node_ = std::make_shared("service_caller"); + slow_control_service_client_ = service_caller_node_->create_client( + "/test_dummy_package_namespace/set_slow_control_mode"); + } + + static void TearDownTestCase() { rclcpp::shutdown(); } + + void TearDown() { controller_.reset(nullptr); } + +protected: + void SetUpController( + bool set_parameters = true, std::string controller_name = "test_dummy_package_namespace") + { + ASSERT_EQ(controller_->init(controller_name), controller_interface::return_type::OK); + + std::vector command_ifs; + command_itfs_.reserve(joint_command_values_.size()); + command_ifs.reserve(joint_command_values_.size()); + + for (size_t i = 0; i < joint_command_values_.size(); ++i) { + command_itfs_.emplace_back(hardware_interface::CommandInterface( + joint_names_[i], interface_name_, &joint_command_values_[i])); + command_ifs.emplace_back(command_itfs_.back()); + } + // TODO(anyone): Add other command interfaces, if any + + std::vector state_ifs; + state_itfs_.reserve(joint_state_values_.size()); + state_ifs.reserve(joint_state_values_.size()); + + for (size_t i = 0; i < joint_state_values_.size(); ++i) { + state_itfs_.emplace_back(hardware_interface::StateInterface( + joint_names_[i], interface_name_, &joint_state_values_[i])); + state_ifs.emplace_back(state_itfs_.back()); + } + // TODO(anyone): Add other state interfaces, if any + + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + + if (set_parameters) { + controller_->get_node()->set_parameter({"joints", joint_names_}); + controller_->get_node()->set_parameter({"state_joints", state_joint_names_}); + controller_->get_node()->set_parameter({"interface_name", interface_name_}); + } + } + + void subscribe_and_get_messages(ControllerStateMsg & msg) + { + // create a new subscriber + rclcpp::Node test_subscription_node("test_subscription_node"); + auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {}; + auto subscription = test_subscription_node.create_subscription( + "/test_dummy_package_namespace/state", 10, subs_callback); + + // call update to publish the test value + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // call update to publish the test value + // since update doesn't guarantee a published message, republish until received + int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop + rclcpp::WaitSet wait_set; // block used to wait on message + wait_set.add_subscription(subscription); + while (max_sub_check_loop_count--) { + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + // check if message has been received + if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) { + break; + } + } + ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " + "controller/broadcaster update loop"; + + // take message from subscription + rclcpp::MessageInfo msg_info; + ASSERT_TRUE(subscription->take(msg, msg_info)); + } + + // TODO(anyone): add/remove arguments as it suites your command message type + void publish_commands( + const std::vector & displacements = {0.45}, + const std::vector & velocities = {0.0}, const double duration = 1.25) + { + auto wait_for_topic = [&](const auto topic_name) { + size_t wait_count = 0; + while (command_publisher_node_->count_subscribers(topic_name) == 0) { + if (wait_count >= 5) { + auto error_msg = + std::string("publishing to ") + topic_name + " but no node subscribes to it"; + throw std::runtime_error(error_msg); + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + ++wait_count; + } + }; + + wait_for_topic(command_publisher_->get_topic_name()); + + ControllerCommandMsg msg; + msg.joint_names = joint_names_; + msg.displacements = displacements; + msg.velocities = velocities; + msg.duration = duration; + + command_publisher_->publish(msg); + } + + std::shared_ptr call_service( + const bool slow_control, rclcpp::Executor & executor) + { + auto request = std::make_shared(); + request->data = slow_control; + + bool wait_for_service_ret = + slow_control_service_client_->wait_for_service(std::chrono::milliseconds(500)); + EXPECT_TRUE(wait_for_service_ret); + if (!wait_for_service_ret) { + throw std::runtime_error("Services is not available!"); + } + auto result = slow_control_service_client_->async_send_request(request); + EXPECT_EQ(executor.spin_until_future_complete(result), rclcpp::FutureReturnCode::SUCCESS); + + return result.get(); + } + +protected: + // TODO(anyone): adjust the members as needed + + // Controller-related parameters + std::vector joint_names_ = {"joint1"}; + std::vector state_joint_names_ = {"joint1state"}; + std::string interface_name_ = "my_interface"; + std::array joint_state_values_ = {1.1}; + std::array joint_command_values_ = {101.101}; + + std::vector state_itfs_; + std::vector command_itfs_; + + // Test related parameters + std::unique_ptr controller_; + rclcpp::Node::SharedPtr command_publisher_node_; + rclcpp::Publisher::SharedPtr command_publisher_; + rclcpp::Node::SharedPtr service_caller_node_; + rclcpp::Client::SharedPtr slow_control_service_client_; +}; + +// From the tutorial: https://www.sandordargo.com/blog/2019/04/24/parameterized-testing-with-gtest +class DummyClassNameTestParameterizedParameters +: public DummyClassNameFixture, + public ::testing::WithParamInterface> +{ +public: + virtual void SetUp() { DummyClassNameFixture::SetUp(); } + + static void TearDownTestCase() { DummyClassNameFixture::TearDownTestCase(); } + +protected: + void SetUpController(bool set_parameters = true) + { + DummyClassNameFixture::SetUpController(set_parameters); + controller_->get_node()->set_parameter({std::get<0>(GetParam()), std::get<1>(GetParam())}); + } +}; + +#endif // TEMPLATES__ROS2_CONTROL__CONTROLLER__TEST_DUMMY_CHAINABLE_CONTROLLER_HPP_ diff --git a/templates/ros2_control/controller/test_dummy_controller.cpp b/templates/ros2_control/controller/test_dummy_controller.cpp index 299154e9..7c42aa2b 100644 --- a/templates/ros2_control/controller/test_dummy_controller.cpp +++ b/templates/ros2_control/controller/test_dummy_controller.cpp @@ -14,11 +14,20 @@ #include "test_dummy_controller.hpp" +#include #include #include #include #include +using dummy_package_namespace::CMD_MY_ITFS; +using dummy_package_namespace::control_mode_type; +using dummy_package_namespace::STATE_MY_ITFS; + +class DummyClassNameTest : public DummyClassNameFixture +{ +}; + // When there are many mandatory parameters, set all by default and remove one by one in a // parameterized test TEST_P(DummyClassNameTestParameterizedParameters, one_parameter_is_missing) @@ -33,6 +42,8 @@ INSTANTIATE_TEST_SUITE_P( MissingMandatoryParameterDuringConfiguration, DummyClassNameTestParameterizedParameters, ::testing::Values( std::make_tuple(std::string("joints"), rclcpp::ParameterValue(std::vector({}))), + std::make_tuple( + std::string("state_joints"), rclcpp::ParameterValue(std::vector({}))), std::make_tuple(std::string("interface_name"), rclcpp::ParameterValue("")))); TEST_F(DummyClassNameTest, joint_names_parameter_not_set) @@ -40,13 +51,34 @@ TEST_F(DummyClassNameTest, joint_names_parameter_not_set) SetUpController(false); ASSERT_TRUE(controller_->joint_names_.empty()); + ASSERT_TRUE(controller_->state_joint_names_.empty()); ASSERT_TRUE(controller_->interface_name_.empty()); + controller_->get_node()->set_parameter({"state_joints", state_joint_names_}); controller_->get_node()->set_parameter({"interface_name", interface_name_}); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); ASSERT_TRUE(controller_->joint_names_.empty()); + ASSERT_TRUE(controller_->state_joint_names_.empty()); + ASSERT_TRUE(controller_->interface_name_.empty()); +} + +TEST_F(DummyClassNameTest, state_joint_names_parameter_not_set) +{ + SetUpController(false); + + ASSERT_TRUE(controller_->joint_names_.empty()); + ASSERT_TRUE(controller_->state_joint_names_.empty()); + ASSERT_TRUE(controller_->interface_name_.empty()); + + controller_->get_node()->set_parameter({"joints", joint_names_}); + controller_->get_node()->set_parameter({"interface_name", interface_name_}); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); + + ASSERT_THAT(controller_->joint_names_, testing::ElementsAreArray(joint_names_)); + ASSERT_TRUE(controller_->state_joint_names_.empty()); ASSERT_TRUE(controller_->interface_name_.empty()); } @@ -54,11 +86,17 @@ TEST_F(DummyClassNameTest, interface_parameter_not_set) { SetUpController(false); + ASSERT_TRUE(controller_->joint_names_.empty()); + ASSERT_TRUE(controller_->state_joint_names_.empty()); + ASSERT_TRUE(controller_->interface_name_.empty()); + controller_->get_node()->set_parameter({"joints", joint_names_}); + controller_->get_node()->set_parameter({"state_joints", state_joint_names_}); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); - ASSERT_TRUE(!controller_->joint_names_.empty()); + ASSERT_THAT(controller_->joint_names_, testing::ElementsAreArray(joint_names_)); + ASSERT_THAT(controller_->state_joint_names_, testing::ElementsAreArray(state_joint_names_)); ASSERT_TRUE(controller_->interface_name_.empty()); } @@ -67,21 +105,17 @@ TEST_F(DummyClassNameTest, all_parameters_set_configure_success) SetUpController(); ASSERT_TRUE(controller_->joint_names_.empty()); + ASSERT_TRUE(controller_->state_joint_names_.empty()); ASSERT_TRUE(controller_->interface_name_.empty()); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_TRUE(!controller_->joint_names_.empty()); - ASSERT_TRUE(controller_->joint_names_.size() == joint_names_.size()); - ASSERT_TRUE(std::equal( - controller_->joint_names_.begin(), controller_->joint_names_.end(), joint_names_.begin(), - joint_names_.end())); - - ASSERT_TRUE(!controller_->interface_name_.empty()); - ASSERT_TRUE(controller_->interface_name_ == interface_name_); + ASSERT_THAT(controller_->joint_names_, testing::ElementsAreArray(joint_names_)); + ASSERT_THAT(controller_->state_joint_names_, testing::ElementsAreArray(state_joint_names_)); + ASSERT_EQ(controller_->interface_name_, interface_name_); } -TEST_F(DummyClassNameTest, check_intefaces) +TEST_F(DummyClassNameTest, check_exported_intefaces) { SetUpController(); @@ -89,9 +123,15 @@ TEST_F(DummyClassNameTest, check_intefaces) auto command_intefaces = controller_->command_interface_configuration(); ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + for (size_t i = 0; i < command_intefaces.names.size(); ++i) { + EXPECT_EQ(command_intefaces.names[i], joint_names_[i] + "/" + interface_name_); + } auto state_intefaces = controller_->state_interface_configuration(); ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); + for (size_t i = 0; i < state_intefaces.names.size(); ++i) { + EXPECT_EQ(state_intefaces.names[i], state_joint_names_[i] + "/" + interface_name_); + } } TEST_F(DummyClassNameTest, activate_success) @@ -100,6 +140,19 @@ TEST_F(DummyClassNameTest, activate_success) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check that the message is reset + auto msg = controller_->input_cmd_.readFromNonRT(); + EXPECT_EQ((*msg)->displacements.size(), joint_names_.size()); + for (const auto & cmd : (*msg)->displacements) { + EXPECT_TRUE(std::isnan(cmd)); + } + EXPECT_EQ((*msg)->velocities.size(), joint_names_.size()); + for (const auto & cmd : (*msg)->velocities) { + EXPECT_TRUE(std::isnan(cmd)); + } + + ASSERT_TRUE(std::isnan((*msg)->duration)); } TEST_F(DummyClassNameTest, update_success) @@ -109,9 +162,9 @@ TEST_F(DummyClassNameTest, update_success) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - rclcpp::Time node_time = controller_->get_node()->now(); - rclcpp::Duration duration = rclcpp::Duration::from_nanoseconds(1000000); // 1ms - ASSERT_EQ(controller_->update(node_time, duration), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); } TEST_F(DummyClassNameTest, deactivate_success) @@ -129,12 +182,101 @@ TEST_F(DummyClassNameTest, reactivate_success) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->command_interfaces_[CMD_MY_ITFS].get_value(), 101.101); ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[CMD_MY_ITFS].get_value())); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[CMD_MY_ITFS].get_value())); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(DummyClassNameTest, test_setting_slow_mode_service) +{ + SetUpController(); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + // initially set to false + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // should stay false + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); + + // set to true + ASSERT_NO_THROW(call_service(true, executor)); + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::SLOW); + + // set back to false + ASSERT_NO_THROW(call_service(false, executor)); + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); +} + +TEST_F(DummyClassNameTest, test_update_logic_fast) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // set command statically + static constexpr double TEST_DISPLACEMENT = 23.24; + std::shared_ptr msg = std::make_shared(); + msg->joint_names = joint_names_; + msg->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT); + msg->velocities.resize(joint_names_.size(), std::numeric_limits::quiet_NaN()); + msg->duration = std::numeric_limits::quiet_NaN(); + controller_->input_cmd_.writeFromNonRT(msg); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); + EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT); + EXPECT_TRUE(std::isnan((*(controller_->input_cmd_.readFromRT()))->displacements[0])); + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); +} + +TEST_F(DummyClassNameTest, test_update_logic_slow) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - rclcpp::Time node_time = controller_->get_node()->now(); - rclcpp::Duration duration = rclcpp::Duration::from_nanoseconds(1000000); // 1ms - ASSERT_EQ(controller_->update(node_time, duration), controller_interface::return_type::OK); + // set command statically + static constexpr double TEST_DISPLACEMENT = 23.24; + std::shared_ptr msg = std::make_shared(); + // When slow mode is enabled command ends up being half of the value + msg->joint_names = joint_names_; + msg->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT); + msg->velocities.resize(joint_names_.size(), std::numeric_limits::quiet_NaN()); + msg->duration = std::numeric_limits::quiet_NaN(); + controller_->input_cmd_.writeFromNonRT(msg); + controller_->control_mode_.writeFromNonRT(control_mode_type::SLOW); + + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::SLOW); + ASSERT_EQ((*(controller_->input_cmd_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT / 2); + EXPECT_TRUE(std::isnan((*(controller_->input_cmd_.readFromRT()))->displacements[0])); } TEST_F(DummyClassNameTest, publish_status_success) @@ -144,9 +286,9 @@ TEST_F(DummyClassNameTest, publish_status_success) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - rclcpp::Time node_time = controller_->get_node()->now(); - rclcpp::Duration duration = rclcpp::Duration::from_nanoseconds(1000000); // 1ms - ASSERT_EQ(controller_->update(node_time, duration), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); ControllerStateMsg msg; subscribe_and_get_messages(msg); @@ -163,9 +305,9 @@ TEST_F(DummyClassNameTest, receive_message_and_publish_updated_status) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - rclcpp::Time node_time = controller_->get_node()->now(); - rclcpp::Duration duration = rclcpp::Duration::from_nanoseconds(1000000); // 1ms - ASSERT_EQ(controller_->update(node_time, duration), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); ControllerStateMsg msg; subscribe_and_get_messages(msg); @@ -175,10 +317,11 @@ TEST_F(DummyClassNameTest, receive_message_and_publish_updated_status) publish_commands(); ASSERT_TRUE(controller_->wait_for_commands(executor)); - node_time = controller_->get_node()->now(); - ASSERT_EQ(controller_->update(node_time, duration), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); - EXPECT_EQ(joint_command_values_[0], 0.45); + EXPECT_EQ(joint_command_values_[CMD_MY_ITFS], 0.45); subscribe_and_get_messages(msg); diff --git a/templates/ros2_control/controller/test_dummy_controller.hpp b/templates/ros2_control/controller/test_dummy_controller.hpp index eae67f54..c06e7d2f 100644 --- a/templates/ros2_control/controller/test_dummy_controller.hpp +++ b/templates/ros2_control/controller/test_dummy_controller.hpp @@ -16,6 +16,7 @@ #define TEMPLATES__ROS2_CONTROL__CONTROLLER__TEST_DUMMY_CONTROLLER_HPP_ #include +#include #include #include #include @@ -32,45 +33,38 @@ #include "rclcpp/utilities.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" -// TODO(anyone): Replace with controller specific messages -#include "control_msgs/msg/joint_controller_state.hpp" -#include "control_msgs/msg/joint_jog.hpp" - // TODO(anyone): replace the state and command message types -using ControllerStateMsg = control_msgs::msg::JointControllerState; -using ControllerCommandMsg = control_msgs::msg::JointJog; +using ControllerStateMsg = dummy_package_namespace::DummyClassName::ControllerStateMsg; +using ControllerCommandMsg = dummy_package_namespace::DummyClassName::ControllerCommandMsg; +using ControllerModeSrvType = dummy_package_namespace::DummyClassName::ControllerModeSrvType; namespace { -constexpr auto NODE_SUCCESS = - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; -constexpr auto NODE_ERROR = - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; - -rclcpp::WaitResultKind wait_for(rclcpp::SubscriptionBase::SharedPtr subscription) -{ - rclcpp::WaitSet wait_set; - wait_set.add_subscription(subscription); - const auto timeout = std::chrono::seconds(10); - return wait_set.wait(timeout).kind(); -} - +constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; +constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; } // namespace // subclassing and friending so we can access member variables class TestableDummyClassName : public dummy_package_namespace::DummyClassName { FRIEND_TEST(DummyClassNameTest, joint_names_parameter_not_set); + FRIEND_TEST(DummyClassNameTest, state_joint_names_parameter_not_set); FRIEND_TEST(DummyClassNameTest, interface_parameter_not_set); FRIEND_TEST(DummyClassNameTest, all_parameters_set_configure_success); + FRIEND_TEST(DummyClassNameTest, activate_success); + FRIEND_TEST(DummyClassNameTest, reactivate_success); + FRIEND_TEST(DummyClassNameTest, test_setting_slow_mode_service); + FRIEND_TEST(DummyClassNameTest, test_update_logic_fast); + FRIEND_TEST(DummyClassNameTest, test_update_logic_slow); public: - CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override { auto ret = dummy_package_namespace::DummyClassName::on_configure(previous_state); // Only if on_configure is successful create subscription if (ret == CallbackReturn::SUCCESS) { - command_subscriber_wait_set_.add_subscription(command_subscriber_); + cmd_subscriber_wait_set_.add_subscription(cmd_subscriber_); } return ret; } @@ -82,23 +76,33 @@ class TestableDummyClassName : public dummy_package_namespace::DummyClassName * * @return true if new ControllerCommandMsg msg was received, false if timeout. */ - bool wait_for_commands( - rclcpp::Executor & executor, + bool wait_for_command( + rclcpp::Executor & executor, rclcpp::WaitSet & subscriber_wait_set, const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) { - bool success = - command_subscriber_wait_set_.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; + bool success = subscriber_wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; if (success) { executor.spin_some(); } return success; } + bool wait_for_commands( + rclcpp::Executor & executor, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + return wait_for_command(executor, cmd_subscriber_wait_set_, timeout); + } + + // TODO(anyone): add implementation of any methods of your controller is needed + private: - rclcpp::WaitSet command_subscriber_wait_set_; + rclcpp::WaitSet cmd_subscriber_wait_set_; }; -class DummyClassNameTest : public ::testing::Test +// We are using template class here for easier reuse of Fixture in specializations of controllers +template +class DummyClassNameFixture : public ::testing::Test { public: static void SetUpTestCase() { rclcpp::init(0, nullptr); } @@ -106,11 +110,15 @@ class DummyClassNameTest : public ::testing::Test void SetUp() { // initialize controller - controller_ = std::make_unique(); + controller_ = std::make_unique(); command_publisher_node_ = std::make_shared("command_publisher"); command_publisher_ = command_publisher_node_->create_publisher( "/test_dummy_package_namespace/commands", rclcpp::SystemDefaultsQoS()); + + service_caller_node_ = std::make_shared("service_caller"); + slow_control_service_client_ = service_caller_node_->create_client( + "/test_dummy_package_namespace/set_slow_control_mode"); } static void TearDownTestCase() { rclcpp::shutdown(); } @@ -118,16 +126,16 @@ class DummyClassNameTest : public ::testing::Test void TearDown() { controller_.reset(nullptr); } protected: - void SetUpController(bool set_parameters = true) + void SetUpController( + bool set_parameters = true, std::string controller_name = "test_dummy_package_namespace") { - const auto result = controller_->init("test_dummy_package_namespace"); - ASSERT_EQ(result, controller_interface::return_type::OK); + ASSERT_EQ(controller_->init(controller_name), controller_interface::return_type::OK); std::vector command_ifs; command_itfs_.reserve(joint_command_values_.size()); command_ifs.reserve(joint_command_values_.size()); - for (auto i = 0u; i < joint_command_values_.size(); ++i) { + for (size_t i = 0; i < joint_command_values_.size(); ++i) { command_itfs_.emplace_back(hardware_interface::CommandInterface( joint_names_[i], interface_name_, &joint_command_values_[i])); command_ifs.emplace_back(command_itfs_.back()); @@ -138,7 +146,7 @@ class DummyClassNameTest : public ::testing::Test state_itfs_.reserve(joint_state_values_.size()); state_ifs.reserve(joint_state_values_.size()); - for (auto i = 0u; i < joint_state_values_.size(); ++i) { + for (size_t i = 0; i < joint_state_values_.size(); ++i) { state_itfs_.emplace_back(hardware_interface::StateInterface( joint_names_[i], interface_name_, &joint_state_values_[i])); state_ifs.emplace_back(state_itfs_.back()); @@ -149,6 +157,7 @@ class DummyClassNameTest : public ::testing::Test if (set_parameters) { controller_->get_node()->set_parameter({"joints", joint_names_}); + controller_->get_node()->set_parameter({"state_joints", state_joint_names_}); controller_->get_node()->set_parameter({"interface_name", interface_name_}); } } @@ -162,19 +171,34 @@ class DummyClassNameTest : public ::testing::Test "/test_dummy_package_namespace/state", 10, subs_callback); // call update to publish the test value - rclcpp::Time node_time = controller_->get_node()->now(); - rclcpp::Duration duration = rclcpp::Duration::from_nanoseconds(1000000); // 1ms - ASSERT_EQ(controller_->update(node_time, duration), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); - // wait for message to be passed - ASSERT_EQ(wait_for(subscription), rclcpp::WaitResultKind::Ready); + // call update to publish the test value + // since update doesn't guarantee a published message, republish until received + int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop + rclcpp::WaitSet wait_set; // block used to wait on message + wait_set.add_subscription(subscription); + while (max_sub_check_loop_count--) { + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + // check if message has been received + if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) { + break; + } + } + ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " + "controller/broadcaster update loop"; // take message from subscription rclcpp::MessageInfo msg_info; ASSERT_TRUE(subscription->take(msg, msg_info)); } - void publish_commands() + // TODO(anyone): add/remove arguments as it suites your command message type + void publish_commands( + const std::vector & displacements = {0.45}, + const std::vector & velocities = {0.0}, const double duration = 1.25) { auto wait_for_topic = [&](const auto topic_name) { size_t wait_count = 0; @@ -193,18 +217,37 @@ class DummyClassNameTest : public ::testing::Test ControllerCommandMsg msg; msg.joint_names = joint_names_; - msg.displacements = {0.45}; - msg.velocities = {0.0}; - msg.duration = 1.25; + msg.displacements = displacements; + msg.velocities = velocities; + msg.duration = duration; command_publisher_->publish(msg); } + std::shared_ptr call_service( + const bool slow_control, rclcpp::Executor & executor) + { + auto request = std::make_shared(); + request->data = slow_control; + + bool wait_for_service_ret = + slow_control_service_client_->wait_for_service(std::chrono::milliseconds(500)); + EXPECT_TRUE(wait_for_service_ret); + if (!wait_for_service_ret) { + throw std::runtime_error("Services is not available!"); + } + auto result = slow_control_service_client_->async_send_request(request); + EXPECT_EQ(executor.spin_until_future_complete(result), rclcpp::FutureReturnCode::SUCCESS); + + return result.get(); + } + protected: // TODO(anyone): adjust the members as needed // Controller-related parameters std::vector joint_names_ = {"joint1"}; + std::vector state_joint_names_ = {"joint1state"}; std::string interface_name_ = "my_interface"; std::array joint_state_values_ = {1.1}; std::array joint_command_values_ = {101.101}; @@ -216,22 +259,24 @@ class DummyClassNameTest : public ::testing::Test std::unique_ptr controller_; rclcpp::Node::SharedPtr command_publisher_node_; rclcpp::Publisher::SharedPtr command_publisher_; + rclcpp::Node::SharedPtr service_caller_node_; + rclcpp::Client::SharedPtr slow_control_service_client_; }; // From the tutorial: https://www.sandordargo.com/blog/2019/04/24/parameterized-testing-with-gtest class DummyClassNameTestParameterizedParameters -: public DummyClassNameTest, +: public DummyClassNameFixture, public ::testing::WithParamInterface> { public: - virtual void SetUp() { DummyClassNameTest::SetUp(); } + virtual void SetUp() { DummyClassNameFixture::SetUp(); } - static void TearDownTestCase() { DummyClassNameTest::TearDownTestCase(); } + static void TearDownTestCase() { DummyClassNameFixture::TearDownTestCase(); } protected: void SetUpController(bool set_parameters = true) { - DummyClassNameTest::SetUpController(set_parameters); + DummyClassNameFixture::SetUpController(set_parameters); controller_->get_node()->set_parameter({std::get<0>(GetParam()), std::get<1>(GetParam())}); } }; diff --git a/templates/ros2_control/hardware/dummy_package_namespace/robot_hardware_interface.hpp b/templates/ros2_control/hardware/dummy_package_namespace/robot_hardware_interface.hpp index 3daa1911..be0c275c 100644 --- a/templates/ros2_control/hardware/dummy_package_namespace/robot_hardware_interface.hpp +++ b/templates/ros2_control/hardware/dummy_package_namespace/robot_hardware_interface.hpp @@ -28,31 +28,34 @@ namespace dummy_package_namespace { -using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - class DummyClassName : public hardware_interface::Dummy_Interface_TypeInterface { public: - TEMPLATES__ROS2_CONTROL__HARDWARE_PUBLIC - CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::CallbackReturn on_init( + const hardware_interface::HardwareInfo & info) override; - TEMPLATES__ROS2_CONTROL__HARDWARE_PUBLIC + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC std::vector export_state_interfaces() override; - TEMPLATES__ROS2_CONTROL__HARDWARE_PUBLIC + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC std::vector export_command_interfaces() override; - TEMPLATES__ROS2_CONTROL__HARDWARE_PUBLIC - CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; - TEMPLATES__ROS2_CONTROL__HARDWARE_PUBLIC - CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; - TEMPLATES__ROS2_CONTROL__HARDWARE_PUBLIC - hardware_interface::return_type read() override; + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::return_type read( + const rclcpp::Time & time, const rclcpp::Duration & period) override; - TEMPLATES__ROS2_CONTROL__HARDWARE_PUBLIC - hardware_interface::return_type write() override; + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::return_type write( + const rclcpp::Time & time, const rclcpp::Duration & period) override; private: std::vector hw_commands_; diff --git a/templates/ros2_control/hardware/dummy_package_namespace/visibility_control.h b/templates/ros2_control/hardware/dummy_package_namespace/visibility_control.h new file mode 100644 index 00000000..79c0a03c --- /dev/null +++ b/templates/ros2_control/hardware/dummy_package_namespace/visibility_control.h @@ -0,0 +1,49 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// 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 TEMPLATES__ROS2_CONTROL__HARDWARE__DUMMY_PACKAGE_NAMESPACE__VISIBILITY_CONTROL_H_ +#define TEMPLATES__ROS2_CONTROL__HARDWARE__DUMMY_PACKAGE_NAMESPACE__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 TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __attribute__((dllexport)) +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT __attribute__((dllimport)) +#else +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __declspec(dllexport) +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT __declspec(dllimport) +#endif +#ifdef TEMPLATES__ROS2_CONTROL__VISIBILITY_BUILDING_DLL +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT +#else +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT +#endif +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC_TYPE TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL +#else +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __attribute__((visibility("default"))) +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT +#if __GNUC__ >= 4 +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC __attribute__((visibility("default"))) +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) +#else +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL +#endif +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC_TYPE +#endif + +#endif // TEMPLATES__ROS2_CONTROL__HARDWARE__DUMMY_PACKAGE_NAMESPACE__VISIBILITY_CONTROL_H_ diff --git a/templates/ros2_control/hardware/robot_hardware_interface.cpp b/templates/ros2_control/hardware/robot_hardware_interface.cpp index b0b5ae2f..1750e959 100644 --- a/templates/ros2_control/hardware/robot_hardware_interface.cpp +++ b/templates/ros2_control/hardware/robot_hardware_interface.cpp @@ -21,7 +21,8 @@ namespace dummy_package_namespace { -CallbackReturn DummyClassName::on_init(const hardware_interface::HardwareInfo & info) +hardware_interface::CallbackReturn DummyClassName::on_init( + const hardware_interface::HardwareInfo & info) { if (hardware_interface::Dummy_Interface_TypeInterface::on_init(info) != CallbackReturn::SUCCESS) { return CallbackReturn::ERROR; @@ -58,28 +59,32 @@ std::vector DummyClassName::export_command return command_interfaces; } -CallbackReturn DummyClassName::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) +hardware_interface::CallbackReturn DummyClassName::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) { // TODO(anyone): prepare the robot to receive commands return CallbackReturn::SUCCESS; } -CallbackReturn DummyClassName::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) +hardware_interface::CallbackReturn DummyClassName::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) { // TODO(anyone): prepare the robot to stop receiving commands return CallbackReturn::SUCCESS; } -hardware_interface::return_type DummyClassName::read() +hardware_interface::return_type DummyClassName::read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { // TODO(anyone): read robot states return hardware_interface::return_type::OK; } -hardware_interface::return_type DummyClassName::write() +hardware_interface::return_type DummyClassName::write( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { // TODO(anyone): write robot's commands' diff --git a/templates/ros2_control/hardware/visibility_control.h b/templates/ros2_control/hardware/visibility_control.h deleted file mode 100644 index 1ad4efaf..00000000 --- a/templates/ros2_control/hardware/visibility_control.h +++ /dev/null @@ -1,49 +0,0 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) -// -// 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 TEMPLATES__ROS2_CONTROL__HARDWARE__VISIBILITY_CONTROL_H_ -#define TEMPLATES__ROS2_CONTROL__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 TEMPLATES__ROS2_CONTROL__HARDWARE_EXPORT __attribute__((dllexport)) -#define TEMPLATES__ROS2_CONTROL__HARDWARE_IMPORT __attribute__((dllimport)) -#else -#define TEMPLATES__ROS2_CONTROL__HARDWARE_EXPORT __declspec(dllexport) -#define TEMPLATES__ROS2_CONTROL__HARDWARE_IMPORT __declspec(dllimport) -#endif -#ifdef TEMPLATES__ROS2_CONTROL__HARDWARE_BUILDING_DLL -#define TEMPLATES__ROS2_CONTROL__HARDWARE_PUBLIC TEMPLATES__ROS2_CONTROL__HARDWARE_EXPORT -#else -#define TEMPLATES__ROS2_CONTROL__HARDWARE_PUBLIC TEMPLATES__ROS2_CONTROL__HARDWARE_IMPORT -#endif -#define TEMPLATES__ROS2_CONTROL__HARDWARE_PUBLIC_TYPE TEMPLATES__ROS2_CONTROL__HARDWARE_PUBLIC -#define TEMPLATES__ROS2_CONTROL__HARDWARE_LOCAL -#else -#define TEMPLATES__ROS2_CONTROL__HARDWARE_EXPORT __attribute__((visibility("default"))) -#define TEMPLATES__ROS2_CONTROL__HARDWARE_IMPORT -#if __GNUC__ >= 4 -#define TEMPLATES__ROS2_CONTROL__HARDWARE_PUBLIC __attribute__((visibility("default"))) -#define TEMPLATES__ROS2_CONTROL__HARDWARE_LOCAL __attribute__((visibility("hidden"))) -#else -#define TEMPLATES__ROS2_CONTROL__HARDWARE_PUBLIC -#define TEMPLATES__ROS2_CONTROL__HARDWARE_LOCAL -#endif -#define TEMPLATES__ROS2_CONTROL__HARDWARE_PUBLIC_TYPE -#endif - -#endif // TEMPLATES__ROS2_CONTROL__HARDWARE__VISIBILITY_CONTROL_H_