diff --git a/.clang-format b/.clang-format index fd961668..6aeb1bc9 100644 --- a/.clang-format +++ b/.clang-format @@ -1,66 +1,64 @@ --- -BasedOnStyle: Google AccessModifierOffset: -2 -ConstructorInitializerIndentWidth: 2 AlignEscapedNewlinesLeft: false AlignTrailingComments: true AllowAllParametersOfDeclarationOnNextLine: false +AllowShortFunctionsOnASingleLine: None AllowShortIfStatementsOnASingleLine: false AllowShortLoopsOnASingleLine: false -AllowShortFunctionsOnASingleLine: None -AlwaysBreakTemplateDeclarations: true AlwaysBreakBeforeMultilineStrings: false +AlwaysBreakTemplateDeclarations: true +BasedOnStyle: Google +BinPackParameters: true + +BraceWrapping: # Control of individual brace wrapping cases + AfterCaseLabel: 'true' + AfterClass: 'true' + AfterControlStatement: 'true' + AfterEnum: 'true' + AfterFunction: 'true' + AfterNamespace: 'true' + AfterStruct: 'true' + AfterUnion: 'true' + BeforeCatch: 'true' + BeforeElse: 'true' + IndentBraces: 'false' + BreakBeforeBinaryOperators: false BreakBeforeTernaryOperators: false BreakConstructorInitializersBeforeComma: true -BinPackParameters: true -ColumnLimit: 120 +BreakBeforeBraces: Custom # Configure each individual brace in BraceWrapping + +ColumnLimit: 120 ConstructorInitializerAllOnOneLineOrOnePerLine: true +ConstructorInitializerIndentWidth: 2 +ContinuationIndentWidth: 4 +Cpp11BracedListStyle: false DerivePointerBinding: false -PointerBindsToType: true ExperimentalAutoDetectBinPacking: false IndentCaseLabels: true +IndentFunctionDeclarationAfterType: false +IndentPPDirectives: AfterHash +IndentWidth: 2 MaxEmptyLinesToKeep: 1 NamespaceIndentation: None ObjCSpaceBeforeProtocolList: true PenaltyBreakBeforeFirstCallParameter: 19 PenaltyBreakComment: 60 -PenaltyBreakString: 1 PenaltyBreakFirstLessLess: 1000 +PenaltyBreakString: 1 PenaltyExcessCharacter: 1000 PenaltyReturnTypeOnItsOwnLine: 90 -SpacesBeforeTrailingComments: 2 -Cpp11BracedListStyle: false -Standard: Auto -IndentWidth: 2 -TabWidth: 2 -UseTab: Never -IndentFunctionDeclarationAfterType: false -SpacesInParentheses: false -SpacesInAngles: false -SpaceInEmptyParentheses: false -SpacesInCStyleCastParentheses: false -SpaceAfterControlStatementKeyword: true -SpaceBeforeAssignmentOperators: true -ContinuationIndentWidth: 4 +PointerBindsToType: true SortIncludes: false SpaceAfterCStyleCast: false - -# Configure each individual brace in BraceWrapping -BreakBeforeBraces: Custom - -# Control of individual brace wrapping cases -BraceWrapping: { - AfterCaseLabel: 'true' - AfterClass: 'true' - AfterControlStatement: 'true' - AfterEnum : 'true' - AfterFunction : 'true' - AfterNamespace : 'true' - AfterStruct : 'true' - AfterUnion : 'true' - BeforeCatch : 'true' - BeforeElse : 'true' - IndentBraces : 'false' -} -... +SpaceAfterControlStatementKeyword: true +SpaceBeforeAssignmentOperators: true +SpaceInEmptyParentheses: false +SpacesBeforeTrailingComments: 2 +SpacesInAngles: false +SpacesInCStyleCastParentheses: false +SpacesInParentheses: false +Standard: Auto +TabWidth: 2 +UseTab: Never diff --git a/.github/helpers/check_urls.sh b/.github/helpers/check_urls.sh index 5d3662ee..d3aa1861 100755 --- a/.github/helpers/check_urls.sh +++ b/.github/helpers/check_urls.sh @@ -38,6 +38,7 @@ urls=$(grep -oP '(http|ftp|https):\/\/([a-zA-Z0-9_-]+(?:(?:\.[a-zA-Z0-9_-]+)+))( fail_counter=0 FAILED_LINKS=() +CHECKED_LINKS=() for item in $urls; do # echo $item skip=0 @@ -53,15 +54,18 @@ for item in $urls; do filename=$(echo "$item" | cut -d':' -f1) url=$(echo "$item" | cut -d':' -f2-) echo -n "Checking $url from file $filename" - if ! curl --head --silent --fail "$url" 2>&1 > /dev/null; then - echo -e " \033[0;31mNOT FOUND\033[32m\n" + if [[ $(echo ${CHECKED_LINKS[@]} | fgrep -w $url) ]]; then + echo -e " \033[36malready checked\033[0m" + elif ! curl --head --silent --fail "$url" 2>&1 > /dev/null; then + echo -e " \033[0;31mNOT FOUND\033[0m" FAILED_LINKS+=("$url from file $filename") ((fail_counter=fail_counter+1)) else - printf " \033[32mok\033[0m\n" + echo -e " \033[32mok\033[0m" fi + CHECKED_LINKS+=("$url") done echo "Failed files:" printf '%s\n' "${FAILED_LINKS[@]}" -exit $fail_counter \ No newline at end of file +exit $fail_counter diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 23f034f6..f0ac9d4a 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -4,7 +4,9 @@ on: pull_request: push: branches: - - main + - master + schedule: + - cron: '38 2 * * *' jobs: build: @@ -15,22 +17,13 @@ jobs: fail-fast: false matrix: env: - - DOCKER_RUN_OPTS: --network ursim_net - BEFORE_INIT: 'apt-get update -qq && apt-get install -y iproute2 iputils-ping && ip addr && ping -c5 192.168.56.101' - CTEST_OUTPUT_ON_FAILURE: 1 - ROBOT_MODEL: 'ur5' + - ROBOT_MODEL: 'ur5' URSIM_VERSION: '3.14.3' PROGRAM_FOLDER: 'tests/resources/dockerursim/programs/cb3' - - DOCKER_RUN_OPTS: --network ursim_net - BEFORE_INIT: 'apt-get update -qq && apt-get install -y iproute2 iputils-ping && ip addr && ping -c5 192.168.56.101' - CTEST_OUTPUT_ON_FAILURE: 1 - ROBOT_MODEL: 'ur5e' + - ROBOT_MODEL: 'ur5e' URSIM_VERSION: '5.9.4' PROGRAM_FOLDER: 'tests/resources/dockerursim/programs/e-series' - - DOCKER_RUN_OPTS: --network ursim_net - BEFORE_INIT: 'apt-get update -qq && apt-get install -y iproute2 iputils-ping && ip addr && ping -c5 192.168.56.101' - CTEST_OUTPUT_ON_FAILURE: 1 - ROBOT_MODEL: 'ur20' + - ROBOT_MODEL: 'ur20' URSIM_VERSION: 'latest' PROGRAM_FOLDER: 'tests/resources/dockerursim/programs/e-series' @@ -39,7 +32,11 @@ jobs: - name: start ursim run: | scripts/start_ursim.sh -m $ROBOT_MODEL -v $URSIM_VERSION -p $PROGRAM_FOLDER -d - env: ${{matrix.env}} + env: + DOCKER_RUN_OPTS: --network ursim_net + ROBOT_MODEL: ${{matrix.env.ROBOT_MODEL}} + URSIM_VERSION: ${{matrix.env.URSIM_VERSION}} + PROGRAM_FOLDER: ${{matrix.env.PROGRAM_FOLDER}} - name: configure run: mkdir build && cd build && cmake .. -DBUILDING_TESTS=1 -DINTEGRATION_TESTS=1 -DWITH_ASAN=ON env: @@ -51,7 +48,13 @@ jobs: - name: Create folder for test artifacts run: mkdir -p test_artifacts - name: test - run: cd build && ctest --output-on-failure + run: cd build && ctest --output-on-failure --output-junit junit.xml + - name: Upload test results to Codecov + if: ${{ !cancelled() }} + uses: codecov/test-results-action@v1 + with: + token: ${{ secrets.CODECOV_TOKEN }} + fail_ci_if_error: true - name: run examples run: ./run_examples.sh "192.168.56.101" 1 - name: install gcovr @@ -63,6 +66,7 @@ jobs: with: fail_ci_if_error: true token: ${{ secrets.CODECOV_TOKEN }} + flags: ${{ matrix.env.ROBOT_MODEL }}-${{ matrix.env.URSIM_VERSION }} - name: Generate URSim log files if: always() run: | @@ -103,5 +107,5 @@ jobs: run: | .github/helpers/check_urls.sh \ -d ".git build CMakeModules debian" \ - -f "package.xml urcl_architecture.svg trajectory_interface.svg dataflow.graphml start_ursim.sh" \ + -f "package.xml urcl_architecture.svg trajectory_interface.svg dataflow.graphml start_ursim.sh ursim_docker.rst" \ -p "vnc\.html opensource\.org\/licenses\/BSD-3-Clause kernel\.org\/pub\/linux\/kernel" diff --git a/3rdparty/endian/endian.h b/3rdparty/endian/endian.h index 3ed95719..9c5500b2 100644 --- a/3rdparty/endian/endian.h +++ b/3rdparty/endian/endian.h @@ -16,88 +16,88 @@ // Byte order #if defined(linux) || defined(__linux) || defined(__linux__) || defined(__CYGWIN__) -# include +# include #elif defined(__APPLE__) -# include +# include -# define htobe16(x) OSSwapHostToBigInt16(x) -# define htole16(x) OSSwapHostToLittleInt16(x) -# define be16toh(x) OSSwapBigToHostInt16(x) -# define le16toh(x) OSSwapLittleToHostInt16(x) +# define htobe16(x) OSSwapHostToBigInt16(x) +# define htole16(x) OSSwapHostToLittleInt16(x) +# define be16toh(x) OSSwapBigToHostInt16(x) +# define le16toh(x) OSSwapLittleToHostInt16(x) -# define htobe32(x) OSSwapHostToBigInt32(x) -# define htole32(x) OSSwapHostToLittleInt32(x) -# define be32toh(x) OSSwapBigToHostInt32(x) -# define le32toh(x) OSSwapLittleToHostInt32(x) +# define htobe32(x) OSSwapHostToBigInt32(x) +# define htole32(x) OSSwapHostToLittleInt32(x) +# define be32toh(x) OSSwapBigToHostInt32(x) +# define le32toh(x) OSSwapLittleToHostInt32(x) -# define htobe64(x) OSSwapHostToBigInt64(x) -# define htole64(x) OSSwapHostToLittleInt64(x) -# define be64toh(x) OSSwapBigToHostInt64(x) -# define le64toh(x) OSSwapLittleToHostInt64(x) +# define htobe64(x) OSSwapHostToBigInt64(x) +# define htole64(x) OSSwapHostToLittleInt64(x) +# define be64toh(x) OSSwapBigToHostInt64(x) +# define le64toh(x) OSSwapLittleToHostInt64(x) -# define __BYTE_ORDER BYTE_ORDER -# define __BIG_ENDIAN BIG_ENDIAN -# define __LITTLE_ENDIAN LITTLE_ENDIAN -# define __PDP_ENDIAN PDP_ENDIAN +# define __BYTE_ORDER BYTE_ORDER +# define __BIG_ENDIAN BIG_ENDIAN +# define __LITTLE_ENDIAN LITTLE_ENDIAN +# define __PDP_ENDIAN PDP_ENDIAN #elif defined(__OpenBSD__) -# include +# include #elif defined(__NetBSD__) || defined(__FreeBSD__) || defined(__DragonFly__) -# include +# include -# define be16toh(x) betoh16(x) -# define le16toh(x) letoh16(x) +# define be16toh(x) betoh16(x) +# define le16toh(x) letoh16(x) -# define be32toh(x) betoh32(x) -# define le32toh(x) letoh32(x) +# define be32toh(x) betoh32(x) +# define le32toh(x) letoh32(x) -# define be64toh(x) betoh64(x) -# define le64toh(x) letoh64(x) +# define be64toh(x) betoh64(x) +# define le64toh(x) letoh64(x) #elif defined(_WIN32) -# include -# if BYTE_ORDER == LITTLE_ENDIAN -# if defined(_MSC_VER) -# define htobe16(x) _byteswap_ushort(x) -# define htole16(x) (x) -# define be16toh(x) _byteswap_ushort(x) -# define le16toh(x) (x) +# include +# if BYTE_ORDER == LITTLE_ENDIAN +# if defined(_MSC_VER) +# define htobe16(x) _byteswap_ushort(x) +# define htole16(x) (x) +# define be16toh(x) _byteswap_ushort(x) +# define le16toh(x) (x) -# define htobe32(x) _byteswap_ulong(x) -# define htole32(x) (x) -# define be32toh(x) _byteswap_ulong(x) -# define le32toh(x) (x) +# define htobe32(x) _byteswap_ulong(x) +# define htole32(x) (x) +# define be32toh(x) _byteswap_ulong(x) +# define le32toh(x) (x) -# define htobe64(x) _byteswap_uint64(x) -# define htole64(x) (x) -# define be64toh(x) _byteswap_uint64(x) -# define le64toh(x) (x) -# elif defined(__GNUC__) || defined(__clang__) -# define htobe16(x) __builtin_bswap16(x) -# define htole16(x) (x) -# define be16toh(x) __builtin_bswap16(x) -# define le16toh(x) (x) +# define htobe64(x) _byteswap_uint64(x) +# define htole64(x) (x) +# define be64toh(x) _byteswap_uint64(x) +# define le64toh(x) (x) +# elif defined(__GNUC__) || defined(__clang__) +# define htobe16(x) __builtin_bswap16(x) +# define htole16(x) (x) +# define be16toh(x) __builtin_bswap16(x) +# define le16toh(x) (x) -# define htobe32(x) __builtin_bswap32(x) -# define htole32(x) (x) -# define be32toh(x) __builtin_bswap32(x) -# define le32toh(x) (x) +# define htobe32(x) __builtin_bswap32(x) +# define htole32(x) (x) +# define be32toh(x) __builtin_bswap32(x) +# define le32toh(x) (x) -# define htobe64(x) __builtin_bswap64(x) -# define htole64(x) (x) -# define be64toh(x) __builtin_bswap64(x) -# define le64toh(x) (x) -# else -# error Compiler is not supported -# endif -# else -# error Byte order is not supported -# endif +# define htobe64(x) __builtin_bswap64(x) +# define htole64(x) (x) +# define be64toh(x) __builtin_bswap64(x) +# define le64toh(x) (x) +# else +# error Compiler is not supported +# endif +# else +# error Byte order is not supported +# endif -# define __BYTE_ORDER BYTE_ORDER -# define __BIG_ENDIAN BIG_ENDIAN -# define __LITTLE_ENDIAN LITTLE_ENDIAN -# define __PDP_ENDIAN PDP_ENDIAN +# define __BYTE_ORDER BYTE_ORDER +# define __BIG_ENDIAN BIG_ENDIAN +# define __LITTLE_ENDIAN LITTLE_ENDIAN +# define __PDP_ENDIAN PDP_ENDIAN #else -# error Platform is not supported +# error Platform is not supported #endif -#endif // PORTABLE_ENDIAN_H__ +#endif // PORTABLE_ENDIAN_H__ diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 1ec44eca..1e3189f9 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,6 +2,26 @@ Changelog for package ur_client_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.6.0 (2025-01-23) +------------------ +* Do not throw exception in DashboardClient::sendRequest (`#249 `_) +* Add instruction executor for high-level robot control (`#242 `_) +* Modernize cmake (`#244 `_) +* Update links to dashboard server documentation (`#243 `_) +* Trajectory point velocities and example (`#241 `_) +* Updated documentation (`#228 `_) +* Update ci (`#239 `_) +* Enable force mode compatibility with various move types (`#230 `_) +* Update package maintainers (`#238 `_) +* Bump codecov/codecov-action from 3 to 5 (`#234 `_) +* Remove the not regarding MIT license (`#237 `_) +* Bump pre-commit/action from 3.0.0 to 3.0.1 (`#236 `_) +* Bump actions/checkout from 1 to 4 (`#232 `_) +* Bump actions/setup-python from 4 to 5 (`#235 `_) +* Bump actions/upload-artifact from 3 to 4 (`#233 `_) +* Add dependabot configuration to update actions (`#231 `_) +* Contributors: Felix Exner, Rune Søe-Knudsen, dependabot[bot] + 1.5.0 (2024-11-25) ------------------ * Adapt RTDE output recipe based on robot response (`#221 `_) diff --git a/CMakeLists.txt b/CMakeLists.txt index 3c99f77a..dd720dc5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -22,9 +22,11 @@ add_library(urcl src/control/trajectory_point_interface.cpp src/control/script_command_interface.cpp src/primary/primary_package.cpp + src/primary/primary_client.cpp src/primary/robot_message.cpp src/primary/robot_state.cpp src/primary/robot_message/version_message.cpp + src/primary/robot_message/error_code_message.cpp src/primary/robot_state/kinematics_info.cpp src/rtde/control_package_pause.cpp src/rtde/control_package_setup_inputs.cpp @@ -39,11 +41,13 @@ add_library(urcl src/ur/ur_driver.cpp src/ur/calibration_checker.cpp src/ur/dashboard_client.cpp + src/ur/instruction_executor.cpp src/ur/tool_communication.cpp src/ur/robot_receive_timeout.cpp src/ur/version_information.cpp src/rtde/rtde_writer.cpp src/default_log_handler.cpp + src/example_robot_wrapper.cpp src/log.cpp src/helpers.cpp ) diff --git a/README.md b/README.md index 27d46f93..24e6f169 100644 --- a/README.md +++ b/README.md @@ -4,6 +4,10 @@ A C++ library for accessing Universal Robots interfaces. With this library C++-b implemented in order to create external applications leveraging the versatility of Universal Robots robotic manipulators. +The library has no external dependencies besides the standard C++ libraries such as ROS, or boost +to make it easy to integrate and maintain. It also serves as the foundation for the ROS and ROS 2 +drivers. +
Universal Robot family diff --git a/doc/architecture.rst b/doc/architecture.rst index 98747d0f..040db7c5 100644 --- a/doc/architecture.rst +++ b/doc/architecture.rst @@ -14,6 +14,7 @@ well as a couple of standalone modules to directly use subsets of the library's architecture/script_sender architecture/trajectory_point_interface architecture/ur_driver + architecture/instruction_executor Dataflow overview with UrDriver diff --git a/doc/architecture/instruction_executor.rst b/doc/architecture/instruction_executor.rst new file mode 100644 index 00000000..65818737 --- /dev/null +++ b/doc/architecture/instruction_executor.rst @@ -0,0 +1,26 @@ +.. _instruction_executor: + +Instruction Executor +==================== + +The Instruction Executor is a convenience wrapper to make common robot instructions such as point +to point motions easily accessible. Currently, it supports the following instructions: + +* Excecute MoveJ point to point motions +* Execute MoveL point to point motions +* Execute sequences consisting of MoveJ and MoveL instructions + +The Instruction Executor uses the :ref:`trajectory_point_interface` and the +:ref:`reverse_interface` +for sending motion instructions to the robot. Hence, it requires a :ref:`ur_driver` object. + +.. note:: + The ``InstructionExecutor`` simply forwards the trajectory points to the robot. Execution + is done using respective URScript functions such as `movej + `_ + or `movel + `_. + Therefore, all parameters and restrictions of these functions apply. For example, velocity and + acceleration parameters will be ignored if there is a time > 0 given. + +As a minimal working example, please see the :ref:`instruction_executor_example`. diff --git a/doc/architecture/script_sender.rst b/doc/architecture/script_sender.rst index 114f4f12..4644c186 100644 --- a/doc/architecture/script_sender.rst +++ b/doc/architecture/script_sender.rst @@ -13,15 +13,7 @@ corresponding request when starting a program on the robot that contains the **E program node. In order to work properly, make sure that the IP address and script sender port are configured correctly on the robot. -The following example creates a ``ScriptSender`` listening on port ``12345`` and sends the script -``textmsg("Hello, World!")`` when requested. A fully compilable example can be found in `script_sender.cpp `_ - -.. literalinclude:: ../../examples/script_sender.cpp - :language: c++ - :caption: examples/script_sender.cpp - :linenos: - :lineno-match: - :start-at: constexpr uint32_t PORT +An example of how to use the ``ScriptSender`` class can be found in the :ref:`script_sender_example`. .. note:: PolyScope X users cannot use the URCap linked above. There is a development version of a URCapX diff --git a/doc/architecture/trajectory_point_interface.rst b/doc/architecture/trajectory_point_interface.rst index b9fdbc90..bdb089db 100644 --- a/doc/architecture/trajectory_point_interface.rst +++ b/doc/architecture/trajectory_point_interface.rst @@ -8,7 +8,7 @@ be used in conjunction with the :ref:`reverse_interface` during trajectory forwa Communication regarding trajectory point execution would usually look like this: -.. figure:: /images/trajectory_interface.svg +.. figure:: ../images/trajectory_interface.svg :width: 100% :alt: Trajectory interface @@ -17,6 +17,15 @@ control PC to the robot for execution. Execution isn't started, before a start c the ``ReverseInterface``. Once trajectory execution is done (either successful, failed or canceled externally), the robot will send a response back to the control PC via the trajectory socket. +.. note:: + The ``TrajectoryPointInterface`` simply forwards the trajectory points to the robot. Execution + is done using respective URScript functions such as `movej + `_ + or `movel + `_. + Therefore, all parameters and restrictions of these functions apply. For example, velocity and + acceleration parameters will be ignored if there is a time > 0 given. + Communication protocol ---------------------- diff --git a/doc/conf.py b/doc/conf.py index 68cf4852..67d788c3 100644 --- a/doc/conf.py +++ b/doc/conf.py @@ -4,7 +4,7 @@ # # This file does only contain a selection of the most common options. For a # full list see the documentation: -# http://www.sphinx-doc.org/en/master/config +# https://www.sphinx-doc.org/en/master/config # -- Path setup -------------------------------------------------------------- @@ -46,9 +46,10 @@ # extensions coming with Sphinx (named 'sphinx.ext.*') or your custom # ones. extensions = [ + "sphinx_tabs.tabs", + "sphinx.ext.todo", "sphinx_copybutton", "sphinx_rtd_theme", - "sphinx.ext.todo", ] copybutton_exclude = ".linenos, .gp, .go" diff --git a/doc/example.rst b/doc/example.rst deleted file mode 100644 index da167bf9..00000000 --- a/doc/example.rst +++ /dev/null @@ -1,12 +0,0 @@ -.. _example-driver: - -Example driver -============== -In the ``examples`` subfolder you will find a minimal example of a running driver. It starts an -instance of the ``UrDriver`` class and prints the RTDE values read from the controller. To run it make -sure to - -* have an instance of a robot controller / URSim running at the configured IP address (or adapt the - address to your needs) -* run it from the package's main folder (the one where this README.md file is stored), as for - simplicity reasons it doesn't use any sophisticated method to locate the required files. diff --git a/doc/examples.rst b/doc/examples.rst new file mode 100644 index 00000000..4e5ad519 --- /dev/null +++ b/doc/examples.rst @@ -0,0 +1,28 @@ +Usage examples +============== + +This library contains a number of examples how this library can be used. You can use them as a +starting point for your own applications. + +All examples take a robot's IP address as a first argument and some of them take a maximum run +duration (in seconds) as second argument. If the second argument is omitted, some of the examples +may be running forever until manually stopped. + +.. note:: Most of these examples use the driver's headless mode. Therefore, on an e-Series (PolyScope 5) robot, the robot has to be + in *remote control mode* to work. + +.. toctree:: + :maxdepth: 1 + + examples/dashboard_client + examples/force_mode + examples/freedrive + examples/instruction_executor + examples/primary_pipeline + examples/primary_pipeline_calibration + examples/rtde_client + examples/script_sender + examples/spline_example + examples/tool_contact_example + examples/trajectory_point_interface + examples/ur_driver diff --git a/doc/examples/dashboard_client.rst b/doc/examples/dashboard_client.rst new file mode 100644 index 00000000..2b5ae406 --- /dev/null +++ b/doc/examples/dashboard_client.rst @@ -0,0 +1,82 @@ +:github_url: https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/doc/examples/dashboard_client.rst + +Dashboard client example +======================== + +This example shows how to use the builtin `Dashboard server `_ to communicate with a robot. + +.. note:: + + The Dashboard Server is only available on CB3 and e-Series robots. It is not available on + PolyScope X. + + +The `dashboard_example.cpp `_ shows how to use this class: + +.. note:: For the following example to work on an e-Series (PolyScope 5) robot, the robot has to be + in *remote control mode*. + +.. literalinclude:: ../../examples/dashboard_example.cpp + :language: c++ + :caption: examples/dashboard_example.cpp + :linenos: + :lineno-match: + :start-at: std::make_unique + :end-at: my_dashboard->commandCloseSafetyPopup(); + +At first, a ``DashboardClient`` object is created with the IP address of the robot. Then, the +client is connected to the robot. After that, the client sends a command to the robot and receives +the answer. + +Some commands support getting the response from the robot. For example, the +``commandPolyScopeVersion()`` function: + +.. literalinclude:: ../../examples/dashboard_example.cpp + :language: c++ + :caption: examples/dashboard_example.cpp + :linenos: + :lineno-match: + :start-at: // Get the PolyScope version + :end-at: URCL_LOG_INFO(version.c_str()); + + +The ``DashboardClient`` can easily be used to cycle through the robot's states, for example for +initialization: + +.. literalinclude:: ../../examples/dashboard_example.cpp + :language: c++ + :caption: examples/dashboard_example.cpp + :linenos: + :lineno-match: + :start-at: // Power it on + :end-at: // Load existing program + +All commands are blocking and will wait for the necessary action being done. The dashboard server's +response will be compared with an expected response. For example, when calling +``commandPowerOn(timeout)``, it is checked that the dashboard server is answering ``"Powering on"`` and +then it is queried until the robot reports ``"Robotmode: IDLE"`` or until the timeout is reached. +The example contains more commands that follow the same scheme. + + +If you want to send a query / command to the dashboard server and only want to receive the +response, you can use the ``sendAndReceive()`` function: + +.. literalinclude:: ../../examples/dashboard_example.cpp + :language: c++ + :caption: examples/dashboard_example.cpp + :linenos: + :lineno-match: + :start-at: // Make a raw request and save the response + :end-at: URCL_LOG_INFO("Program state: %s", program_state.c_str()); + +For checking the response against an expected regular expression use ``sendRequest()``: + +.. literalinclude:: ../../examples/dashboard_example.cpp + :language: c++ + :caption: examples/dashboard_example.cpp + :linenos: + :lineno-match: + :start-at: // The response can be checked with a regular expression + :end-at: URCL_LOG_INFO("Power off command success: %d", success); + + diff --git a/doc/examples/force_mode.rst b/doc/examples/force_mode.rst new file mode 100644 index 00000000..6ee364d3 --- /dev/null +++ b/doc/examples/force_mode.rst @@ -0,0 +1,64 @@ +:github_url: https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/doc/examples/force_mode.rst + +Force Mode example +================== + +The ``ur_client_library`` supports leveraging the robot's force mode directly. An example on how to +use it can be found in `force_mode_example.cpp `_. + +In order to utilize force mode, we'll have to create and initialize a driver object +first. We use the ``ExampleRobotWrapper`` class for this example. That starts a :ref:`ur_driver` +and a :ref:`dashboard_client` to communicate with the robot: + +.. literalinclude:: ../../examples/force_mode_example.cpp + :language: c++ + :caption: examples/force_mode_example.cpp + :linenos: + :lineno-match: + :start-at: bool headless_mode = true; + :end-at: // End of initialization + +Start force mode +---------------- + +After that, we can start force mode by calling the ``startForceMode()`` function: + +.. literalinclude:: ../../examples/force_mode_example.cpp + :language: c++ + :caption: examples/force_mode_example.cpp + :linenos: + :lineno-match: + :start-at: // Start force mode + :end-at: if (!success) + +All parameters for the force mode are included into the ``startForceMode()`` function call. If you +want to change the parameters, e.g. change the forces applied, you can simply call +``startForceMode()`` again with the new parameters. + +.. note:: + CB3 robots don't support specifying force_mode's ``gain_scaling``, so there are two different + functions available. + +Once force mode is started successfully, we'll have to send keepalive messages to the robot in +order to keep the communication active: + +.. literalinclude:: ../../examples/force_mode_example.cpp + :language: c++ + :caption: examples/force_mode_example.cpp + :linenos: + :lineno-match: + :start-at: std::chrono::duration time_done(0); + :end-at: URCL_LOG_INFO("Timeout reached."); + +Stop force mode +--------------- + +Once finished, force_mode can be stopped by calling ``endForceMode()``. + +.. literalinclude:: ../../examples/force_mode_example.cpp + :language: c++ + :caption: examples/force_mode_example.cpp + :linenos: + :lineno-match: + :start-at: endForceMode() + :end-at: endForceMode() diff --git a/doc/examples/freedrive.rst b/doc/examples/freedrive.rst new file mode 100644 index 00000000..9b3d9fd3 --- /dev/null +++ b/doc/examples/freedrive.rst @@ -0,0 +1,70 @@ +:github_url: https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/doc/examples/freedrive.rst + +Freedrive Mode example +====================== + +`Freedrive +`_ +allows the robot arm to be manually pulled into desired positions and/or poses. The joints move +with little resistance because the brakes are released. + +An example to utilize the freedrive mode can be found in the `freedrive_example.cpp `_. + +.. note:: For the following example to work on an e-Series (PolyScope 5) robot, the robot has to be + in *remote control mode*. + +At first, we create a ``ExampleRobotWrapper`` object in order to initialize communication with the +robot. + +.. literalinclude:: ../../examples/freedrive_example.cpp + :language: c++ + :caption: examples/freedrive_example.cpp + :linenos: + :lineno-match: + :start-at: bool headless_mode = true; + :end-before: URCL_LOG_INFO("Starting freedrive mode"); + + +Start freedrive mode +-------------------- + +The ``UrDriver`` provides a method to start freedrive mode directly: + +.. literalinclude:: ../../examples/freedrive_example.cpp + :language: c++ + :caption: examples/freedrive_example.cpp + :linenos: + :lineno-match: + :start-at: URCL_LOG_INFO("Starting freedrive mode"); + :end-at: sendFreedriveMessageOrDie(control::FreedriveControlMessage::FREEDRIVE_START); + +As it is potentially dangerous to leave the robot in freedrive mode, the robot program expect +frequent keepalive messages to verify that the remote connection is still available and freedrive +mode is being expected to be active. + +Freedrive mode will be active from this point on until it is either stopped, or no keepalive +message is received by the robot anymore. + +Therefore, we have to make sure to send regular keepalive messages to the robot. The following +section will keep freedrive mode active for a period of time defined in ``seconds_to_run``. + +.. literalinclude:: ../../examples/freedrive_example.cpp + :language: c++ + :caption: examples/freedrive_example.cpp + :linenos: + :lineno-match: + :start-at: std::chrono::duration time_done(0); + :end-before: sendFreedriveMessageOrDie(control::FreedriveControlMessage::FREEDRIVE_STOP); + +Stop force Mode +--------------- + +To stop force mode either stop sending keepalive signals or request deactivating it explicitly: + +.. literalinclude:: ../../examples/freedrive_example.cpp + :language: c++ + :caption: examples/freedrive_example.cpp + :linenos: + :lineno-match: + :start-at: sendFreedriveMessageOrDie(control::FreedriveControlMessage::FREEDRIVE_STOP); + :end-at: sendFreedriveMessageOrDie(control::FreedriveControlMessage::FREEDRIVE_STOP); diff --git a/doc/examples/instruction_executor.rst b/doc/examples/instruction_executor.rst new file mode 100644 index 00000000..58439972 --- /dev/null +++ b/doc/examples/instruction_executor.rst @@ -0,0 +1,66 @@ +:github_url: https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/doc/examples/instruction_executor.rst + +.. _instruction_executor_example: + +Instruction Executor example +============================ + +This example shows how to use the :ref:`instruction_executor` class. It can be used for easily +executing a sequence of instructions such as motions on the robot using the built-in URScript functions. + +The `instruction_executor.cpp `_ shows how to use this class: + +.. note:: For the instruciton executor to work there has to be an established connection to the + :ref:`reverse_interface`. That means, the respective program has to be running on the robot. The + example below will do that automatically, if the connected robot is in *remote_control* mode. + + +.. literalinclude:: ../../examples/instruction_executor.cpp + :language: c++ + :caption: examples/instruction_executor.cpp + :linenos: + :lineno-match: + :start-at: bool headless_mode = true; + :end-at: auto instruction_executor = std::make_shared(g_my_robot->ur_driver_); + +At first, a ``InstructionExecutor`` object is created with the URDriver object as it needs that +for communication with the robot. + +Currently, the ``InstructionExecutor`` can either be used to run sequences of motions or single motions. + +Execute a sequence of motions +----------------------------- + +To run a sequence of motions, create an +``std::vector>`` and pass it to the +``executeMotion`` function: + +.. literalinclude:: ../../examples/instruction_executor.cpp + :language: c++ + :caption: examples/instruction_executor.cpp + :linenos: + :lineno-match: + :start-at: // Trajectory definition + :end-at: instruction_executor->executeMotion(motion_sequence); + +Each element in the motion sequence can be a different motion type. In the example, there are two +``MoveJ`` motions and two ``MoveL`` motion. The primitives' parameters are directly forwarded to +the underlying script functions, so the parameter descriptions for them apply, as well. +Particularly, you may want to choose between either a time-based execution speed or an acceleration +/ velocity parametrization. The latter will be ignored if a time > 0 is given. + +Execute a single motion +----------------------- + +To run a single motion, the ``InstructionExecutor`` provides the methods ``moveJ(...)`` and +``moveL(...)``: + +.. literalinclude:: ../../examples/instruction_executor.cpp + :language: c++ + :caption: examples/instruction_executor.cpp + :linenos: + :lineno-match: + :start-at: double goal_time_sec = 2.0; + :end-before: g_my_robot->ur_driver_->stopControl(); + +Again, time parametrization has priority over acceleration / velocity parameters. diff --git a/doc/examples/primary_pipeline.rst b/doc/examples/primary_pipeline.rst new file mode 100644 index 00000000..24b62f42 --- /dev/null +++ b/doc/examples/primary_pipeline.rst @@ -0,0 +1,78 @@ +:github_url: https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/doc/examples/primary_pipeline.rst + +.. _primary_pipeline_example: + +Primary Pipeline example +======================== + +This example shows how to use the ``Pipeline`` class for the robot's `primary interface +`_. +It reads all packages coming in from the robot't primary interface and prints their contents. + +At the current time parsing primary interface data is very limited, so this example will print the +raw binary data for most package types. The example serves to demonstrate the basic control flow +used for reading data from the robot. + +In this library, a "pipeline" uses a producer / consumer architecture. A producer is reading data +from a *stream*, parses that data and puts it into a *queue*. A consumer reads data from the queue +and can do whatever its purpose is. + +Producer setup +-------------- + +To setup the producer, we need to create a stream, a parser and create a producer with those: + +.. literalinclude:: ../../examples/primary_pipeline.cpp + :language: c++ + :caption: examples/primary_pipeline.cpp + :linenos: + :lineno-match: + :start-at: // First of all, we need a stream + :end-at: prod.setupProducer(); + +Consumer setup +-------------- + +The consumer can be any consumer that is able to consume data produced by the producer, in this +case ``urcl::primary_interface::PrimaryPackage``. Here, we use a ``ShellExecutor`` that will try to +print each package's content to the shell output: + +.. literalinclude:: ../../examples/primary_pipeline.cpp + :language: c++ + :caption: examples/primary_pipeline.cpp + :linenos: + :lineno-match: + :start-at: // The shell consumer + :end-at: auto consumer = std::make_unique + +Assemble the pipeline +--------------------- + +Finally, we need to assemble the pipeline by connecting the producer to the consumer: + +.. literalinclude:: ../../examples/primary_pipeline.cpp + :language: c++ + :caption: examples/primary_pipeline.cpp + :linenos: + :lineno-match: + :start-after: auto consumer = std::make_unique + :end-at: pipeline.run() + +You can setup a custom notifier that can handle start and stop events of the pipeline. In this +example we use the basic ``INotifier`` which doesn't do anything. + +With all that, we can create the ``pipeline`` by passing the producer, consumer, a name and the +notifier to it's constructor. + +From this point on, the producer will read the data coming on from the stream and that data will be +processed by the consumer. We keep the example program alive for a while to see some data: + +.. literalinclude:: ../../examples/primary_pipeline.cpp + :language: c++ + :caption: examples/primary_pipeline.cpp + :linenos: + :lineno-match: + :start-at: do + :end-at: return 0 + + diff --git a/doc/examples/primary_pipeline_calibration.rst b/doc/examples/primary_pipeline_calibration.rst new file mode 100644 index 00000000..fa9d7845 --- /dev/null +++ b/doc/examples/primary_pipeline_calibration.rst @@ -0,0 +1,52 @@ +:github_url: https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/doc/examples/primary_pipeline_calibration.rst + +.. _primary_pipeline_calibration_example: + +Primary Pipeline Calibration example +==================================== + +This example is very similar to the :ref:`primary_pipeline_example`. However, it uses a +specialized consumer that will analyze the calibration data sent from the robot instead of the +``ShellConsumer``. + +Consumer setup +-------------- + +This example uses a specialized type of consumer that stores data about the received robot +calibration. + +.. literalinclude:: ../../examples/primary_pipeline_calibration.cpp + :language: c++ + :caption: examples/primary_pipeline_calibration.cpp + :linenos: + :lineno-match: + :start-at: class CalibrationConsumer + :end-at: }; + +Since the producer is reading every package from the primary interface, the consumer has to be able +to consume any primary package. + +Assemble the pipeline +--------------------- + +The rest of the pipeline setup is the same as in the other pipeline example, just that we create a +``CalibrationConsumer`` instead of a ``ShellConsumer``: + +.. literalinclude:: ../../examples/primary_pipeline_calibration.cpp + :language: c++ + :caption: examples/primary_pipeline_calibration.cpp + :linenos: + :lineno-match: + :start-at: // The calibration consumer + :end-at: calib_pipeline.run() + +Again, product handling will be happening in the background, so in the rest of the program we wait +until the calibration consumer received and processed data and then print information about that: + +.. literalinclude:: ../../examples/primary_pipeline_calibration.cpp + :language: c++ + :caption: examples/primary_pipeline_calibration.cpp + :linenos: + :lineno-match: + :start-at: while (!calib_consumer + :end-at: return 0 diff --git a/doc/examples/rtde_client.rst b/doc/examples/rtde_client.rst new file mode 100644 index 00000000..bf47d97b --- /dev/null +++ b/doc/examples/rtde_client.rst @@ -0,0 +1,89 @@ +:github_url: https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/doc/examples/rtde_client.rst + +.. _rtde_client_example: + +RTDE Client example +=================== + +This example shows how to use the ``RTDEClient`` class for the robot's `Real-Time Data Exchange +(RTDE) interface +`_. + +The RTDE client has to be initialized with a list of keys that should be streamed from the robot +and a list of keys that should be sent to the robot. The client will then start a background thread +establishing communication. + +In this example, those keys are stored in two text files relative to this repository's root: +``rtde_input_keys.txt`` and ``rtde_output_keys.txt``. The example will read those files and use them +to initialize the RTDE client. + +.. literalinclude:: ../../examples/rtde_client.cpp + :language: c++ + :caption: examples/rtde_client.cpp + :linenos: + :lineno-match: + :start-at: const std::string OUTPUT_RECIPE + :end-at: const std::string INPUT_RECIPE + + +Internally, the RTDE client uses the same producer / consumer architecture as show in the +:ref:`primary_pipeline_example` example. However, it doesn't have a consumer thread, so data has to +be read by the user to avoid the pipeline's queue from overflowing. + +Creating an RTDE Client +----------------------- + +An RTDE client can be directly created passing the robot's IP address, a ``INotifier`` object, an +output and an input recipe. Optionally, a communication frequency can be passed as well. If that is +omitted, RTDE communication will be established at the robot's control frequency. + +.. literalinclude:: ../../examples/rtde_client.cpp + :language: c++ + :caption: examples/rtde_client.cpp + :linenos: + :lineno-match: + :start-at: comm::INotifier notifier; + :end-at: my_client.init(); + +An RTDE data package containing every key-value pair from the output recipe can be fetched using +the ``getDataPackage()`` method. This method will block until a new package is available. + + +Reading data from the RTDE client +--------------------------------- + +Once the RTDE client is initialized, we'll have to start communication separately. As mentioned +above, we'll have to read data from the client once communication is started, hence we start +communication right before a loop reading data. + +.. literalinclude:: ../../examples/rtde_client.cpp + :language: c++ + :caption: examples/rtde_client.cpp + :linenos: + :lineno-match: + :start-at: // Once RTDE communication is started + :end-before: // Change the speed slider + +Writing Data to the RTDE client +------------------------------- + +In this example, we use the RTDE client to oscillate the speed slider on the teach pendant between +0 and 1. While this doesn't bear any practical use it shows how sending data to the RTDE interface +works. + +To send data to the RTDE client, we can use ``RTDEWriter`` object stored in the RTDE client. This +has methods implemented for each data type that can be sent to the robot. The input recipe used to +initialize the RTDE client has to contain the keys necessary to send that specific data. + +.. literalinclude:: ../../examples/rtde_client.cpp + :language: c++ + :caption: examples/rtde_client.cpp + :linenos: + :lineno-match: + :start-at: my_client.getWriter().sendSpeedSlider + :end-at: } + + +.. note:: Many RTDE inputs require setting up the data key and a mask key. See the `RTDE guide + `_ + for more information. diff --git a/doc/examples/script_sender.rst b/doc/examples/script_sender.rst new file mode 100644 index 00000000..ba78d534 --- /dev/null +++ b/doc/examples/script_sender.rst @@ -0,0 +1,16 @@ +:github_url: https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/doc/examples/script_sender.rst + +.. _script_sender_example: + +Script Sender example +===================== + +The following example creates a ``ScriptSender`` listening on port ``12345`` and sends the script +``textmsg("Hello, World!")`` when requested. A fully compilable example can be found in `script_sender.cpp `_ + +.. literalinclude:: ../../examples/script_sender.cpp + :language: c++ + :caption: examples/script_sender.cpp + :linenos: + :lineno-match: + :start-at: constexpr uint32_t PORT diff --git a/doc/examples/spline_example.rst b/doc/examples/spline_example.rst new file mode 100644 index 00000000..5faf50e3 --- /dev/null +++ b/doc/examples/spline_example.rst @@ -0,0 +1,14 @@ +:github_url: https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/doc/examples/spline_example.rst + +.. _spline_example: + +Spline Interpolation example +============================ + +The URScript code inside `external_control.urscript +`_ implements trajectory execution based upon spline interpolation of a given sequence of waypoints. + +Depending on the waypoint parametrization, either cubic splines (positions, velocities, times) or +quintic splines (positions, velocities, accelerations, times) will be used. + +The example in `spline_example.cpp `_ shows how to use the :ref:`trajectory_point_interface` to execute trajectories using this spline interpolation. diff --git a/doc/examples/tool_contact_example.rst b/doc/examples/tool_contact_example.rst new file mode 100644 index 00000000..d38630b5 --- /dev/null +++ b/doc/examples/tool_contact_example.rst @@ -0,0 +1,60 @@ +:github_url: https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/doc/examples/tool_contact_example.rst + +.. _tool_contact_example: + +Tool Contact example +==================== + +Univeral Robots' **tool contact mode** allows detecting collisions between the robot's tool and the +environment and interrupting motions when that happens. This example demonstrates how to use the +tool contact mode to detect collisions and stop the robot. + +As a basic concept, we will move the robot linearly in the negative z axis until the tool hits +something or the program's timeout is hit. + +At first, we create a initialize a driver as usual: + +.. literalinclude:: ../../examples/tool_contact_example.cpp + :language: c++ + :caption: examples/tool_contact_example.cpp + :linenos: + :lineno-match: + :start-at: bool headless_mode = true; + :end-before: g_my_robot->ur_driver_->registerToolContactResultCallback + +We use a small helper function to make sure that the reverse interface is active and connected +before proceeding. + +When a tool contact is detected, a callback will be triggered. For that we define a function to +handle this event: + +.. literalinclude:: ../../examples/tool_contact_example.cpp + :language: c++ + :caption: examples/tool_contact_example.cpp + :linenos: + :lineno-match: + :start-at: void handleToolContactResult(control::ToolContactResult result) + :end-at: } + + + +This function is registered as a callback to the driver and then tool_contact mode is enabled: + +.. literalinclude:: ../../examples/tool_contact_example.cpp + :language: c++ + :caption: examples/tool_contact_example.cpp + :linenos: + :lineno-match: + :start-at: g_my_robot->ur_driver_->registerToolContactResultCallback(&handleToolContactResult); + :end-at: g_my_robot->ur_driver_->startToolContact() + +Once everything is initialized, we can start a control loop commanding the robot to move in the +negative z direction until the program timeout is hit or a tool contact is detected: + +.. literalinclude:: ../../examples/tool_contact_example.cpp + :language: c++ + :caption: examples/tool_contact_example.cpp + :linenos: + :lineno-match: + :start-at: // This will move the robot + :end-at: return 0; diff --git a/doc/examples/trajectory_point_interface.rst b/doc/examples/trajectory_point_interface.rst new file mode 100644 index 00000000..51607f36 --- /dev/null +++ b/doc/examples/trajectory_point_interface.rst @@ -0,0 +1,103 @@ +:github_url: https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/doc/examples/trajectory_point_interface.rst + +.. _trajecotry_joint_interface_example: + +Trajectory Joint Interface example +================================== + + + +At first, we create a ``UrDriver`` object as usual: + +.. literalinclude:: ../../examples/trajectory_point_interface.cpp + :language: c++ + :caption: examples/trajectory_point_interface.cpp + :linenos: + :lineno-match: + :start-at: bool headless_mode = true; + :end-before: // --------------- INITIALIZATION END ------------------- + +We use a small helper function to make sure that the reverse interface is active and connected +before proceeding. + + +Initialization +-------------- + +As trajectory execution will be triggered asynchronously, we define a callback function to handle a +finished trajectory. A trajectory is considered finished when the robot is no longer executing it, +independent of whether it successfully reached its final point. The trajectory result will reflect +whether it was executed successfully, was canceled upon request or failed for some reason. + +.. literalinclude:: ../../examples/trajectory_point_interface.cpp + :language: c++ + :caption: examples/trajectory_point_interface.cpp + :linenos: + :lineno-match: + :start-at: void trajDoneCallback(const urcl::control::TrajectoryResult& result) + :end-at: } + +That callback can be registered at the ``UrDriver`` object: + + +.. literalinclude:: ../../examples/trajectory_point_interface.cpp + :language: c++ + :caption: examples/trajectory_point_interface.cpp + :linenos: + :lineno-match: + :start-at: g_my_robot->ur_driver_->registerTrajectoryDoneCallback(&trajDoneCallback); + :end-at: g_my_robot->ur_driver_->registerTrajectoryDoneCallback(&trajDoneCallback); + + +MoveJ Trajectory +---------------- + +Then, in order to execute a trajectory, we need to define a trajectory as a sequence of points and +parameters. The following example shows execution of a 2-point trajectory using URScript's +``movej`` function: + +.. literalinclude:: ../../examples/trajectory_point_interface.cpp + :language: c++ + :caption: examples/trajectory_point_interface.cpp + :linenos: + :lineno-match: + :start-after: // --------------- MOVEJ TRAJECTORY ------------------- + :end-before: // --------------- END MOVEJ TRAJECTORY ------------------- + +In fact, the path is followed twice, once parametrized by a segment duration and once using maximum +velocity / acceleration settings. If a duration > 0 is given for a segment, the velocity and +acceleration settings will be ignored as in the underlying URScript functions. In the example +above, each of the ``g_my_robot->ur_driver_->writeTrajectoryPoint()`` calls will be translated into a +``movej`` command in URScript. + +While the trajectory is running, we need to tell the robot program that connection is still active +and we expect the trajectory to be running. This is being done by the +``g_my_robot->ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP);`` +call. + +MoveL Trajectory +---------------- + +Similar to the ``movej``-based trajectory, execution can be done interpolating in joint space: + +.. literalinclude:: ../../examples/trajectory_point_interface.cpp + :language: c++ + :caption: examples/trajectory_point_interface.cpp + :linenos: + :lineno-match: + :start-after: // --------------- MOVEL TRAJECTORY ------------------- + :end-before: // --------------- END MOVEL TRAJECTORY ------------------- + +Spline based interpolation +-------------------------- + +Similar to the :ref:`spline_example`, the trajectory point interface can be used to execute motions +using the spline interpolation: + +.. literalinclude:: ../../examples/trajectory_point_interface.cpp + :language: c++ + :caption: examples/trajectory_point_interface.cpp + :linenos: + :lineno-match: + :start-after: // --------------- SPLINE TRAJECTORY ------------------- + :end-before: // --------------- END SPLINE TRAJECTORY ------------------- diff --git a/doc/examples/ur_driver.rst b/doc/examples/ur_driver.rst new file mode 100644 index 00000000..874c4922 --- /dev/null +++ b/doc/examples/ur_driver.rst @@ -0,0 +1,73 @@ +:github_url: https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/doc/examples/ur_driver.rst + +.. _example-driver: + +Example driver +============== +In the ``examples`` subfolder you will find a minimal example of a running driver. It starts an +instance of the ``UrDriver`` class and prints the RTDE values read from the controller. To run it make +sure to + +* have an instance of a robot controller / URSim running at the configured IP address (or adapt the + address to your needs) +* run it from the package's main folder (the one where the README.md file is stored), as for + simplicity reasons it doesn't use any sophisticated method to locate the required files. + +This page will walk you through the `full_driver.cpp +`_ +example. + +Initialization +-------------- + +At first, we create a ``ExampleRobotWrapper`` object giving it the robot's IP address, script file and RTDE +recipes. + +.. literalinclude:: ../../examples/full_driver.cpp + :language: c++ + :caption: examples/full_driver.cpp + :linenos: + :lineno-match: + :start-at: bool headless_mode = true; + :end-at: // --------------- INITIALIZATION END ------------------- + +Robot control loop +------------------ + +This example reads the robot's joint positions, increments / decrements the 5th axis and sends that +back as a joint command for the next cycle. This way, the robot will move its wrist first until a +positive limit and then back to 0. + +To read the joint data, the driver's RTDE client is used: + + +.. literalinclude:: ../../examples/full_driver.cpp + :language: c++ + :caption: examples/full_driver.cpp + :linenos: + :lineno-match: + :start-at: // Once RTDE communication is started + :end-before: // Open loop control + +The first read operation will initialize the target buffer with the current robot position. Next, +the target joint configuration is calculated: + + +.. literalinclude:: ../../examples/full_driver.cpp + :language: c++ + :caption: examples/full_driver.cpp + :linenos: + :lineno-match: + :start-at: // Open loop control + :end-at: joint_target[5] += increment + +To send the control command, the robot's :ref:`reverse_interface` is used via the +``writeJointCommand()`` function: + +.. literalinclude:: ../../examples/full_driver.cpp + :language: c++ + :caption: examples/full_driver.cpp + :linenos: + :lineno-match: + :start-at: // Setting the RobotReceiveTimeout + :end-before: URCL_LOG_DEBUG("data_pkg:\n%s", data_pkg->toString().c_str()); diff --git a/doc/images/initial_setup_images/cb3_01_welcome.png b/doc/images/initial_setup_images/cb3_01_welcome.png new file mode 100644 index 00000000..1e0af1e9 Binary files /dev/null and b/doc/images/initial_setup_images/cb3_01_welcome.png differ diff --git a/doc/images/initial_setup_images/cb3_05_urcaps_installed.png b/doc/images/initial_setup_images/cb3_05_urcaps_installed.png new file mode 100644 index 00000000..0f111cb0 Binary files /dev/null and b/doc/images/initial_setup_images/cb3_05_urcaps_installed.png differ diff --git a/doc/images/initial_setup_images/cb3_07_installation_excontrol.png b/doc/images/initial_setup_images/cb3_07_installation_excontrol.png new file mode 100644 index 00000000..133e9b6c Binary files /dev/null and b/doc/images/initial_setup_images/cb3_07_installation_excontrol.png differ diff --git a/doc/images/initial_setup_images/cb3_10_prog_structure_urcaps.png b/doc/images/initial_setup_images/cb3_10_prog_structure_urcaps.png new file mode 100644 index 00000000..9a001031 Binary files /dev/null and b/doc/images/initial_setup_images/cb3_10_prog_structure_urcaps.png differ diff --git a/doc/images/initial_setup_images/cb3_11_program_view_excontrol.png b/doc/images/initial_setup_images/cb3_11_program_view_excontrol.png new file mode 100644 index 00000000..dd1ba4a9 Binary files /dev/null and b/doc/images/initial_setup_images/cb3_11_program_view_excontrol.png differ diff --git a/doc/images/initial_setup_images/es_01_welcome.png b/doc/images/initial_setup_images/es_01_welcome.png new file mode 100644 index 00000000..fba14a58 Binary files /dev/null and b/doc/images/initial_setup_images/es_01_welcome.png differ diff --git a/doc/images/initial_setup_images/es_05_urcaps_installed.png b/doc/images/initial_setup_images/es_05_urcaps_installed.png new file mode 100644 index 00000000..8a02bc54 Binary files /dev/null and b/doc/images/initial_setup_images/es_05_urcaps_installed.png differ diff --git a/doc/images/initial_setup_images/es_07_installation_excontrol.png b/doc/images/initial_setup_images/es_07_installation_excontrol.png new file mode 100644 index 00000000..3b1a9361 Binary files /dev/null and b/doc/images/initial_setup_images/es_07_installation_excontrol.png differ diff --git a/doc/images/initial_setup_images/es_10_prog_structure_urcaps.png b/doc/images/initial_setup_images/es_10_prog_structure_urcaps.png new file mode 100644 index 00000000..6d1e2fff Binary files /dev/null and b/doc/images/initial_setup_images/es_10_prog_structure_urcaps.png differ diff --git a/doc/images/initial_setup_images/es_11_program_view_excontrol.png b/doc/images/initial_setup_images/es_11_program_view_excontrol.png new file mode 100644 index 00000000..9b2c50d4 Binary files /dev/null and b/doc/images/initial_setup_images/es_11_program_view_excontrol.png differ diff --git a/doc/images/initial_setup_images/family_photo.png b/doc/images/initial_setup_images/family_photo.png new file mode 100644 index 00000000..678d312d Binary files /dev/null and b/doc/images/initial_setup_images/family_photo.png differ diff --git a/doc/images/initial_setup_images/remote_control.png b/doc/images/initial_setup_images/remote_control.png new file mode 100644 index 00000000..0f104b6f Binary files /dev/null and b/doc/images/initial_setup_images/remote_control.png differ diff --git a/doc/images/initial_setup_images/services_polyscope5.png b/doc/images/initial_setup_images/services_polyscope5.png new file mode 100644 index 00000000..a32720ff Binary files /dev/null and b/doc/images/initial_setup_images/services_polyscope5.png differ diff --git a/doc/images/initial_setup_images/services_polyscopex.png b/doc/images/initial_setup_images/services_polyscopex.png new file mode 100644 index 00000000..d70140ba Binary files /dev/null and b/doc/images/initial_setup_images/services_polyscopex.png differ diff --git a/doc/index.rst b/doc/index.rst index be3ca1f6..0aa88d58 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -10,7 +10,8 @@ can be found on `GitHub Network) and enter these settings: + +.. code-block:: + + IP address: 192.168.56.101 + Subnet mask: 255.255.255.0 + Default gateway: 192.168.56.1 + Preferred DNS server: 192.168.56.1 + Alternative DNS server: 0.0.0.0 + +Remote PC setup +--------------- + +#. On the remote PC, turn off all network devices except the "wired connection", e.g. turn off + wifi. You can skip this step if you are certain that no other active connection is using the + same IP range. + +#. Open Network Settings and create a new Wired connection with these settings. You may want to name this new connection ``UR`` or something similar: + + .. code-block:: + + IPv4 + Manual + Address: 192.168.56.1 + Netmask: 255.255.255.0 + +#. Verify the connection from the PC with e.g. ping. You should see something like this: + + .. code-block:: console + + $ ping 192.168.56.101 + PING 192.168.56.101 (192.168.56.101) 56(84) bytes of data. + 64 bytes from 192.168.56.101: icmp_seq=1 ttl=64 time=0.037 ms + 64 bytes from 192.168.56.101: icmp_seq=2 ttl=64 time=0.043 ms + 64 bytes from 192.168.56.101: icmp_seq=3 ttl=64 time=0.047 ms diff --git a/doc/setup/robot_setup.rst b/doc/setup/robot_setup.rst new file mode 100644 index 00000000..95462e29 --- /dev/null +++ b/doc/setup/robot_setup.rst @@ -0,0 +1,213 @@ +.. _robot_setup: + +Robot setup +=========== + +The robot needs to be prepared before it can be used with the client library. + +.. tabs:: + + .. group-tab:: CB3 + + CB3 robots can directly be used with the ur_client_library. No special preparation is needed. + + + .. group-tab:: PolyScope 5 + + There are two ways the client library can be enabled to send command to the robot. + + - **Remote control**: There the full control is given to the client library and enables it to e.g. power on and off, brake release, load PolyScope programs and send URScript programs directly to the controller. + - **External Control URCap**: There the control to power on, off and start programs etc. still remains on the teach pendant. The External Control URCap injects the needed URScript code from the client library. This also makes it possible to combine the use of the client library and other PolyScope program nodes, like standard moves or other third-party URCaps. + + **Enable remote control:** + + #. Go to the hamburger menu -> settings. + #. Go to System -> Remote control. + #. Unlock the menu using the admin password, and enable Remote Control. + #. Press exit. + #. The robot can now be toggled between local and remote control in the upper right hand corner. + + .. image:: ../images/initial_setup_images/remote_control.png + :width: 600 + :alt: Screenshot showing remote control toggle. + + + **If using PolyScope 5.10 or greater: Enable services** + + #. Go to the hamburger menu -> settings. + #. Go to Security -> Services. + #. Unlock the menu using the admin password. + #. Enable the Dashboard Server, Primary Client Interface, Secondary Client Interface and Real-Time Data Exchange (RTDE) interfaces. + #. Lock the menu and press exit. + + .. image:: ../images/initial_setup_images/services_polyscope5.png + :width: 600 + :alt: Screenshot from PolyScope 5.xx services menu. + + + .. group-tab:: PolyScope X + + **Enable services:** + + #. Go to the hamburger menu -> settings. + #. Go to Security -> Services. + #. Unlock the menu using the admin password. + #. Enable the Primary Client Interface, Secondary Client Interface and Real-Time Data Exchange (RTDE) interfaces. + #. Lock the menu and press exit. + + .. image:: ../images/initial_setup_images/services_polyscopex.png + :width: 600 + :alt: Screenshot from PolyScope X screen. + +.. _install_urcap: + +URCap installation +------------------ + +To use the client library with a robot, you'll have to have the **External Control URCap** +installed. It allows a remote PC to control the robot externally. Generally, you will launch the +driver on the remote PC and then start a program from the tech pendant to connect to the remote +application. + +.. tabs:: + + .. group-tab:: CB3 + + .. note:: + + A minimal PolyScope version of 3.14.3 is required to use this URCap + + The latest release can be downloaded from `its own repository `_. + + To install the URCap you first have to copy it to the robot's **programs** folder which can be done either + via scp or using a USB stick. + + On the welcome screen select *Setup Robot* and then *URCaps* to enter the URCaps installation + screen. + + + .. image:: ../images/initial_setup_images/cb3_01_welcome.png + :target: initial_setup_images/cb3_01_welcome.png + :alt: Welcome screen of a CB3 robot + + + There, click the little plus sign at the bottom to open the file selector. There you should see + all urcap files stored inside the robot's programs folder or a plugged USB drive. Select and open + the **externalcontrol-X.Y.Z.urcap** file and click *open*. Your URCaps view should now show the + **External Control** in the list of active URCaps and a notification to restart the robot. Do that + now. + + + .. image:: ../images/initial_setup_images/cb3_05_urcaps_installed.png + :target: initial_setup_images/cb3_05_urcaps_installed.png + :alt: URCaps screen with installed urcaps + + + After the reboot you should find the **External Control** URCaps inside the *Installation* section. + For this select *Program Robot* on the welcome screen, select the *Installation* tab and select + **External Control** from the list. + + + .. image:: ../images/initial_setup_images/cb3_07_installation_excontrol.png + :target: initial_setup_images/cb3_07_installation_excontrol.png + :alt: Installation screen of URCaps + + + Here you'll have to setup the IP address of the external PC which will be running the remote + application. + Note that the robot and the external PC have to be in the same network, ideally in a direct + connection with each other to minimize network disturbances. The custom port should be left + untouched for now. + + + .. image:: ../images/initial_setup_images/cb3_10_prog_structure_urcaps.png + :target: initial_setup_images/cb3_10_prog_structure_urcaps.png + :alt: Insert the external control node + + + To use the new URCaps, create a new program and insert the **External Control** program node into + the program tree + + + .. image:: ../images/initial_setup_images/cb3_11_program_view_excontrol.png + :target: initial_setup_images/cb3_11_program_view_excontrol.png + :alt: Program view of external control + + + If you click on the *command* tab again, you'll see the settings entered inside the *Installation*. + Check that they are correct, then save the program. Your robot is now ready to be used together with + this driver + + .. group-tab:: PolyScope 5 + + .. note:: + + A minimal PolyScope version of 5.9.4 is required to use this URCap + + The latest release can be downloaded from `its own repository `_. + + To install it you first have to copy it to the robot's **programs** folder which can be done either + via scp or using a USB stick. + + On the welcome screen click on the hamburger menu in the top-right corner and select *Settings* to enter the robot's setup. There select *System* and then *URCaps* to enter the URCaps installation screen. + + + .. image:: ../images/initial_setup_images/es_01_welcome.png + :target: initial_setup_images/es_01_welcome.png + :alt: Welcome screen of an e-Series robot + + + There, click the little plus sign at the bottom to open the file selector. There you should see + all urcap files stored inside the robot's programs folder or a plugged USB drive. Select and open + the **externalcontrol-X.Y.Z.urcap** file and click *open*. Your URCaps view should now show the + **External Control** in the list of active URCaps and a notification to restart the robot. Do that + now. + + + .. image:: ../images/initial_setup_images/es_05_urcaps_installed.png + :target: initial_setup_images/es_05_urcaps_installed.png + :alt: URCaps screen with installed urcaps + + + After the reboot you should find the **External Control** URCaps inside the *Installation* section. + For this select *Program Robot* on the welcome screen, select the *Installation* tab and select + **External Control** from the list. + + + .. image:: ../images/initial_setup_images/es_07_installation_excontrol.png + :target: initial_setup_images/es_07_installation_excontrol.png + :alt: Installation screen of URCaps + + + Here you'll have to setup the IP address of the external PC which will be running the remote + application. Note that the robot and the external PC have to be in the same network, ideally in a + direct connection with each other to minimize network disturbances. The custom port should be left + untouched for now. + + + .. image:: ../images/initial_setup_images/es_10_prog_structure_urcaps.png + :target: initial_setup_images/es_10_prog_structure_urcaps.png + :alt: Insert the external control node + + + To use the new URCaps, create a new program and insert the **External Control** program node into + the program tree + + + .. image:: ../images/initial_setup_images/es_11_program_view_excontrol.png + :target: initial_setup_images/es_11_program_view_excontrol.png + :alt: Program view of external control + + + If you click on the *command* tab again, you'll see the settings entered inside the *Installation*. + Check that they are correct, then save the program. Your robot is now ready to be used together with + this driver. + + .. group-tab:: PolyScope X + + .. warning:: + + Support for PolyScope X isn't fully developed, yet. Please consider using External Control + with PolyScope X as an open beta. + + For details on installing the External Control URCapX, please see https://github.com/UniversalRobots/Universal_Robots_ExternalControl_URCapX diff --git a/doc/setup/ursim_docker.rst b/doc/setup/ursim_docker.rst new file mode 100644 index 00000000..45d1bfc5 --- /dev/null +++ b/doc/setup/ursim_docker.rst @@ -0,0 +1,127 @@ +.. _ursim_docker: + +Setup URSim with Docker +======================= +URSim is the offline simulator by Universal Robots. Packed into a remote or virtual machine it acts almost +identically to a real robot connected over the network. While it is possible to get URSim running +locally on a Linux system or inside a VirtualBox virtual machine, we will focus on getting things +setup using Docker. Using Docker for your simulated robot allows you to very quickly spin up a robot +testing instance with very little computational overload. + +This guide will assume that you have Docker already installed and setup such that you can startup +Docker containers using your current user. + +The following example will start up a simulator on a UR5e. See the Docker image `documentation `_ for more details and configuration options. + +Start a URSim docker container +------------------------------ + +To startup a simulated robot run the following command. This will start a Docker container named +``ursim`` and startup a simulated UR5e robot. It exposes ports 5900 and 6080 for the browser-based +polyscope access. Note that this will expose the simulated robot to your local area network if you +don't have any further level of security such as a firewall active. To prevent this, you can either +skip the port forwarding instructions (skip the two ``-p port:port`` statements) in which case +you'll have to use the container's IP address to access the polyscope gui rather than ``localhost`` or +you can restrict the port forwarding to a certain network interface (such as the looppack interface) +see Docker's upstream documentation on port exposure for further information. + +.. code-block:: bash + + docker run --rm -it -p 5900:5900 -p 6080:6080 --name ursim universalrobots/ursim_e-series + +External Control +---------------- + +To use the external control functionality, we will need the ``external_control`` URCap installed on +the robot and a program containing its *ExternalControl* program node. Both can be prepared on the +host machine either by creating an own Dockerfile containing those or by mounting two folders +containing installed URCaps and programs. See the Docker image `documentation `_. + +In this example, we will bind-mount a folder for the programs and URCaps. First, let's create a +local folder where we can store things inside: + +.. code-block:: bash + + mkdir -p ${HOME}/.ursim/programs + mkdir -p ${HOME}/.ursim/urcaps + +Then, we can "install" the URCap by placing its ``.jar`` file inside the urcaps folder + +.. code-block:: bash + + URCAP_VERSION=1.0.5 # latest version as if writing this + curl -L -o ${HOME}/.ursim/urcaps/externalcontrol-${URCAP_VERSION}.jar \ + https://github.com/UniversalRobots/Universal_Robots_ExternalControl_URCap/releases/download/v${URCAP_VERSION}/externalcontrol-${URCAP_VERSION}.jar + +With this, start your URSim containers with the following command: + +.. code-block:: bash + + docker run --rm -it -p 5900:5900 -p 6080:6080 -v ${HOME}/.ursim/urcaps:/urcaps -v ${HOME}/.ursim/programs:/ursim/programs --name ursim universalrobots/ursim_e-series + +With this, you should be able to setup the ``external_control`` URCap and create a program as +described in :ref:`URCap setup guide `. + +Network setup +------------- + +As described above, you can always start the URSim container using the default network setup. It will most probably +always get the same IP address assigned every time, as long as you don't have any other docker containers running. +However, to make things a bit more explicit, we can setup our own docker network where we can assign a static IP +address to our URSim container. + +.. code-block:: bash + + docker network create --subnet=192.168.56.0/24 ursim_net + docker run --rm -it -p 5900:5900 -p 6080:6080 --net ursim_net --ip 192.168.56.101 universalrobots/ursim_e-series + +The above commands first create a network for docker and then create a container with the URSim +image attaching to this network. + +We can skip the port exposure, as we use a fixed IP address for the simulated robot. Therefore, the VNC web server will be available at ``_. + +Script startup +-------------- + +All of the above is put together in a script in the ``ur_client_library`` package. + +.. tabs:: + + + .. tab:: ROS 1 + + .. code-block:: bash + + rosrun ur_client_library start_ursim.sh + + This will start a URSim docker container running on ``192.168.56.101`` with the ``external_control`` + URCap preinstalled. Created programs and installation changes will be stored persistently inside + ``${HOME}/.ursim/programs``. + + With this, you can run + + .. code-block:: bash + + roslaunch ur_robot_driver ur5e_bringup.launch robot_ip:=192.168.56.101 + + .. tab:: ROS 2 + + .. code-block:: bash + + ros2 run ur_client_library start_ursim.sh + + This will start a URSim docker container running on ``192.168.56.101`` with the ``external_control`` + URCap preinstalled. Created programs and installation changes will be stored persistently inside + ``${HOME}/.ursim/programs``. + + With this, you can run + + .. code-block:: bash + + ros2 launch ur_robot_driver ur_control.launch.py ur_type:=ur5e robot_ip:=192.168.56.101 + + .. tab:: Other + + If you have installed the client library from another source than ROS / ROS 2 or have + compiled it yourself, run the ``start_ursim.sh`` script directly from the package's + ``scripts`` folder. diff --git a/doc_requirements.txt b/doc_requirements.txt index 0033332f..325ab111 100644 --- a/doc_requirements.txt +++ b/doc_requirements.txt @@ -1,4 +1,5 @@ sphinx -sphinx_rtd_theme sphinx-copybutton +sphinx-tabs +sphinx_rtd_theme catkin-pkg diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 79fc67fa..48452197 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -47,3 +47,7 @@ target_link_libraries(script_sender_example ur_client_library::urcl) add_executable(trajectory_point_interface_example trajectory_point_interface.cpp) target_link_libraries(trajectory_point_interface_example ur_client_library::urcl) + +add_executable(instruction_executor +instruction_executor.cpp) +target_link_libraries(instruction_executor ur_client_library::urcl) diff --git a/examples/dashboard_example.cpp b/examples/dashboard_example.cpp index a5a85cf9..1e7232cf 100644 --- a/examples/dashboard_example.cpp +++ b/examples/dashboard_example.cpp @@ -32,9 +32,7 @@ #include #include -#include -#include #include #include @@ -71,6 +69,11 @@ int main(int argc, char* argv[]) return 1; } + // Get the PolyScope version + std::string version; + my_dashboard->commandPolyscopeVersion(version); + URCL_LOG_INFO(version.c_str()); + my_dashboard->commandCloseSafetyPopup(); // Power it on @@ -95,11 +98,7 @@ int main(int argc, char* argv[]) return 1; } -#ifdef _WIN32 - ::Sleep(1000); -#else // _WIN32 - sleep(1); -#endif // _WIN32 + std::this_thread::sleep_for(std::chrono::seconds(1)); // Play loaded program if (!my_dashboard->commandPlay()) @@ -143,7 +142,13 @@ int main(int argc, char* argv[]) return 1; } - // Now the robot is ready to receive a program + // Make a raw request and save the response + std::string program_state = my_dashboard->sendAndReceive("programState"); + URCL_LOG_INFO("Program state: %s", program_state.c_str()); + + // The response can be checked with a regular expression + bool success = my_dashboard->sendRequest("power off", "Powering off"); + URCL_LOG_INFO("Power off command success: %d", success); return 0; } diff --git a/examples/force_mode_example.cpp b/examples/force_mode_example.cpp index 02bc9070..47d70f0e 100644 --- a/examples/force_mode_example.cpp +++ b/examples/force_mode_example.cpp @@ -33,6 +33,7 @@ // In a real-world example it would be better to get those values from command line parameters / a // better configuration system such as Boost.Program_options +#include #include #include #include @@ -52,29 +53,11 @@ const std::string OUTPUT_RECIPE = "examples/resources/rtde_output_recipe.txt"; const std::string INPUT_RECIPE = "examples/resources/rtde_input_recipe.txt"; const std::string CALIBRATION_CHECKSUM = "calib_12788084448423163542"; -std::unique_ptr g_my_driver; -std::unique_ptr g_my_dashboard; - -std::condition_variable g_program_running_cv; -std::mutex g_program_running_mutex; -bool g_program_running; - -// We need a callback function to register. See UrDriver's parameters for details. -void handleRobotProgramState(bool program_running) -{ - // Print the text in green so we see it better - std::cout << "\033[1;32mProgram running: " << std::boolalpha << program_running << "\033[0m\n" << std::endl; - if (program_running) - { - std::lock_guard lk(g_program_running_mutex); - g_program_running = program_running; - g_program_running_cv.notify_one(); - } -} +std::unique_ptr g_my_robot; void sendFreedriveMessageOrDie(const control::FreedriveControlMessage freedrive_action) { - bool ret = g_my_driver->writeFreedriveControlMessage(freedrive_action); + bool ret = g_my_robot->ur_driver_->writeFreedriveControlMessage(freedrive_action); if (!ret) { URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?"); @@ -82,17 +65,6 @@ void sendFreedriveMessageOrDie(const control::FreedriveControlMessage freedrive_ } } -bool waitForProgramRunning(int milliseconds = 100) -{ - std::unique_lock lk(g_program_running_mutex); - if (g_program_running_cv.wait_for(lk, std::chrono::milliseconds(milliseconds)) == std::cv_status::no_timeout || - g_program_running == true) - { - return true; - } - return false; -} - int main(int argc, char* argv[]) { urcl::setLogLevel(urcl::LogLevel::INFO); @@ -110,50 +82,16 @@ int main(int argc, char* argv[]) second_to_run = std::chrono::seconds(std::stoi(argv[2])); } - // Making the robot ready for the program by: - // Connect the robot Dashboard - g_my_dashboard.reset(new DashboardClient(robot_ip)); - if (!g_my_dashboard->connect()) - { - URCL_LOG_ERROR("Could not connect to dashboard"); - return 1; - } + bool headless_mode = true; + g_my_robot = std::make_unique(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, headless_mode, + "external_control.urp"); - // // Stop program, if there is one running - if (!g_my_dashboard->commandStop()) + if (!g_my_robot->isHealthy()) { - URCL_LOG_ERROR("Could not send stop program command"); + URCL_LOG_ERROR("Something in the robot initialization went wrong. Exiting. Please check the output above."); return 1; } - - // Power it off - // if (!g_my_dashboard->commandPowerOff()) - //{ - // URCL_LOG_ERROR("Could not send Power off command"); - // return 1; - //} - - // Power it on - // if (!g_my_dashboard->commandPowerOn()) - //{ - // URCL_LOG_ERROR("Could not send Power on command"); - // return 1; - //} - - // Release the brakes - // if (!g_my_dashboard->commandBrakeRelease()) - //{ - // URCL_LOG_ERROR("Could not send BrakeRelease command"); - // return 1; - //} - - // Now the robot is ready to receive a program - std::unique_ptr tool_comm_setup; - const bool headless = true; - g_my_driver.reset(new UrDriver(robot_ip, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, &handleRobotProgramState, headless, - std::move(tool_comm_setup))); - - if (!g_my_driver->checkCalibration(CALIBRATION_CHECKSUM)) + if (!g_my_robot->ur_driver_->checkCalibration(CALIBRATION_CHECKSUM)) { URCL_LOG_ERROR("Calibration checksum does not match actual robot."); URCL_LOG_ERROR("Use the ur_calibration tool to extract the correct calibration from the robot and pass that into " @@ -162,40 +100,31 @@ int main(int argc, char* argv[]) "for details."); } - // Once RTDE communication is started, we have to make sure to read from the interface buffer, as - // otherwise we will get pipeline overflows. Therefore, do this directly before starting your main - // loop. - g_my_driver->startRTDECommunication(); + // End of initialization -- We've started the external control program, which means we have to + // write keepalive signals from now on. Otherwise the connection will be dropped. - std::chrono::duration time_done(0); - std::chrono::duration timeout(second_to_run); - auto stopwatch_last = std::chrono::steady_clock::now(); - auto stopwatch_now = stopwatch_last; - // Make sure that external control script is running - if (!waitForProgramRunning()) - { - URCL_LOG_ERROR("External Control script not running."); - return 1; - } + // Start force mode // Task frame at the robot's base with limits being large enough to cover the whole workspace // Compliance in z axis and rotation around z axis bool success; - if (g_my_driver->getVersion().major < 5) - success = g_my_driver->startForceMode({ 0, 0, 0, 0, 0, 0 }, // Task frame at the robot's base - { 0, 0, 1, 0, 0, 1 }, // Compliance in z axis and rotation around z axis - { 0, 0, 0, 0, 0, 0 }, // do not apply any active wrench - 2, // do not transform the force frame at all - { 0.1, 0.1, 1.5, 3.14, 3.14, 0.5 }, // limits - 0.025); // damping_factor. See ScriptManual for details. + if (g_my_robot->ur_driver_->getVersion().major < 5) + success = + g_my_robot->ur_driver_->startForceMode({ 0, 0, 0, 0, 0, 0 }, // Task frame at the robot's base + { 0, 0, 1, 0, 0, 1 }, // Compliance in z axis and rotation around z axis + { 0, 0, -2, 0, 0, 0 }, // Press in -z direction + 2, // do not transform the force frame at all + { 0.1, 0.1, 1.5, 3.14, 3.14, 0.5 }, // limits + 0.005); // damping_factor. See ScriptManual for details. else { - success = g_my_driver->startForceMode({ 0, 0, 0, 0, 0, 0 }, // Task frame at the robot's base - { 0, 0, 1, 0, 0, 1 }, // Compliance in z axis and rotation around z axis - { 0, 0, 0, 0, 0, 0 }, // do not apply any active wrench - 2, // do not transform the force frame at all - { 0.1, 0.1, 1.5, 3.14, 3.14, 0.5 }, // limits - 0.025, // damping_factor - 0.8); // gain_scaling. See ScriptManual for details. + success = + g_my_robot->ur_driver_->startForceMode({ 0, 0, 0, 0, 0, 0 }, // Task frame at the robot's base + { 0, 0, 1, 0, 0, 1 }, // Compliance in z axis and rotation around z axis + { 0, 0, -2, 0, 0, 0 }, // Press in -z direction + 2, // do not transform the force frame at all + { 0.1, 0.1, 1.5, 3.14, 3.14, 0.5 }, // limits + 0.005, // damping_factor + 1.0); // gain_scaling. See ScriptManual for details. } if (!success) { @@ -203,31 +132,19 @@ int main(int argc, char* argv[]) return 1; } - while (true) + std::chrono::duration time_done(0); + std::chrono::duration timeout(second_to_run); + auto stopwatch_last = std::chrono::steady_clock::now(); + auto stopwatch_now = stopwatch_last; + while (time_done < timeout || second_to_run.count() == 0) { - // Read latest RTDE package. This will block for a hard-coded timeout (see UrDriver), so the - // robot will effectively be in charge of setting the frequency of this loop. - // In a real-world application this thread should be scheduled with real-time priority in order - // to ensure that this is called in time. - std::unique_ptr data_pkg = g_my_driver->getDataPackage(); - if (data_pkg) - { - g_my_driver->writeKeepalive(); - - if (time_done > timeout && second_to_run.count() != 0) - { - URCL_LOG_INFO("Timeout reached."); - break; - } - } - else - { - URCL_LOG_WARN("Could not get fresh data package from robot"); - } + g_my_robot->ur_driver_->writeKeepalive(); stopwatch_now = std::chrono::steady_clock::now(); time_done += stopwatch_now - stopwatch_last; stopwatch_last = stopwatch_now; + std::this_thread::sleep_for(std::chrono::milliseconds(2)); } - g_my_driver->endForceMode(); + URCL_LOG_INFO("Timeout reached."); + g_my_robot->ur_driver_->endForceMode(); } diff --git a/examples/freedrive_example.cpp b/examples/freedrive_example.cpp index 53466cd4..4d691d0d 100644 --- a/examples/freedrive_example.cpp +++ b/examples/freedrive_example.cpp @@ -36,6 +36,7 @@ #include #include #include +#include #include #include @@ -47,27 +48,17 @@ using namespace urcl; const std::string DEFAULT_ROBOT_IP = "192.168.56.101"; -const std::string SCRIPT_FILE = "resources/external_control.urscript"; const std::string OUTPUT_RECIPE = "examples/resources/rtde_output_recipe.txt"; const std::string INPUT_RECIPE = "examples/resources/rtde_input_recipe.txt"; -const std::string CALIBRATION_CHECKSUM = "calib_12788084448423163542"; -std::unique_ptr g_my_driver; -std::unique_ptr g_my_dashboard; - -// We need a callback function to register. See UrDriver's parameters for details. -void handleRobotProgramState(bool program_running) -{ - // Print the text in green so we see it better - std::cout << "\033[1;32mProgram running: " << std::boolalpha << program_running << "\033[0m\n" << std::endl; -} +std::unique_ptr g_my_robot; void sendFreedriveMessageOrDie(const control::FreedriveControlMessage freedrive_action) { - bool ret = g_my_driver->writeFreedriveControlMessage(freedrive_action); + bool ret = g_my_robot->ur_driver_->writeFreedriveControlMessage(freedrive_action); if (!ret) { - URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?"); + URCL_LOG_ERROR("Could not send joint command. Is there an external_control program running on the robot?"); exit(1); } } @@ -89,86 +80,35 @@ int main(int argc, char* argv[]) second_to_run = std::chrono::seconds(std::stoi(argv[2])); } - // Making the robot ready for the program by: - // Connect the robot Dashboard - g_my_dashboard.reset(new DashboardClient(robot_ip)); - if (!g_my_dashboard->connect()) - { - URCL_LOG_ERROR("Could not connect to dashboard"); - return 1; - } + bool headless_mode = true; - // // Stop program, if there is one running - if (!g_my_dashboard->commandStop()) - { - URCL_LOG_ERROR("Could not send stop program command"); - return 1; - } + g_my_robot = std::make_unique(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, headless_mode, + "external_control.urp"); - // Power it off - if (!g_my_dashboard->commandPowerOff()) + if (!g_my_robot->isHealthy()) { - URCL_LOG_ERROR("Could not send Power off command"); + URCL_LOG_ERROR("Something in the robot initialization went wrong. Exiting. Please check the output above."); return 1; } - // Power it on - if (!g_my_dashboard->commandPowerOn()) - { - URCL_LOG_ERROR("Could not send Power on command"); - return 1; - } - - // Release the brakes - if (!g_my_dashboard->commandBrakeRelease()) - { - URCL_LOG_ERROR("Could not send BrakeRelease command"); - return 1; - } - - // Now the robot is ready to receive a program - std::unique_ptr tool_comm_setup; - const bool headless = true; - g_my_driver.reset(new UrDriver(robot_ip, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, &handleRobotProgramState, headless, - std::move(tool_comm_setup), CALIBRATION_CHECKSUM)); - - // Once RTDE communication is started, we have to make sure to read from the interface buffer, as - // otherwise we will get pipeline overflows. Therefore, do this directly before starting your main - // loop. - g_my_driver->startRTDECommunication(); + URCL_LOG_INFO("Starting freedrive mode"); + sendFreedriveMessageOrDie(control::FreedriveControlMessage::FREEDRIVE_START); std::chrono::duration time_done(0); std::chrono::duration timeout(second_to_run); auto stopwatch_last = std::chrono::steady_clock::now(); auto stopwatch_now = stopwatch_last; - g_my_driver->writeKeepalive(); - sendFreedriveMessageOrDie(control::FreedriveControlMessage::FREEDRIVE_START); - while (true) + while (time_done < timeout || second_to_run.count() == 0) { - // Read latest RTDE package. This will block for a hard-coded timeout (see UrDriver), so the - // robot will effectively be in charge of setting the frequency of this loop. - // In a real-world application this thread should be scheduled with real-time priority in order - // to ensure that this is called in time. - std::unique_ptr data_pkg = g_my_driver->getDataPackage(); - if (data_pkg) - { - sendFreedriveMessageOrDie(control::FreedriveControlMessage::FREEDRIVE_NOOP); - - if (time_done > timeout && second_to_run.count() != 0) - { - URCL_LOG_INFO("Timeout reached."); - break; - } - } - else - { - URCL_LOG_WARN("Could not get fresh data package from robot"); - } + sendFreedriveMessageOrDie(control::FreedriveControlMessage::FREEDRIVE_NOOP); stopwatch_now = std::chrono::steady_clock::now(); time_done += stopwatch_now - stopwatch_last; stopwatch_last = stopwatch_now; + std::this_thread::sleep_for(std::chrono::milliseconds(2)); } + + URCL_LOG_INFO("Stopping freedrive mode"); sendFreedriveMessageOrDie(control::FreedriveControlMessage::FREEDRIVE_STOP); } diff --git a/examples/full_driver.cpp b/examples/full_driver.cpp index e2cbe6d8..900301c8 100644 --- a/examples/full_driver.cpp +++ b/examples/full_driver.cpp @@ -28,6 +28,7 @@ */ //---------------------------------------------------------------------- +#include #include #include #include @@ -43,19 +44,10 @@ const std::string DEFAULT_ROBOT_IP = "192.168.56.101"; const std::string SCRIPT_FILE = "resources/external_control.urscript"; const std::string OUTPUT_RECIPE = "examples/resources/rtde_output_recipe.txt"; const std::string INPUT_RECIPE = "examples/resources/rtde_input_recipe.txt"; -const std::string CALIBRATION_CHECKSUM = "calib_12788084448423163542"; -std::unique_ptr g_my_driver; -std::unique_ptr g_my_dashboard; +std::unique_ptr g_my_robot; vector6d_t g_joint_positions; -// We need a callback function to register. See UrDriver's parameters for details. -void handleRobotProgramState(bool program_running) -{ - // Print the text in green so we see it better - std::cout << "\033[1;32mProgram running: " << std::boolalpha << program_running << "\033[0m\n" << std::endl; -} - int main(int argc, char* argv[]) { urcl::setLogLevel(urcl::LogLevel::INFO); @@ -67,59 +59,19 @@ int main(int argc, char* argv[]) robot_ip = std::string(argv[1]); } - // Making the robot ready for the program by: - // Connect the the robot Dashboard - g_my_dashboard.reset(new DashboardClient(robot_ip)); - if (!g_my_dashboard->connect()) - { - URCL_LOG_ERROR("Could not connect to dashboard"); - return 1; - } - - // Stop program, if there is one running - if (!g_my_dashboard->commandStop()) - { - URCL_LOG_ERROR("Could not send stop program command"); - return 1; - } - - // Power it off - if (!g_my_dashboard->commandPowerOff()) + bool headless_mode = true; + g_my_robot = std::make_unique(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, headless_mode, + "external_control.urp"); + if (!g_my_robot->isHealthy()) { - URCL_LOG_ERROR("Could not send Power off command"); + URCL_LOG_ERROR("Something in the robot initialization went wrong. Exiting. Please check the output above."); return 1; } - - // Power it on - if (!g_my_dashboard->commandPowerOn()) - { - URCL_LOG_ERROR("Could not send Power on command"); - return 1; - } - - // Release the brakes - if (!g_my_dashboard->commandBrakeRelease()) - { - URCL_LOG_ERROR("Could not send BrakeRelease command"); - return 1; - } - - // Now the robot is ready to receive a program - - std::unique_ptr tool_comm_setup; - const bool headless = true; - g_my_driver.reset(new UrDriver(robot_ip, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, &handleRobotProgramState, headless, - std::move(tool_comm_setup), CALIBRATION_CHECKSUM)); - - // Once RTDE communication is started, we have to make sure to read from the interface buffer, as - // otherwise we will get pipeline overflows. Therefor, do this directly before starting your main - // loop. - - g_my_driver->startRTDECommunication(); + // --------------- INITIALIZATION END ------------------- // Increment depends on robot version double increment_constant = 0.0005; - if (g_my_driver->getVersion().major < 5) + if (g_my_robot->ur_driver_->getVersion().major < 5) { increment_constant = 0.002; } @@ -130,64 +82,67 @@ int main(int argc, char* argv[]) bool passed_positive_part = false; URCL_LOG_INFO("Start moving the robot"); urcl::vector6d_t joint_target = { 0, 0, 0, 0, 0, 0 }; + + // Once RTDE communication is started, we have to make sure to read from the interface buffer, as + // otherwise we will get pipeline overflows. Therefor, do this directly before starting your main + // loop. + g_my_robot->ur_driver_->startRTDECommunication(); while (!(passed_positive_part && passed_negative_part)) { // Read latest RTDE package. This will block for a hard-coded timeout (see UrDriver), so the // robot will effectively be in charge of setting the frequency of this loop. // In a real-world application this thread should be scheduled with real-time priority in order // to ensure that this is called in time. - std::unique_ptr data_pkg = g_my_driver->getDataPackage(); - if (data_pkg) + std::unique_ptr data_pkg = g_my_robot->ur_driver_->getDataPackage(); + if (!data_pkg) { - // Read current joint positions from robot data - if (!data_pkg->getData("actual_q", g_joint_positions)) - { - // This throwing should never happen unless misconfigured - std::string error_msg = "Did not find 'actual_q' in data sent from robot. This should not happen!"; - throw std::runtime_error(error_msg); - } + URCL_LOG_WARN("Could not get fresh data package from robot"); + return 1; + } + // Read current joint positions from robot data + if (!data_pkg->getData("actual_q", g_joint_positions)) + { + // This throwing should never happen unless misconfigured + std::string error_msg = "Did not find 'actual_q' in data sent from robot. This should not happen!"; + throw std::runtime_error(error_msg); + } - if (first_pass) - { - joint_target = g_joint_positions; - first_pass = false; - } + if (first_pass) + { + joint_target = g_joint_positions; + first_pass = false; + } - // Open loop control. The target is incremented with a constant each control loop - if (passed_positive_part == false) - { - increment = increment_constant; - if (g_joint_positions[5] >= 2) - { - passed_positive_part = true; - } - } - else if (passed_negative_part == false) + // Open loop control. The target is incremented with a constant each control loop + if (passed_positive_part == false) + { + increment = increment_constant; + if (g_joint_positions[5] >= 2) { - increment = -increment_constant; - if (g_joint_positions[5] <= 0) - { - passed_negative_part = true; - } + passed_positive_part = true; } - joint_target[5] += increment; - // Setting the RobotReceiveTimeout time is for example purposes only. This will make the example running more - // reliable on non-realtime systems. Use with caution in productive applications. - bool ret = g_my_driver->writeJointCommand(joint_target, comm::ControlMode::MODE_SERVOJ, - RobotReceiveTimeout::millisec(100)); - if (!ret) + } + else if (passed_negative_part == false) + { + increment = -increment_constant; + if (g_joint_positions[5] <= 0) { - URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?"); - return 1; + passed_negative_part = true; } - URCL_LOG_DEBUG("data_pkg:\n%s", data_pkg->toString().c_str()); } - else + joint_target[5] += increment; + // Setting the RobotReceiveTimeout time is for example purposes only. This will make the example running more + // reliable on non-realtime systems. Use with caution in productive applications. + bool ret = g_my_robot->ur_driver_->writeJointCommand(joint_target, comm::ControlMode::MODE_SERVOJ, + RobotReceiveTimeout::millisec(100)); + if (!ret) { - URCL_LOG_WARN("Could not get fresh data package from robot"); + URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?"); + return 1; } + URCL_LOG_DEBUG("data_pkg:\n%s", data_pkg->toString().c_str()); } - g_my_driver->stopControl(); + g_my_robot->ur_driver_->stopControl(); URCL_LOG_INFO("Movement done"); return 0; } diff --git a/examples/instruction_executor.cpp b/examples/instruction_executor.cpp new file mode 100644 index 00000000..9a55affb --- /dev/null +++ b/examples/instruction_executor.cpp @@ -0,0 +1,110 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2024 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#include +#include +#include + +#include "ur_client_library/types.h" +#include "ur_client_library/ur/ur_driver.h" +#include "ur_client_library/log.h" +#include +#include "ur_client_library/control/trajectory_point_interface.h" +#include "ur_client_library/ur/dashboard_client.h" +#include "ur_client_library/ur/instruction_executor.h" + +const std::string DEFAULT_ROBOT_IP = "192.168.56.101"; +const std::string SCRIPT_FILE = "resources/external_control.urscript"; +const std::string OUTPUT_RECIPE = "examples/resources/rtde_output_recipe.txt"; +const std::string INPUT_RECIPE = "examples/resources/rtde_input_recipe.txt"; +const std::string CALIBRATION_CHECKSUM = "calib_12788084448423163542"; + +std::unique_ptr g_my_robot; + +int main(int argc, char* argv[]) +{ + urcl::setLogLevel(urcl::LogLevel::INFO); + // Parse the ip arguments if given + std::string robot_ip = DEFAULT_ROBOT_IP; + if (argc > 1) + { + robot_ip = std::string(argv[1]); + } + + bool headless_mode = true; + g_my_robot = std::make_unique(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, headless_mode, + "external_control.urp"); + if (!g_my_robot->ur_driver_->checkCalibration(CALIBRATION_CHECKSUM)) + { + URCL_LOG_ERROR("Calibration checksum does not match actual robot."); + URCL_LOG_ERROR("Use the ur_calibration tool to extract the correct calibration from the robot and pass that into " + "the description. See " + "[https://github.com/UniversalRobots/Universal_Robots_ROS_Driver#extract-calibration-information] " + "for details."); + } + if (!g_my_robot->isHealthy()) + { + URCL_LOG_ERROR("Something in the robot initialization went wrong. Exiting. Please check the output above."); + return 1; + } + + auto instruction_executor = std::make_shared(g_my_robot->ur_driver_); + // --------------- INITIALIZATION END ------------------- + + URCL_LOG_INFO("Running motion"); + // Trajectory definition + std::vector> motion_sequence{ + std::make_shared(urcl::vector6d_t{ -1.57, -1.57, 0, 0, 0, 0 }, 0.1, + std::chrono::seconds(5)), + // This point uses acceleration / velocity parametrization + std::make_shared(urcl::vector6d_t{ -1.57, -1.6, 1.6, -0.7, 0.7, 0.2 }, 0.1, + std::chrono::seconds(0), 1.4, 2.0), + + std::make_shared(urcl::Pose(-0.203, 0.263, 0.559, 0.68, -1.083, -2.076), 0.1, + std::chrono::seconds(2)), + std::make_shared(urcl::Pose{ -0.203, 0.463, 0.559, 0.68, -1.083, -2.076 }, 0.1, + std::chrono::seconds(2)), + }; + instruction_executor->executeMotion(motion_sequence); + + double goal_time_sec = 2.0; + + // acceleration / velocity parametrization + instruction_executor->moveJ({ -1.57, -1.57, 0, 0, 0, 0 }, 2.0, 2.0); + // goal time parametrization -- acceleration and velocity will be ignored + instruction_executor->moveJ({ -1.57, -1.6, 1.6, -0.7, 0.7, 0.2 }, 0.1, 0.1, goal_time_sec); + // acceleration / velocity parametrization + instruction_executor->moveL({ -0.203, 0.263, 0.559, 0.68, -1.083, -2.076 }, 1.5, 1.5); + // goal time parametrization -- acceleration and velocity will be ignored + instruction_executor->moveL({ -0.203, 0.463, 0.559, 0.68, -1.083, -2.076 }, 0.1, 0.1, goal_time_sec); + + g_my_robot->ur_driver_->stopControl(); + return 0; +} diff --git a/examples/primary_pipeline.cpp b/examples/primary_pipeline.cpp index e4390d8b..e022b5d7 100644 --- a/examples/primary_pipeline.cpp +++ b/examples/primary_pipeline.cpp @@ -55,7 +55,7 @@ int main(int argc, char* argv[]) second_to_run = std::stoi(argv[2]); } - // First of all, we need a stream that connects to the robot + // First of all, we need a stream that connects to the robot's primary interface comm::URStream primary_stream(robot_ip, urcl::primary_interface::UR_PRIMARY_PORT); // This will parse the primary packages @@ -63,11 +63,12 @@ int main(int argc, char* argv[]) // The producer needs both, the stream and the parser to fully work comm::URProducer prod(primary_stream, parser); + + // Connect to the stream prod.setupProducer(); // The shell consumer will print the package contents to the shell - std::unique_ptr> consumer; - consumer.reset(new comm::ShellConsumer()); + auto consumer = std::make_unique>(); // The notifer will be called at some points during connection setup / loss. This isn't fully // implemented atm. diff --git a/examples/primary_pipeline_calibration.cpp b/examples/primary_pipeline_calibration.cpp index 9f54e2e8..bb1baed4 100644 --- a/examples/primary_pipeline_calibration.cpp +++ b/examples/primary_pipeline_calibration.cpp @@ -31,8 +31,11 @@ class CalibrationConsumer : public urcl::comm::IConsumer product) { + // Try to cast the product to a KinematicsInfo package. If that succeeds, we handle the + // calibration information. auto kin_info = std::dynamic_pointer_cast(product); if (kin_info != nullptr) { @@ -105,11 +108,11 @@ int main(int argc, char* argv[]) if (calib_consumer.isCalibrated()) { - printf("The robot on IP: %s is calibrated\n", robot_ip.c_str()); + printf("The robot on IP: %s is calibrated.\n", robot_ip.c_str()); } else { - printf("The robot controller on IP: %s do not have a valid calibration\n", robot_ip.c_str()); + printf("The robot controller on IP: %s does not have a valid calibration.\n", robot_ip.c_str()); printf("Remeber to turn on the robot to get calibration stored on the robot!\n"); } diff --git a/examples/rtde_client.cpp b/examples/rtde_client.cpp index 01a1bb40..0972aafc 100644 --- a/examples/rtde_client.cpp +++ b/examples/rtde_client.cpp @@ -27,6 +27,7 @@ #include +#include #include #include #include @@ -40,6 +41,20 @@ const std::string OUTPUT_RECIPE = "examples/resources/rtde_output_recipe.txt"; const std::string INPUT_RECIPE = "examples/resources/rtde_input_recipe.txt"; const std::chrono::milliseconds READ_TIMEOUT{ 100 }; +void printFraction(const double fraction, const std::string& label, const size_t width = 20) +{ + std::cout << "\r" << label << ": ["; + for (size_t i = 0; i < std::ceil(fraction * width); i++) + { + std::cout << "#"; + } + for (size_t i = 0; i < std::floor((1.0 - fraction) * width); i++) + { + std::cout << "-"; + } + std::cout << "]" << std::flush; +} + int main(int argc, char* argv[]) { // Parse the ip arguments if given @@ -56,13 +71,14 @@ int main(int argc, char* argv[]) second_to_run = std::stoi(argv[2]); } - // TODO: Write good docstring for notifier comm::INotifier notifier; - rtde_interface::RTDEClient my_client(robot_ip, notifier, OUTPUT_RECIPE, INPUT_RECIPE); + const double rtde_frequency = 50; // Hz + rtde_interface::RTDEClient my_client(robot_ip, notifier, OUTPUT_RECIPE, INPUT_RECIPE, rtde_frequency); my_client.init(); // We will use the speed_slider_fraction as an example how to write to RTDE double speed_slider_fraction = 1.0; + double target_speed_fraction = 1.0; double speed_slider_increment = 0.01; // Once RTDE communication is started, we have to make sure to read from the interface buffer, as @@ -70,8 +86,10 @@ int main(int argc, char* argv[]) // loop. my_client.start(); - unsigned long start_time = clock(); - while (second_to_run < 0 || ((clock() - start_time) / CLOCKS_PER_SEC) < static_cast(second_to_run)) + auto start_time = std::chrono::steady_clock::now(); + while (second_to_run <= 0 || + std::chrono::duration_cast(std::chrono::steady_clock::now() - start_time).count() < + second_to_run) { // Read latest RTDE package. This will block for READ_TIMEOUT, so the // robot will effectively be in charge of setting the frequency of this loop unless RTDE @@ -81,22 +99,19 @@ int main(int argc, char* argv[]) std::unique_ptr data_pkg = my_client.getDataPackage(READ_TIMEOUT); if (data_pkg) { - std::cout << data_pkg->toString() << std::endl; + // Data fields in the data package are accessed by their name. Only names present in the + // output recipe can be accessed. Otherwise this function will return false. + data_pkg->getData("target_speed_fraction", target_speed_fraction); + printFraction(target_speed_fraction, "target_speed_fraction"); } else { + // The client isn't connected properly anymore / doesn't receive any data anymore. Stop the + // program. std::cout << "Could not get fresh data package from robot" << std::endl; return 1; } - if (!my_client.getWriter().sendSpeedSlider(speed_slider_fraction)) - { - // This will happen for example, when the required keys are not configured inside the input - // recipe. - std::cout << "\033[1;31mSending RTDE data failed." << "\033[0m\n" << std::endl; - return 1; - } - // Change the speed slider so that it will move between 0 and 1 all the time. This is for // demonstration purposes only and gains no real value. if (speed_slider_increment > 0) @@ -111,6 +126,14 @@ int main(int argc, char* argv[]) speed_slider_increment *= -1; } speed_slider_fraction += speed_slider_increment; + + if (!my_client.getWriter().sendSpeedSlider(speed_slider_fraction)) + { + // This will happen for example, when the required keys are not configured inside the input + // recipe. + std::cout << "\033[1;31mSending RTDE data failed." << "\033[0m\n" << std::endl; + return 1; + } } // Resetting the speedslider back to 100% diff --git a/examples/spline_example.cpp b/examples/spline_example.cpp index 9af4240d..3fb5e1d9 100644 --- a/examples/spline_example.cpp +++ b/examples/spline_example.cpp @@ -30,6 +30,7 @@ // POSSIBILITY OF SUCH DAMAGE. // -- END LICENSE BLOCK ------------------------------------------------ +#include #include #include #include @@ -46,10 +47,8 @@ const std::string DEFAULT_ROBOT_IP = "192.168.56.101"; const std::string SCRIPT_FILE = "resources/external_control.urscript"; const std::string OUTPUT_RECIPE = "examples/resources/rtde_output_recipe.txt"; const std::string INPUT_RECIPE = "examples/resources/rtde_input_recipe.txt"; -const std::string CALIBRATION_CHECKSUM = "calib_12788084448423163542"; -std::unique_ptr g_my_driver; -std::unique_ptr g_my_dashboard; +std::unique_ptr g_my_robot; vector6d_t g_joint_positions; void sendTrajectory(const std::vector& p_p, const std::vector& p_v, @@ -58,43 +57,37 @@ void sendTrajectory(const std::vector& p_p, const std::vectorwriteTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, p_p.size()); + g_my_robot->ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, + p_p.size()); for (size_t i = 0; i < p_p.size() && p_p.size() == time.size() && p_p[i].size() == 6; i++) { // MoveJ if (!use_spline_interpolation_) { - g_my_driver->writeTrajectoryPoint(p_p[i], false, time[i]); + g_my_robot->ur_driver_->writeTrajectoryPoint(p_p[i], false, time[i]); } else // Use spline interpolation { // QUINTIC if (p_v.size() == time.size() && p_a.size() == time.size() && p_v[i].size() == 6 && p_a[i].size() == 6) { - g_my_driver->writeTrajectorySplinePoint(p_p[i], p_v[i], p_a[i], time[i]); + g_my_robot->ur_driver_->writeTrajectorySplinePoint(p_p[i], p_v[i], p_a[i], time[i]); } // CUBIC else if (p_v.size() == time.size() && p_v[i].size() == 6) { - g_my_driver->writeTrajectorySplinePoint(p_p[i], p_v[i], time[i]); + g_my_robot->ur_driver_->writeTrajectorySplinePoint(p_p[i], p_v[i], time[i]); } else { - g_my_driver->writeTrajectorySplinePoint(p_p[i], time[i]); + g_my_robot->ur_driver_->writeTrajectorySplinePoint(p_p[i], time[i]); } } } URCL_LOG_INFO("Finished Sending Trajectory"); } -// We need a callback function to register. See UrDriver's parameters for details. -void handleRobotProgramState(bool program_running) -{ - // Print the text in green so we see it better - std::cout << "\033[1;32mProgram running: " << std::boolalpha << program_running << "\033[0m\n" << std::endl; -} - // Callback function for trajectory execution. bool g_trajectory_running(false); void handleTrajectoryState(control::TrajectoryResult state) @@ -129,64 +122,24 @@ int main(int argc, char* argv[]) robot_ip = std::string(argv[1]); } - // Making the robot ready for the program by: - // Connect the the robot Dashboard - g_my_dashboard.reset(new DashboardClient(robot_ip)); - if (!g_my_dashboard->connect()) + bool headless_mode = true; + g_my_robot = std::make_unique(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, headless_mode, + "external_control.urp"); + if (!g_my_robot->isHealthy()) { - URCL_LOG_ERROR("Could not connect to dashboard"); + URCL_LOG_ERROR("Something in the robot initialization went wrong. Exiting. Please check the output above."); return 1; } - // Stop program, if there is one running - if (!g_my_dashboard->commandStop()) - { - URCL_LOG_ERROR("Could not send stop program command"); - return 1; - } - - // if the robot is not powered on and ready - std::string robot_mode_running("RUNNING"); - while (!g_my_dashboard->commandRobotMode(robot_mode_running)) - { - // Power it off - if (!g_my_dashboard->commandPowerOff()) - { - URCL_LOG_ERROR("Could not send Power off command"); - return 1; - } - - // Power it on - if (!g_my_dashboard->commandPowerOn()) - { - URCL_LOG_ERROR("Could not send Power on command"); - return 1; - } - } - - // Release the brakes - if (!g_my_dashboard->commandBrakeRelease()) - { - URCL_LOG_ERROR("Could not send BrakeRelease command"); - return 1; - } - - // Now the robot is ready to receive a program - - std::unique_ptr tool_comm_setup; - const bool headless = true; - g_my_driver.reset(new UrDriver(robot_ip, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, &handleRobotProgramState, headless, - std::move(tool_comm_setup), CALIBRATION_CHECKSUM)); - - g_my_driver->registerTrajectoryDoneCallback(&handleTrajectoryState); + g_my_robot->ur_driver_->registerTrajectoryDoneCallback(&handleTrajectoryState); // Once RTDE communication is started, we have to make sure to read from the interface buffer, as // otherwise we will get pipeline overflows. Therefore, do this directly before starting your main // loop. - g_my_driver->startRTDECommunication(); + g_my_robot->ur_driver_->startRTDECommunication(); - std::unique_ptr data_pkg = g_my_driver->getDataPackage(); + std::unique_ptr data_pkg = g_my_robot->ur_driver_->getDataPackage(); if (data_pkg) { @@ -209,7 +162,7 @@ int main(int argc, char* argv[]) 4.00000000e+00 }; bool ret = false; - ret = g_my_driver->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_NOOP); + ret = g_my_robot->ur_driver_->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_NOOP); if (!ret) { std::stringstream lastq; @@ -246,7 +199,7 @@ int main(int argc, char* argv[]) g_trajectory_running = true; while (g_trajectory_running) { - std::unique_ptr data_pkg = g_my_driver->getDataPackage(); + std::unique_ptr data_pkg = g_my_robot->ur_driver_->getDataPackage(); if (data_pkg) { // Read current joint positions from robot data @@ -256,7 +209,7 @@ int main(int argc, char* argv[]) std::string error_msg = "Did not find 'actual_q' in data sent from robot. This should not happen!"; throw std::runtime_error(error_msg); } - ret = g_my_driver->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_NOOP); + ret = g_my_robot->ur_driver_->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_NOOP); if (!ret) { @@ -277,7 +230,7 @@ int main(int argc, char* argv[]) g_trajectory_running = true; while (g_trajectory_running) { - std::unique_ptr data_pkg = g_my_driver->getDataPackage(); + std::unique_ptr data_pkg = g_my_robot->ur_driver_->getDataPackage(); if (data_pkg) { // Read current joint positions from robot data @@ -287,7 +240,7 @@ int main(int argc, char* argv[]) std::string error_msg = "Did not find 'actual_q' in data sent from robot. This should not happen!"; throw std::runtime_error(error_msg); } - ret = g_my_driver->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_NOOP); + ret = g_my_robot->ur_driver_->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_NOOP); if (!ret) { @@ -302,7 +255,7 @@ int main(int argc, char* argv[]) URCL_LOG_INFO("QUINTIC Movement done"); - ret = g_my_driver->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_NOOP); + ret = g_my_robot->ur_driver_->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_NOOP); if (!ret) { std::stringstream lastq; @@ -311,6 +264,6 @@ int main(int argc, char* argv[]) lastq.str().c_str()); return 1; } - g_my_driver->stopControl(); + g_my_robot->ur_driver_->stopControl(); return 0; } diff --git a/examples/tool_contact_example.cpp b/examples/tool_contact_example.cpp index 35bf739a..7d817ba8 100644 --- a/examples/tool_contact_example.cpp +++ b/examples/tool_contact_example.cpp @@ -33,6 +33,7 @@ // In a real-world example it would be better to get those values from command line parameters / a // better configuration system such as Boost.Program_options +#include #include #include #include @@ -46,20 +47,11 @@ const std::string DEFAULT_ROBOT_IP = "192.168.56.101"; const std::string SCRIPT_FILE = "resources/external_control.urscript"; const std::string OUTPUT_RECIPE = "examples/resources/rtde_output_recipe.txt"; const std::string INPUT_RECIPE = "examples/resources/rtde_input_recipe.txt"; -const std::string CALIBRATION_CHECKSUM = "calib_12788084448423163542"; -std::unique_ptr g_my_driver; -std::unique_ptr g_my_dashboard; -bool g_tool_contact_result_triggered; +std::unique_ptr g_my_robot; +std::atomic g_tool_contact_result_triggered; control::ToolContactResult g_tool_contact_result; -// We need a callback function to register. See UrDriver's parameters for details. -void handleRobotProgramState(bool program_running) -{ - // Print the text in green so we see it better - std::cout << "\033[1;32mProgram running: " << std::boolalpha << program_running << "\033[0m\n" << std::endl; -} - void handleToolContactResult(control::ToolContactResult result) { // Print the text in green so we see it better @@ -85,104 +77,40 @@ int main(int argc, char* argv[]) second_to_run = std::chrono::seconds(std::stoi(argv[2])); } - // Making the robot ready for the program by: - // Connect the the robot Dashboard - g_my_dashboard.reset(new DashboardClient(robot_ip)); - if (!g_my_dashboard->connect()) - { - URCL_LOG_ERROR("Could not connect to dashboard"); - return 1; - } - - // // Stop program, if there is one running - if (!g_my_dashboard->commandStop()) - { - URCL_LOG_ERROR("Could not send stop program command"); - return 1; - } - - // Power it off - if (!g_my_dashboard->commandPowerOff()) - { - URCL_LOG_ERROR("Could not send Power off command"); - return 1; - } - - // Power it on - if (!g_my_dashboard->commandPowerOn()) + bool headless_mode = true; + g_my_robot = std::make_unique(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, headless_mode, + "external_control.urp"); + if (!g_my_robot->isHealthy()) { - URCL_LOG_ERROR("Could not send Power on command"); + URCL_LOG_ERROR("Something in the robot initialization went wrong. Exiting. Please check the output above."); return 1; } - - // Release the brakes - if (!g_my_dashboard->commandBrakeRelease()) - { - URCL_LOG_ERROR("Could not send BrakeRelease command"); - return 1; - } - - // Now the robot is ready to receive a program - std::unique_ptr tool_comm_setup; - const bool headless = true; - g_my_driver.reset(new UrDriver(robot_ip, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, &handleRobotProgramState, headless, - std::move(tool_comm_setup), CALIBRATION_CHECKSUM)); - - g_my_driver->registerToolContactResultCallback(&handleToolContactResult); - - // Once RTDE communication is started, we have to make sure to read from the interface buffer, as - // otherwise we will get pipeline overflows. Therefor, do this directly before starting your main - // loop. - g_my_driver->startRTDECommunication(); + g_my_robot->ur_driver_->registerToolContactResultCallback(&handleToolContactResult); + g_my_robot->ur_driver_->startToolContact(); // This will move the robot downward in the z direction of the base until a tool contact is detected or seconds_to_run // is reached - std::chrono::duration time_done(0); - std::chrono::duration timeout(second_to_run); - vector6d_t tcp_speed = { 0.0, 0.0, -0.02, 0.0, 0.0, 0.0 }; - auto stopwatch_last = std::chrono::steady_clock::now(); - auto stopwatch_now = stopwatch_last; - g_my_driver->startToolContact(); - - while (true) + const vector6d_t tcp_speed = { 0.0, 0.0, -0.02, 0.0, 0.0, 0.0 }; + auto start_time = std::chrono::system_clock::now(); + while (second_to_run.count() < 0 || (std::chrono::system_clock::now() - start_time) < second_to_run) { - // Read latest RTDE package. This will block for a hard-coded timeout (see UrDriver), so the - // robot will effectively be in charge of setting the frequency of this loop. - // In a real-world application this thread should be scheduled with real-time priority in order - // to ensure that this is called in time. - std::unique_ptr data_pkg = g_my_driver->getDataPackage(); - if (data_pkg) + // Setting the RobotReceiveTimeout time is for example purposes only. This will make the example running more + // reliable on non-realtime systems. Use with caution in productive applications. + bool ret = g_my_robot->ur_driver_->writeJointCommand(tcp_speed, comm::ControlMode::MODE_SPEEDL, + RobotReceiveTimeout::millisec(100)); + if (!ret) { - // Setting the RobotReceiveTimeout time is for example purposes only. This will make the example running more - // reliable on non-realtime systems. Use with caution in productive applications. - bool ret = - g_my_driver->writeJointCommand(tcp_speed, comm::ControlMode::MODE_SPEEDL, RobotReceiveTimeout::millisec(100)); - if (!ret) - { - URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?"); - return 1; - } - - if (g_tool_contact_result_triggered) - { - URCL_LOG_INFO("Tool contact result triggered. Received tool contact result %i.", - toUnderlying(g_tool_contact_result)); - break; - } - - if (time_done > timeout && second_to_run.count() != 0) - { - URCL_LOG_INFO("Timed out before reaching tool contact."); - break; - } + URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?"); + return 1; } - else + + if (g_tool_contact_result_triggered) { - URCL_LOG_WARN("Could not get fresh data package from robot"); + URCL_LOG_INFO("Tool contact detected"); + break; } - - stopwatch_now = std::chrono::steady_clock::now(); - time_done += stopwatch_now - stopwatch_last; - stopwatch_last = stopwatch_now; } + URCL_LOG_INFO("Timed out before reaching tool contact."); + g_my_robot->ur_driver_->stopControl(); + return 0; } diff --git a/examples/trajectory_point_interface.cpp b/examples/trajectory_point_interface.cpp index 5a7173b2..fd169e9c 100644 --- a/examples/trajectory_point_interface.cpp +++ b/examples/trajectory_point_interface.cpp @@ -32,6 +32,7 @@ #include #include +#include #include "ur_client_library/types.h" #include "ur_client_library/ur/ur_driver.h" #include "ur_client_library/log.h" @@ -42,24 +43,13 @@ const std::string DEFAULT_ROBOT_IP = "192.168.56.101"; const std::string SCRIPT_FILE = "resources/external_control.urscript"; const std::string OUTPUT_RECIPE = "examples/resources/rtde_output_recipe.txt"; const std::string INPUT_RECIPE = "examples/resources/rtde_input_recipe.txt"; -const std::string CALIBRATION_CHECKSUM = "calib_12788084448423163542"; - -std::unique_ptr g_my_dashboard; -std::unique_ptr g_my_driver; +std::unique_ptr g_my_robot; std::atomic g_trajectory_done = false; -// We need a callback function to register. See UrDriver's parameters for details. -void handleRobotProgramState(bool program_running) -{ - // Print the text in green so we see it better - std::cout << "\033[1;32mProgram running: " << std::boolalpha << program_running << "\033[0m\n" << std::endl; -} - void trajDoneCallback(const urcl::control::TrajectoryResult& result) { - URCL_LOG_INFO("Trajectory done with result %d", result); - ; + URCL_LOG_INFO("Trajectory done with result %s", urcl::control::trajectoryResultToString(result).c_str()); g_trajectory_done = true; } @@ -73,51 +63,17 @@ int main(int argc, char* argv[]) robot_ip = std::string(argv[1]); } - // --------------- INITIALIZATION BEGIN ------------------- - // Making the robot ready for the program by: - // Connect the robot Dashboard - g_my_dashboard.reset(new urcl::DashboardClient(robot_ip)); - if (!g_my_dashboard->connect()) - { - URCL_LOG_ERROR("Could not connect to dashboard"); - return 1; - } - - // // Stop program, if there is one running - if (!g_my_dashboard->commandStop()) + bool headless_mode = true; + g_my_robot = std::make_unique(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, headless_mode, + "external_control.urp"); + if (!g_my_robot->isHealthy()) { - URCL_LOG_ERROR("Could not send stop program command"); + URCL_LOG_ERROR("Something in the robot initialization went wrong. Exiting. Please check the output above."); return 1; } - - // Power it off - if (!g_my_dashboard->commandPowerOff()) - { - URCL_LOG_ERROR("Could not send Power off command"); - return 1; - } - - // Power it on - if (!g_my_dashboard->commandPowerOn()) - { - URCL_LOG_ERROR("Could not send Power on command"); - return 1; - } - - // Release the brakes - if (!g_my_dashboard->commandBrakeRelease()) - { - URCL_LOG_ERROR("Could not send BrakeRelease command"); - return 1; - } - - std::unique_ptr tool_comm_setup; - const bool headless = true; - g_my_driver.reset(new urcl::UrDriver(robot_ip, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, &handleRobotProgramState, - headless, std::move(tool_comm_setup), CALIBRATION_CHECKSUM)); // --------------- INITIALIZATION END ------------------- - g_my_driver->registerTrajectoryDoneCallback(&trajDoneCallback); + g_my_robot->ur_driver_->registerTrajectoryDoneCallback(&trajDoneCallback); URCL_LOG_INFO("Running MoveJ motion"); // --------------- MOVEJ TRAJECTORY ------------------- @@ -126,27 +82,30 @@ int main(int argc, char* argv[]) // Trajectory definition std::vector points{ { -1.57, -1.57, 0, 0, 0, 0 }, { -1.57, -1.6, 1.6, -0.7, 0.7, 0.2 } }; std::vector motion_durations{ 5.0, 2.0 }; + std::vector velocities{ 2.0, 2.3 }; + std::vector accelerations{ 2.5, 2.5 }; std::vector blend_radii{ 0.1, 0.1 }; // Trajectory execution - g_my_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, - points.size() * 2); + g_my_robot->ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, + points.size() * 2); for (size_t i = 0; i < points.size(); i++) { - g_my_driver->writeTrajectoryPoint(points[i], false, motion_durations[i], blend_radii[i]); + g_my_robot->ur_driver_->writeTrajectoryPoint(points[i], false, motion_durations[i], blend_radii[i]); } // Same motion, but parametrized with acceleration and velocity motion_durations = { 0.0, 0.0 }; for (size_t i = 0; i < points.size(); i++) { - g_my_driver->writeTrajectoryPoint(points[i], 2.0, 2.0, false, motion_durations[i], blend_radii[i]); + g_my_robot->ur_driver_->writeTrajectoryPoint(points[i], accelerations[i], velocities[i], false, + motion_durations[i], blend_radii[i]); } while (!g_trajectory_done) { std::this_thread::sleep_for(std::chrono::milliseconds(100)); - g_my_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP); + g_my_robot->ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP); } } // --------------- END MOVEJ TRAJECTORY ------------------- @@ -159,21 +118,31 @@ int main(int argc, char* argv[]) std::vector points{ { -0.203, 0.263, 0.559, 0.68, -1.083, -2.076 }, { -0.203, 0.463, 0.559, 0.68, -1.083, -2.076 } }; std::vector motion_durations{ 5.0, 5.0 }; + std::vector velocities{ 2.0, 2.3 }; + std::vector accelerations{ 2.5, 2.5 }; std::vector blend_radii{ 0.0, 0.0 }; - // Trajectory execution - g_my_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, - points.size()); + // Trajectory execution of the path that goes through the points twice. + g_my_robot->ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, + points.size() * 2); for (size_t i = 0; i < points.size(); i++) { // setting the cartesian parameter makes it interpret the 6d vector as a pose and use movel - g_my_driver->writeTrajectoryPoint(points[i], true, motion_durations[i], blend_radii[i]); + g_my_robot->ur_driver_->writeTrajectoryPoint(points[i], true, motion_durations[i], blend_radii[i]); + } + + // Same motion, but parametrized with acceleration and velocity + motion_durations = { 0.0, 0.0 }; + for (size_t i = 0; i < points.size(); i++) + { + g_my_robot->ur_driver_->writeTrajectoryPoint(points[i], accelerations[i], velocities[i], true, + motion_durations[i], blend_radii[i]); } while (!g_trajectory_done) { std::this_thread::sleep_for(std::chrono::milliseconds(100)); - g_my_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP); + g_my_robot->ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP); } } // --------------- END MOVEL TRAJECTORY ------------------- @@ -193,21 +162,21 @@ int main(int argc, char* argv[]) std::vector motion_durations{ 3.0, 3.0, 3.0, 3.0 }; // Trajectory execution - g_my_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, - positions.size()); + g_my_robot->ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, + positions.size()); for (size_t i = 0; i < positions.size(); i++) { - g_my_driver->writeTrajectorySplinePoint(positions[i], velocities[i], motion_durations[i]); + g_my_robot->ur_driver_->writeTrajectorySplinePoint(positions[i], velocities[i], motion_durations[i]); } while (!g_trajectory_done) { std::this_thread::sleep_for(std::chrono::milliseconds(100)); - g_my_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP); + g_my_robot->ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP); } } - // --------------- END MOVEJ TRAJECTORY ------------------- + // --------------- END SPLINE TRAJECTORY ------------------- - g_my_driver->stopControl(); + g_my_robot->ur_driver_->stopControl(); return 0; } diff --git a/include/ur_client_library/comm/pipeline.h b/include/ur_client_library/comm/pipeline.h index afd9100b..f18f1741 100644 --- a/include/ur_client_library/comm/pipeline.h +++ b/include/ur_client_library/comm/pipeline.h @@ -29,6 +29,8 @@ #include #include #include +#include +#include namespace urcl { @@ -83,7 +85,7 @@ class IConsumer /*! * \brief Consumer, that allows one product to be consumed by multiple arbitrary - * conusmers. + * consumers. * * @tparam T Type of the consumed products */ @@ -91,7 +93,7 @@ template class MultiConsumer : public IConsumer { private: - std::vector*> consumers_; + std::vector>> consumers_; public: /*! @@ -99,10 +101,38 @@ class MultiConsumer : public IConsumer * * \param consumers The list of consumers that should all consume given products */ - MultiConsumer(std::vector*> consumers) : consumers_(consumers) + MultiConsumer(std::vector>> consumers) : consumers_(consumers) { } + /*! + * \brief Adds a new consumer to the list of consumers + * + * \param consumer Consumer that should be added to the list + */ + void addConsumer(std::shared_ptr> consumer) + { + std::lock_guard lk(consumer_list_); + consumers_.push_back(consumer); + } + + /*! + * \brief Remove a consumer from the list of consumers + * + * \param consumer Consumer that should be removed from the list + */ + void removeConsumer(std::shared_ptr> consumer) + { + std::lock_guard lk(consumer_list_); + auto it = std::find(consumers_.begin(), consumers_.end(), consumer); + if (it == consumers_.end()) + { + URCL_LOG_ERROR("Unable to remove consumer as it is not part of the consumer list"); + return; + } + consumers_.erase(it); + } + /*! * \brief Sets up all registered consumers. */ @@ -153,6 +183,7 @@ class MultiConsumer : public IConsumer */ bool consume(std::shared_ptr product) { + std::lock_guard lk(consumer_list_); bool res = true; for (auto& con : consumers_) { @@ -161,6 +192,9 @@ class MultiConsumer : public IConsumer } return res; } + +private: + std::mutex consumer_list_; }; /*! @@ -234,7 +268,7 @@ class INotifier }; /*! - * \brief The Pipepline manages the production and optionally consumption of packages. Cyclically + * \brief The Pipeline manages the production and optionally consumption of packages. Cyclically * the producer is called and returned packages are saved in a queue. This queue is then either also * cyclically utilized by the registered consumer or can be externally used. * diff --git a/include/ur_client_library/comm/socket_t.h b/include/ur_client_library/comm/socket_t.h index c4fd3b56..9c1e980b 100644 --- a/include/ur_client_library/comm/socket_t.h +++ b/include/ur_client_library/comm/socket_t.h @@ -18,18 +18,18 @@ #ifdef _WIN32 -#define NOMINMAX -#define WIN32_LEAN_AND_MEAN -#include -#include +# define NOMINMAX +# define WIN32_LEAN_AND_MEAN +# include +# include -#ifndef TCP_QUICKACK -#define TCP_QUICKACK 12 -#endif +# ifndef TCP_QUICKACK +# define TCP_QUICKACK 12 +# endif -#ifdef ERROR -#undef ERROR -#endif // ERROR +# ifdef ERROR +# undef ERROR +# endif // ERROR typedef SOCKET socket_t; typedef SSIZE_T ssize_t; @@ -44,20 +44,20 @@ static inline int ur_close(socket_t s) return ::closesocket(s); } -#else // _WIN32 +#else // _WIN32 -#include -#include -#include -#include +# include +# include +# include +# include typedef int socket_t; -#ifndef INVALID_SOCKET -#define INVALID_SOCKET (-1) -#endif +# ifndef INVALID_SOCKET +# define INVALID_SOCKET (-1) +# endif -#define ur_setsockopt setsockopt -#define ur_close close +# define ur_setsockopt setsockopt +# define ur_close close #endif // _WIN32 diff --git a/include/ur_client_library/comm/tcp_server.h b/include/ur_client_library/comm/tcp_server.h index e8f521a2..66f113e4 100644 --- a/include/ur_client_library/comm/tcp_server.h +++ b/include/ur_client_library/comm/tcp_server.h @@ -29,7 +29,6 @@ #ifndef UR_CLIENT_LIBRARY_TCP_SERVER_H_INCLUDED #define UR_CLIENT_LIBRARY_TCP_SERVER_H_INCLUDED - #include #include #include @@ -37,7 +36,6 @@ #include "ur_client_library/comm/socket_t.h" - namespace urcl { namespace comm diff --git a/include/ur_client_library/comm/tcp_socket.h b/include/ur_client_library/comm/tcp_socket.h index c166d379..5a1d468c 100644 --- a/include/ur_client_library/comm/tcp_socket.h +++ b/include/ur_client_library/comm/tcp_socket.h @@ -27,7 +27,6 @@ #include "ur_client_library/comm/socket_t.h" - namespace urcl { namespace comm diff --git a/include/ur_client_library/control/motion_primitives.h b/include/ur_client_library/control/motion_primitives.h new file mode 100644 index 00000000..130eb27c --- /dev/null +++ b/include/ur_client_library/control/motion_primitives.h @@ -0,0 +1,95 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2025 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#ifndef UR_CLIENT_LIBRARY_MOTION_PRIMITIVES_H_INCLUDED +#define UR_CLIENT_LIBRARY_MOTION_PRIMITIVES_H_INCLUDED + +#include +#include + +namespace urcl +{ +namespace control +{ + +enum class MotionType : uint8_t +{ + MOVEJ = 0, + MOVEL = 1, + MOVEP = 2, + MOVEC = 3, + SPLINE = 51, + UNKNOWN = 255 +}; +struct MotionPrimitive +{ + MotionType type; + std::chrono::duration duration; + double acceleration; + double velocity; + double blend_radius; +}; + +struct MoveJPrimitive : public MotionPrimitive +{ + MoveJPrimitive(const urcl::vector6d_t& target, const double blend_radius = 0, + const std::chrono::duration duration = std::chrono::milliseconds(0), + const double acceleration = 1.4, const double velocity = 1.04) + { + type = MotionType::MOVEJ; + target_joint_configuration = target; + this->duration = duration; + this->acceleration = acceleration; + this->velocity = velocity; + this->blend_radius = blend_radius; + } + + urcl::vector6d_t target_joint_configuration; +}; + +struct MoveLPrimitive : public MotionPrimitive +{ + MoveLPrimitive(const urcl::Pose& target, const double blend_radius = 0, + const std::chrono::duration duration = std::chrono::milliseconds(0), + const double acceleration = 1.4, const double velocity = 1.04) + { + type = MotionType::MOVEL; + target_pose = target; + this->duration = duration; + this->acceleration = acceleration; + this->velocity = velocity; + this->blend_radius = blend_radius; + } + + urcl::Pose target_pose; +}; +} // namespace control +} // namespace urcl +#endif // UR_CLIENT_LIBRARY_MOTION_PRIMITIVES_H_INCLUDED \ No newline at end of file diff --git a/include/ur_client_library/control/reverse_interface.h b/include/ur_client_library/control/reverse_interface.h index 685426c6..eb3f56bd 100644 --- a/include/ur_client_library/control/reverse_interface.h +++ b/include/ur_client_library/control/reverse_interface.h @@ -141,6 +141,11 @@ class ReverseInterface "commands.")]] virtual void setKeepaliveCount(const uint32_t count); + void registerDisconnectionCallback(std::function disconnection_fun) + { + disconnection_callback_ = disconnection_fun; + } + protected: virtual void connectionCallback(const socket_t filedescriptor); @@ -148,6 +153,7 @@ class ReverseInterface virtual void messageCallback(const socket_t filedescriptor, char* buffer, int nbytesrecv); + std::function disconnection_callback_ = nullptr; socket_t client_fd_; comm::TCPServer server_; diff --git a/include/ur_client_library/control/trajectory_point_interface.h b/include/ur_client_library/control/trajectory_point_interface.h index ddb211a5..350fe249 100644 --- a/include/ur_client_library/control/trajectory_point_interface.h +++ b/include/ur_client_library/control/trajectory_point_interface.h @@ -52,6 +52,8 @@ enum class TrajectoryResult : int32_t TRAJECTORY_RESULT_FAILURE = 2 ///< Aborted due to error during execution }; +std::string trajectoryResultToString(const TrajectoryResult result); + /*! * Spline types */ diff --git a/include/ur_client_library/example_robot_wrapper.h b/include/ur_client_library/example_robot_wrapper.h new file mode 100644 index 00000000..1a24fe46 --- /dev/null +++ b/include/ur_client_library/example_robot_wrapper.h @@ -0,0 +1,196 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2025 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#ifndef UR_CLIENT_LIBRARY_EXAMPLE_ROBOT_WRAPPER_H_INCLUDED +#define UR_CLIENT_LIBRARY_EXAMPLE_ROBOT_WRAPPER_H_INCLUDED + +#include +#include +#include + +namespace urcl +{ +/*! + * \class ExampleRobotWrapper + * \brief This class is a high-level abstraction around UrDriver and DashboardClient. It's main + * purpose is to help us avoiding repetitive robot initialization code in our examples and tests. + * + * It is therefore not intended to be used in production code, but rather as a helper class for + * developers. If you want to use this wrapper in your own code, please make sure to understand the + * logic behind it and adjust it to your needs. + * + * Since this is mainly intended for internal use, don't count on the API being stable for this + * class! + */ +class ExampleRobotWrapper +{ +public: + inline static const std::string DEFAULT_ROBOT_IP = "192.168.56.101"; + inline static const std::string SCRIPT_FILE = "resources/external_control.urscript"; + + ExampleRobotWrapper() = delete; + /*! + * \brief Construct a new Example Robot Wrapper object + * + * This will connect to a robot and initialize it. In headless mode the program will be running + * instantly, in teach pendant mode the from \p autostart_program will be started. + * + * Note: RTDE communication has to be started separately. + * + * \param robot_ip IP address of the robot to connect to + * \param output_recipe_file Output recipe file for RTDE communication + * \param input_recipe_file Input recipe file for RTDE communication + * \param headless_mode Should the driver be started in headless mode or not? + * \param autostart_program Program to start automatically after initialization when not in + * headless mode. This flag is ignored in headless mode. + * \param script_file URScript file to send to the robot. That should be script code + * communicating to the driver's reverse interface and trajectory interface. + */ + ExampleRobotWrapper(const std::string& robot_ip, const std::string& output_recipe_file, + const std::string& input_recipe_file, const bool headless_mode = true, + const std::string& autostart_program = "", const std::string& script_file = SCRIPT_FILE); + ~ExampleRobotWrapper(); + + /** + * @brief Initializes the robot in order to be able to start a program. + * + * The robot will be power-cycled once and end up switched on, breaks released. + */ + bool initializeRobotWithDashboard(); + + /** + * @brief Starts RTDE communication with the robot. + * + * @param consume_data Once the RTDE client is started, it's data has to be consumed. If you + * don't actually care about that data, this class can silently consume RTDE data when `true` is + * passed. This can be stopped and started at any time using the startConsumingRTDEData() and + * stopConsumingRTDEData() methods. + */ + void startRTDECommununication(const bool consume_data = false); + + /** + * @brief Start consuming RTDE data in the background. + */ + void startConsumingRTDEData(); + + /** + * @brief Stop consuming RTDE data in the background. Note that data has to be consumed manually + * using readDataPackage(). + */ + void stopConsumingRTDEData(); + + /** + * @brief Get the latest RTDE package. + * + * Do not call this, while RTDE data is being consumed in the background. In doubt, call + * stopConsumingRTDEData() before calling this function. + * + * @param[out] data_pkg The data package will be stored in that object + * @return true on a successful read, false if no package can be read or when RTDE data is + * already being consumed in the background. + */ + bool readDataPackage(std::unique_ptr& data_pkg); + + /** + * @brief Blocks until there is a robot program connected to the driver's reverse interface or + * until the timeout is hit. + * + * @param milliseconds How long to wait for a successful connection. + * @return True on a successful connection, false if not connection could be detected before the + * timeout. + */ + bool waitForProgramRunning(int milliseconds = 100); + + /** + * @brief Blocks until there is a disconnection event from the driver's reverse interface + * detected or until the timeout is hit. + * + * @param milliseconds How long to wait for a disconnection. + * @return True on a disconnection event has been detected, false if no event could be detected before the + * timeout. + */ + bool waitForProgramNotRunning(int milliseconds = 100); + + /** + * @brief Depending on whether it is headless or not start autostart_program or call driver's resendRobotProgram + * function. + * + * @return True on successful program start, false otherwise. + */ + bool resendRobotProgram(); + + /** + * @brief Start the program \p program_file_name on the robot. + * + * The program has be be present on the robot, otherwise this call does not succeed. The robot + * needs to be in remote_control mode for this to work properly. + * + * @param program_file_name Filename on the robot including the ".urp" extension. + * @return True on successful program start, false otherwise. + */ + bool startRobotProgram(const std::string& program_file_name); + + /** + * @brief Clear protective stop on the robot. + * + * This will try to clear a protective stop on the robot. If the robot is not in protective stop + * this call will do nothing. + */ + bool clearProtectiveStop(); + + bool isHealthy() const; + + std::shared_ptr dashboard_client_; /*!< Dashboard client to interact with the robot */ + std::shared_ptr ur_driver_; /*!< UR driver to interact with the robot */ + +private: + void handleRobotProgramState(bool program_running); + + std::atomic rtde_communication_started_ = false; + std::atomic consume_rtde_packages_ = false; + std::mutex read_package_mutex_; + std::unique_ptr data_pkg_; + + bool robot_initialized_ = false; + + bool program_running_; + std::condition_variable program_running_cv_; + std::condition_variable program_not_running_cv_; + std::mutex program_running_mutex_; + std::mutex program_not_running_mutex_; + + std::thread rtde_consumer_thread_; + + bool headless_mode_; + std::string autostart_program_; +}; +} // namespace urcl + +#endif diff --git a/include/ur_client_library/exceptions.h b/include/ur_client_library/exceptions.h index 1ccdc429..125e4117 100644 --- a/include/ur_client_library/exceptions.h +++ b/include/ur_client_library/exceptions.h @@ -35,12 +35,12 @@ #include "ur/version_information.h" #ifdef _WIN32 -#define NOMINMAX -#define WIN32_LEAN_AND_MEAN -#include -#ifdef ERROR -#undef ERROR -#endif // ERROR +# define NOMINMAX +# define WIN32_LEAN_AND_MEAN +# include +# ifdef ERROR +# undef ERROR +# endif // ERROR #endif namespace urcl diff --git a/include/ur_client_library/helpers.h b/include/ur_client_library/helpers.h index cf2b133c..e1744023 100644 --- a/include/ur_client_library/helpers.h +++ b/include/ur_client_library/helpers.h @@ -29,22 +29,20 @@ #ifndef UR_CLIENT_LIBRARY_HELPERS_H_INCLUDED #define UR_CLIENT_LIBRARY_HELPERS_H_INCLUDED - #ifdef _WIN32 -#define NOMINMAX -#define WIN32_LEAN_AND_MEAN -#include +# define NOMINMAX +# define WIN32_LEAN_AND_MEAN +# include -#ifdef ERROR -#undef ERROR -#endif // ERROR +# ifdef ERROR +# undef ERROR +# endif // ERROR -#define SCHED_FIFO (1) +# define SCHED_FIFO (1) typedef HANDLE pthread_t; - static inline pthread_t pthread_self() { return ::GetCurrentThread(); @@ -56,11 +54,11 @@ static inline int sched_get_priority_max(int policy) return THREAD_PRIORITY_TIME_CRITICAL; } -#else // _WIN32 +#else // _WIN32 -#include +# include -#endif // _WIN32 +#endif // _WIN32 namespace urcl { diff --git a/include/ur_client_library/primary/abstract_primary_consumer.h b/include/ur_client_library/primary/abstract_primary_consumer.h index b2c13c6d..3ed464e4 100644 --- a/include/ur_client_library/primary/abstract_primary_consumer.h +++ b/include/ur_client_library/primary/abstract_primary_consumer.h @@ -32,6 +32,7 @@ #include "ur_client_library/log.h" #include "ur_client_library/comm/pipeline.h" #include "ur_client_library/primary/robot_message/version_message.h" +#include "ur_client_library/primary/robot_message/error_code_message.h" #include "ur_client_library/primary/robot_state/kinematics_info.h" namespace urcl @@ -51,7 +52,7 @@ class AbstractPrimaryConsumer : public comm::IConsumer virtual ~AbstractPrimaryConsumer() = default; /*! - * \brief This consume method is usally being called by the Pipeline structure. We don't + * \brief This consume method is usually being called by the Pipeline structure. We don't * necessarily need to know the specific package type here, as the packages themselves will take * care to be consumed with the correct function (visitor pattern). * @@ -73,6 +74,7 @@ class AbstractPrimaryConsumer : public comm::IConsumer virtual bool consume(RobotState& pkg) = 0; virtual bool consume(VersionMessage& pkg) = 0; virtual bool consume(KinematicsInfo& pkg) = 0; + virtual bool consume(ErrorCodeMessage& pkg) = 0; private: /* data */ diff --git a/include/ur_client_library/primary/primary_client.h b/include/ur_client_library/primary/primary_client.h new file mode 100644 index 00000000..d8a1bf15 --- /dev/null +++ b/include/ur_client_library/primary/primary_client.h @@ -0,0 +1,99 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright © 2024-2025 Ocado Group +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#ifndef UR_CLIENT_LIBRARY_PRIMARY_CLIENT_H_INCLUDED +#define UR_CLIENT_LIBRARY_PRIMARY_CLIENT_H_INCLUDED + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace urcl +{ +namespace primary_interface +{ +class PrimaryClient +{ +public: + PrimaryClient() = delete; + PrimaryClient(const std::string& robot_ip, comm::INotifier& notifier); + ~PrimaryClient(); + + /*! + * \brief Adds a primary consumer to the list of consumers + * + * \param primary_consumer Primary consumer that should be added to the list + */ + void addPrimaryConsumer(std::shared_ptr> primary_consumer); + + /*! + * \brief Remove a primary consumer from the list of consumers + * + * \param primary_consumer Primary consumer that should be removed from the list + */ + void removePrimaryConsumer(std::shared_ptr> primary_consumer); + void start(const size_t max_connection_attempts = 0, + const std::chrono::milliseconds reconnection_timeout = urcl::comm::TCPSocket::DEFAULT_RECONNECTION_TIME); + + /*! + * \brief Retrieves previously raised error codes from PrimaryClient. After calling this, recorded errors will be + * deleted. + */ + std::deque getErrorCodes(); + +private: + // The function is called whenever an error code message is received + void errorMessageCallback(ErrorCode& code); + + PrimaryParser parser_; + std::shared_ptr consumer_; + std::unique_ptr> multi_consumer_; + + comm::INotifier notifier_; + + comm::URStream stream_; + std::unique_ptr> prod_; + std::unique_ptr> pipeline_; + + std::mutex error_code_queue_mutex_; + std::deque error_code_queue_; +}; + +} // namespace primary_interface +} // namespace urcl + +#endif // ifndef UR_CLIENT_LIBRARY_PRIMARY_CLIENT_H_INCLUDED diff --git a/include/ur_client_library/primary/primary_consumer.h b/include/ur_client_library/primary/primary_consumer.h new file mode 100644 index 00000000..cdb84057 --- /dev/null +++ b/include/ur_client_library/primary/primary_consumer.h @@ -0,0 +1,172 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- +// +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2020 FZI Forschungszentrum Informatik +// +// 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. +// -- END LICENSE BLOCK ------------------------------------------------ + +//---------------------------------------------------------------------- +/*!\file + * + * \author Felix Exner exner@fzi.de + * \date 2020-04-30 + * + */ +//---------------------------------------------------------------------- + +#ifndef UR_CLIENT_LIBRARY_PRIMARY_CONSUMER_H_INCLUDED +#define UR_CLIENT_LIBRARY_PRIMARY_CONSUMER_H_INCLUDED + +#include "ur_client_library/primary/abstract_primary_consumer.h" + +#include +#include +#include + +namespace urcl +{ +namespace primary_interface +{ +/*! + * \brief Primary consumer implementation + * + * This class implements an AbstractPrimaryConsumer such that it can consume all incoming primary + * messages. The actual work for each package will be done in this class. + */ +class PrimaryConsumer : public AbstractPrimaryConsumer +{ +public: + PrimaryConsumer() + { + } + virtual ~PrimaryConsumer() = default; + + /*! + * \brief Consume a RobotMessage + * + * \param msg Robot message + * + * \returns True + */ + virtual bool consume(RobotMessage& msg) override + { + return true; + } + + /*! + * \brief Consume a RobotState + * + * \param msg Robot state + * + * \returns True + */ + virtual bool consume(RobotState& msg) override + { + return true; + } + + /*! + * \brief Handle a VersionMessage + * + * \param pkg VersionMessage + * + * \returns True + */ + virtual bool consume(VersionMessage& pkg) override + { + return true; + } + + /*! + * \brief Handle a KinematicsInfo + * + * \param pkg KinematicsInfo + * + * \returns True + */ + virtual bool consume(KinematicsInfo& pkg) override + { + return true; + } + + /*! + * \brief Handle an ErrorCodeMessage + * + * \param pkg ErrorCodeMessage + * + * \returns True + */ + virtual bool consume(ErrorCodeMessage& pkg) override + { + urcl::primary_interface::ErrorCode code; + code.message_code = pkg.message_code_; + code.message_argument = pkg.message_argument_; + code.report_level = pkg.report_level_; + code.data_type = pkg.data_type_; + code.data = pkg.data_; + code.text = pkg.text_; + code.timestamp = pkg.timestamp_; + code.to_string = pkg.toString(); + + const auto log_contents = "Logging an ErrorCodeMessage from the UR Controller Box: " + pkg.toString(); + + switch (code.report_level) + { + case urcl::primary_interface::ReportLevel::DEBUG: + case urcl::primary_interface::ReportLevel::DEVL_DEBUG: + case urcl::primary_interface::ReportLevel::DEVL_INFO: + case urcl::primary_interface::ReportLevel::DEVL_WARNING: + case urcl::primary_interface::ReportLevel::DEVL_VIOLATION: + case urcl::primary_interface::ReportLevel::DEVL_FAULT: + URCL_LOG_DEBUG(log_contents.c_str()); + break; + case urcl::primary_interface::ReportLevel::INFO: + URCL_LOG_INFO(log_contents.c_str()); + break; + case urcl::primary_interface::ReportLevel::WARNING: + URCL_LOG_WARN(log_contents.c_str()); + break; + default: + // urcl::primary_interface::ReportLevel::VIOLATION: + // urcl::primary_interface::ReportLevel::FAULT: + URCL_LOG_ERROR(log_contents.c_str()); + break; + } + + if (error_code_message_callback_ != nullptr) + { + error_code_message_callback_(code); + } + return true; + } + + /*! + * \brief Set callback function which will be triggered whenever error code messages are received + * + * \param callback_function Function handling the event information. The error code message received is passed to the + * function. + */ + void setErrorCodeMessageCallback(std::function callback_function) + { + error_code_message_callback_ = callback_function; + } + +private: + std::function error_code_message_callback_; +}; + +} // namespace primary_interface +} // namespace urcl + +#endif // ifndef UR_CLIENT_LIBRARY_PRIMARY_CONSUMER_H_INCLUDED diff --git a/include/ur_client_library/primary/primary_parser.h b/include/ur_client_library/primary/primary_parser.h index 3dfba100..74d423a8 100644 --- a/include/ur_client_library/primary/primary_parser.h +++ b/include/ur_client_library/primary/primary_parser.h @@ -28,6 +28,7 @@ #include "ur_client_library/primary/robot_message.h" #include "ur_client_library/primary/robot_state/kinematics_info.h" #include "ur_client_library/primary/robot_message/version_message.h" +#include "ur_client_library/primary/robot_message/error_code_message.h" namespace urcl { @@ -174,6 +175,8 @@ class PrimaryParser : public comm::Parser return new MBD;*/ case RobotMessagePackageType::ROBOT_MESSAGE_VERSION: return new VersionMessage(timestamp, source); + case RobotMessagePackageType::ROBOT_MESSAGE_ERROR_CODE: + return new ErrorCodeMessage(timestamp, source); default: return new RobotMessage(timestamp, source); } diff --git a/include/ur_client_library/primary/robot_message.h b/include/ur_client_library/primary/robot_message.h index 5ee0c3a2..120a37e2 100644 --- a/include/ur_client_library/primary/robot_message.h +++ b/include/ur_client_library/primary/robot_message.h @@ -66,6 +66,17 @@ class RobotMessage : public PrimaryPackage RobotMessage(const uint64_t timestamp, const uint8_t source) : timestamp_(timestamp), source_(source) { } + /*! + * \brief Creates a new RobotMessage object to be filled from a package. + * + * \param timestamp Timestamp of the package + * \param source The package's source + * \param message_type The package's message type + */ + RobotMessage(const uint64_t timestamp, const int8_t source, const RobotMessagePackageType message_type) + : timestamp_(timestamp), source_(source), message_type_(message_type) + { + } virtual ~RobotMessage() = default; /*! diff --git a/include/ur_client_library/primary/robot_message/error_code_message.h b/include/ur_client_library/primary/robot_message/error_code_message.h new file mode 100644 index 00000000..a4fc63bb --- /dev/null +++ b/include/ur_client_library/primary/robot_message/error_code_message.h @@ -0,0 +1,118 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2020 FZI Forschungszentrum Informatik +// +// Licensed under the Apache License, Text 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. +// -- END LICENSE BLOCK ------------------------------------------------ + +//---------------------------------------------------------------------- +/*!\file + * + * \author Felix Exner exner@fzi.de + * \date 2020-04-23 + * + */ +//---------------------------------------------------------------------- + +#ifndef UR_CLIENT_LIBRARY_PRIMARY_ERROR_CODE_MESSAGE_H_INCLUDED +#define UR_CLIENT_LIBRARY_PRIMARY_ERROR_CODE_MESSAGE_H_INCLUDED + +#include "ur_client_library/primary/robot_message.h" + +namespace urcl +{ +namespace primary_interface +{ +enum class ReportLevel : int32_t +{ + DEBUG = 0, + INFO = 1, + WARNING = 2, + VIOLATION = 3, + FAULT = 4, + DEVL_DEBUG = 128, + DEVL_INFO = 129, + DEVL_WARNING = 130, + DEVL_VIOLATION = 131, + DEVL_FAULT = 132 +}; + +struct ErrorCode +{ + int32_t message_code{ -1 }; + int32_t message_argument{ -1 }; + ReportLevel report_level{ ReportLevel::DEBUG }; + uint32_t data_type{ 0 }; + uint32_t data{ 0 }; + std::string text; + uint64_t timestamp{ 0 }; + std::string to_string; +}; + +/*! + * \brief The ErrorCodeMessage class handles the error code messages sent via the primary UR interface. + */ +class ErrorCodeMessage : public RobotMessage +{ +public: + ErrorCodeMessage() = delete; + /*! + * \brief Creates a new ErrorCodeMessage object to be filled from a package. + * + * \param timestamp Timestamp of the package + * \param source The package's source + */ + ErrorCodeMessage(uint64_t timestamp, int8_t source) + : RobotMessage(timestamp, source, RobotMessagePackageType::ROBOT_MESSAGE_ERROR_CODE) + { + } + virtual ~ErrorCodeMessage() = default; + + /*! + * \brief Sets the attributes of the package by parsing a serialized representation of the + * package. + * + * \param bp A parser containing a serialized text of the package + * + * \returns True, if the package was parsed successfully, false otherwise + */ + virtual bool parseWith(comm::BinParser& bp); + + /*! + * \brief Consume this package with a specific consumer. + * + * \param consumer Placeholder for the consumer calling this + * + * \returns true on success + */ + virtual bool consumeWith(AbstractPrimaryConsumer& consumer); + + /*! + * \brief Produces a human readable representation of the package object. + * + * \returns A string representing the object + */ + virtual std::string toString() const; + + int32_t message_code_; + int32_t message_argument_; + ReportLevel report_level_; + uint32_t data_type_; + uint32_t data_; + std::string text_; +}; +} // namespace primary_interface +} // namespace urcl + +#endif // ifndef UR_CLIENT_LIBRARY_PRIMARY_TEXT_MESSAGE_H_INCLUDED diff --git a/include/ur_client_library/queue/atomicops.h b/include/ur_client_library/queue/atomicops.h index 35059966..e4b24826 100644 --- a/include/ur_client_library/queue/atomicops.h +++ b/include/ur_client_library/queue/atomicops.h @@ -19,23 +19,23 @@ // Platform detection #if defined(__INTEL_COMPILER) -#define AE_ICC +# define AE_ICC #elif defined(_MSC_VER) -#define AE_VCPP +# define AE_VCPP #elif defined(__GNUC__) -#define AE_GCC +# define AE_GCC #endif #if defined(_M_IA64) || defined(__ia64__) -#define AE_ARCH_IA64 +# define AE_ARCH_IA64 #elif defined(_WIN64) || defined(__amd64__) || defined(_M_X64) || defined(__x86_64__) -#define AE_ARCH_X64 +# define AE_ARCH_X64 #elif defined(_M_IX86) || defined(__i386__) -#define AE_ARCH_X86 +# define AE_ARCH_X86 #elif defined(_M_PPC) || defined(__powerpc__) -#define AE_ARCH_PPC +# define AE_ARCH_PPC #else -#define AE_ARCH_UNKNOWN +# define AE_ARCH_UNKNOWN #endif // AE_UNUSED @@ -43,22 +43,22 @@ // AE_FORCEINLINE #if defined(AE_VCPP) || defined(AE_ICC) -#define AE_FORCEINLINE __forceinline +# define AE_FORCEINLINE __forceinline #elif defined(AE_GCC) // #define AE_FORCEINLINE __attribute__((always_inline)) -#define AE_FORCEINLINE inline +# define AE_FORCEINLINE inline #else -#define AE_FORCEINLINE inline +# define AE_FORCEINLINE inline #endif // AE_ALIGN #if defined(AE_VCPP) || defined(AE_ICC) -#define AE_ALIGN(x) __declspec(align(x)) +# define AE_ALIGN(x) __declspec(align(x)) #elif defined(AE_GCC) -#define AE_ALIGN(x) __attribute__((aligned(x))) +# define AE_ALIGN(x) __attribute__((aligned(x))) #else // Assume GCC compliant syntax... -#define AE_ALIGN(x) __attribute__((aligned(x))) +# define AE_ALIGN(x) __attribute__((aligned(x))) #endif // Portable atomic fences implemented below: @@ -83,28 +83,28 @@ enum memory_order #if (defined(AE_VCPP) && (_MSC_VER < 1700 || defined(__cplusplus_cli))) || defined(AE_ICC) // VS2010 and ICC13 don't support std::atomic_*_fence, implement our own fences -#include - -#if defined(AE_ARCH_X64) || defined(AE_ARCH_X86) -#define AeFullSync _mm_mfence -#define AeLiteSync _mm_mfence -#elif defined(AE_ARCH_IA64) -#define AeFullSync __mf -#define AeLiteSync __mf -#elif defined(AE_ARCH_PPC) -#include -#define AeFullSync __sync -#define AeLiteSync __lwsync -#endif - -#ifdef AE_VCPP -#pragma warning(push) -#pragma warning(disable : 4365) // Disable erroneous 'conversion from long to unsigned int, signed/unsigned mismatch' - // error when using `assert` -#ifdef __cplusplus_cli -#pragma managed(push, off) -#endif -#endif +# include + +# if defined(AE_ARCH_X64) || defined(AE_ARCH_X86) +# define AeFullSync _mm_mfence +# define AeLiteSync _mm_mfence +# elif defined(AE_ARCH_IA64) +# define AeFullSync __mf +# define AeLiteSync __mf +# elif defined(AE_ARCH_PPC) +# include +# define AeFullSync __sync +# define AeLiteSync __lwsync +# endif + +# ifdef AE_VCPP +# pragma warning(push) +# pragma warning(disable : 4365) // Disable erroneous 'conversion from long to unsigned int, signed/unsigned + // mismatch' error when using `assert` +# ifdef __cplusplus_cli +# pragma managed(push, off) +# endif +# endif namespace moodycamel { @@ -134,7 +134,7 @@ AE_FORCEINLINE void compilerFence(memory_order order) // x86/x64 have a strong memory model -- all loads and stores have // acquire and release semantics automatically (so only need compiler // barriers for those). -#if defined(AE_ARCH_X86) || defined(AE_ARCH_X64) +# if defined(AE_ARCH_X86) || defined(AE_ARCH_X64) AE_FORCEINLINE void fence(memory_order order) { switch (order) @@ -159,7 +159,7 @@ AE_FORCEINLINE void fence(memory_order order) assert(false); } } -#else +# else AE_FORCEINLINE void fence(memory_order order) { // Non-specialized arch, use heavier memory barriers everywhere just in case :-( @@ -191,11 +191,11 @@ AE_FORCEINLINE void fence(memory_order order) assert(false); } } -#endif +# endif } // end namespace moodycamel #else // Use standard library of atomics -#include +# include namespace moodycamel { @@ -250,11 +250,11 @@ AE_FORCEINLINE void fence(memory_order order) #endif #if !defined(AE_VCPP) || (_MSC_VER >= 1700 && !defined(__cplusplus_cli)) -#define AE_USE_STD_ATOMIC_FOR_WEAK_ATOMIC +# define AE_USE_STD_ATOMIC_FOR_WEAK_ATOMIC #endif #ifdef AE_USE_STD_ATOMIC_FOR_WEAK_ATOMIC -#include +# include #endif #include @@ -272,7 +272,7 @@ class WeakAtomic { } #ifdef AE_VCPP -#pragma warning(disable : 4100) // Get rid of (erroneous) 'unreferenced formal parameter' warning +# pragma warning(disable : 4100) // Get rid of (erroneous) 'unreferenced formal parameter' warning #endif template WeakAtomic(U&& x) : value_(std::forward(x)) @@ -291,7 +291,7 @@ class WeakAtomic { } #ifdef AE_VCPP -#pragma warning(default : 4100) +# pragma warning(default : 4100) #endif AE_FORCEINLINE operator T() const @@ -319,32 +319,32 @@ class WeakAtomic AE_FORCEINLINE T fetchAddAcquire(T increment) { -#if defined(AE_ARCH_X64) || defined(AE_ARCH_X86) +# if defined(AE_ARCH_X64) || defined(AE_ARCH_X86) if (sizeof(T) == 4) return _InterlockedExchangeAdd((long volatile*)&value_, (long)increment); -#if defined(_M_AMD64) +# if defined(_M_AMD64) else if (sizeof(T) == 8) return _InterlockedExchangeAdd64((long long volatile*)&value_, (long long)increment); -#endif -#else -#error Unsupported platform -#endif +# endif +# else +# error Unsupported platform +# endif assert(false && "T must be either a 32 or 64 bit type"); return value_; } AE_FORCEINLINE T fetchAddRelease(T increment) { -#if defined(AE_ARCH_X64) || defined(AE_ARCH_X86) +# if defined(AE_ARCH_X64) || defined(AE_ARCH_X86) if (sizeof(T) == 4) return _InterlockedExchangeAdd((long volatile*)&value_, (long)increment); -#if defined(_M_AMD64) +# if defined(_M_AMD64) else if (sizeof(T) == 8) return _InterlockedExchangeAdd64((long long volatile*)&value_, (long long)increment); -#endif -#else -#error Unsupported platform -#endif +# endif +# else +# error Unsupported platform +# endif assert(false && "T must be either a 32 or 64 bit type"); return value_; } @@ -407,9 +407,9 @@ __declspec(dllimport) unsigned long __stdcall WaitForSingleObject(void* hHandle, __declspec(dllimport) int __stdcall ReleaseSemaphore(void* hSemaphore, long lReleaseCount, long* lpPreviousCount); } #elif defined(__MACH__) -#include +# include #elif defined(__unix__) -#include +# include #endif namespace moodycamel @@ -625,7 +625,7 @@ class Semaphore } }; #else -#error Unsupported platform! (No semaphore wrapper available) +# error Unsupported platform! (No semaphore wrapper available) #endif //--------------------------------------------------------- @@ -731,8 +731,8 @@ class LightweightSemaphore } // end namespace moodycamel #if defined(AE_VCPP) && (_MSC_VER < 1700 || defined(__cplusplus_cli)) -#pragma warning(pop) -#ifdef __cplusplus_cli -#pragma managed(pop) -#endif +# pragma warning(pop) +# ifdef __cplusplus_cli +# pragma managed(pop) +# endif #endif diff --git a/include/ur_client_library/queue/readerwriterqueue.h b/include/ur_client_library/queue/readerwriterqueue.h index e56e4441..ed2d5ba6 100644 --- a/include/ur_client_library/queue/readerwriterqueue.h +++ b/include/ur_client_library/queue/readerwriterqueue.h @@ -13,7 +13,7 @@ #include #include "atomicops.h" #if __cplusplus > 199711L || _MSC_VER >= 1700 // C++11 or VS2012 -#include +# include #endif // A lock-free queue for a single-consumer, single-producer architecture. @@ -30,21 +30,21 @@ // Using the queue exclusively from one thread is fine, though a bit silly. #ifndef MOODYCAMEL_CACHE_LINE_SIZE -#define MOODYCAMEL_CACHE_LINE_SIZE 64 +# define MOODYCAMEL_CACHE_LINE_SIZE 64 #endif #ifndef MOODYCAMEL_EXCEPTIONS_ENABLED -#if (defined(_MSC_VER) && defined(_CPPUNWIND)) || (defined(__GNUC__) && defined(__EXCEPTIONS)) || \ - (!defined(_MSC_VER) && !defined(__GNUC__)) -#define MOODYCAMEL_EXCEPTIONS_ENABLED -#endif +# if (defined(_MSC_VER) && defined(_CPPUNWIND)) || (defined(__GNUC__) && defined(__EXCEPTIONS)) || \ + (!defined(_MSC_VER) && !defined(__GNUC__)) +# define MOODYCAMEL_EXCEPTIONS_ENABLED +# endif #endif #ifdef AE_VCPP -#pragma warning(push) -#pragma warning(disable : 4324) // structure was padded due to __declspec(align()) -#pragma warning(disable : 4820) // padding was added -#pragma warning(disable : 4127) // conditional expression is constant +# pragma warning(push) +# pragma warning(disable : 4324) // structure was padded due to __declspec(align()) +# pragma warning(disable : 4820) // padding was added +# pragma warning(disable : 4127) // conditional expression is constant #endif namespace moodycamel @@ -861,5 +861,5 @@ class BlockingReaderWriterQueue } // end namespace moodycamel #ifdef AE_VCPP -#pragma warning(pop) +# pragma warning(pop) #endif diff --git a/include/ur_client_library/rtde/rtde_client.h b/include/ur_client_library/rtde/rtde_client.h index 17dfd0fa..d94faa5d 100644 --- a/include/ur_client_library/rtde/rtde_client.h +++ b/include/ur_client_library/rtde/rtde_client.h @@ -53,7 +53,6 @@ namespace rtde_interface { static const uint16_t MAX_RTDE_PROTOCOL_VERSION = 2; static const unsigned MAX_REQUEST_RETRIES = 5; -static const unsigned MAX_INITIALIZE_ATTEMPTS = 10; enum class UrRtdeRobotStatusBits { @@ -131,14 +130,21 @@ class RTDEClient * \brief Sets up RTDE communication with the robot. The handshake includes negotiation of the * used protocol version and setting of input and output recipes. * - * \param max_num_tries Maximum number of connection attempts before counting the connection as + * \param max_connection_attempts Maximum number of (socket) connection attempts before counting the connection as * failed. Unlimited number of attempts when set to 0. - * \param reconnection_time time in between connection attempts to the server + * \param reconnection_timeout Time in between connection attempts to the socket + * \param max_initialization_attempts Maximum number of initialization attempts before counting + * the initialization as failed. Initialization can + * fail given an established socket connection e.g. when the connected socket does not implement + * an RTDE interface. + * \param initialization_timeout Time in between initialization attempts of the RTDE interface * * \returns Success of the handshake */ - bool init(const size_t max_num_tries = 0, - const std::chrono::milliseconds reconnection_time = std::chrono::seconds(10)); + bool init(const size_t max_connection_attempts = 0, + const std::chrono::milliseconds reconnection_timeout = comm::TCPSocket::DEFAULT_RECONNECTION_TIME, + const size_t max_initialization_attempts = 3, + const std::chrono::milliseconds initialization_timeout = std::chrono::seconds(1)); /*! * \brief Triggers the robot to start sending RTDE data packages in the negotiated format. * diff --git a/include/ur_client_library/types.h b/include/ur_client_library/types.h index 7a9cbbca..9d0f230b 100644 --- a/include/ur_client_library/types.h +++ b/include/ur_client_library/types.h @@ -31,6 +31,23 @@ using vector6d_t = std::array; using vector6int32_t = std::array; using vector6uint32_t = std::array; +struct Pose +{ + Pose() : x(0.0), y(0.0), z(0.0), rx(0.0), ry(0.0), rz(0.0) + { + } + Pose(const double x, const double y, const double z, const double rx, const double ry, const double rz) + : x(x), y(y), z(z), rx(rx), ry(ry), rz(rz) + { + } + double x; + double y; + double z; + double rx; + double ry; + double rz; +}; + template std::ostream& operator<<(std::ostream& out, const std::array& item) { @@ -59,4 +76,4 @@ constexpr typename std::underlying_type::type toUnderlying(const E e) noexcep { return static_cast::type>(e); } -} // namespace urcl +} // namespace urcl \ No newline at end of file diff --git a/include/ur_client_library/ur/dashboard_client.h b/include/ur_client_library/ur/dashboard_client.h index 6d4286ed..953598e5 100644 --- a/include/ur_client_library/ur/dashboard_client.h +++ b/include/ur_client_library/ur/dashboard_client.h @@ -89,7 +89,7 @@ class DashboardClient : public comm::TCPSocket * \brief Sends command and compare it with the expected answer * * \param command Command that will be sent to the server. - * \param expected Expected response + * \param expected Expected response as a regex string * * \return True if the reply to the command is as expected */ @@ -99,7 +99,7 @@ class DashboardClient : public comm::TCPSocket * \brief Sends command and compare it with the expected answer * * \param command Command that will be sent to the server. - * \param expected Expected response + * \param expected Expected response as a regex string * * \throws UrException if the received answer does not match the expected one. * diff --git a/include/ur_client_library/ur/instruction_executor.h b/include/ur_client_library/ur/instruction_executor.h new file mode 100644 index 00000000..b4050980 --- /dev/null +++ b/include/ur_client_library/ur/instruction_executor.h @@ -0,0 +1,105 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2025 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#ifndef UR_CLIENT_LIBRARY_INSTRUCTION_EXECUTOR_H_INCLUDED +#define UR_CLIENT_LIBRARY_INSTRUCTION_EXECUTOR_H_INCLUDED + +#include "ur_client_library/ur/ur_driver.h" +#include "ur_client_library/control/motion_primitives.h" + +namespace urcl +{ +class InstructionExecutor +{ +public: + InstructionExecutor() = delete; + InstructionExecutor(std::shared_ptr driver) : driver_(driver) + { + driver_->registerTrajectoryDoneCallback( + std::bind(&InstructionExecutor::trajDoneCallback, this, std::placeholders::_1)); + driver_->registerTrajectoryInterfaceDisconnectedCallback( + std::bind(&InstructionExecutor::trajDisconnectCallback, this, std::placeholders::_1)); + } + + /** + * \brief Execute a sequence of motion primitives. + * + * This function will execute a sequence of motion primitives. The robot will move according to the given motion + * primitives. The function will return once the robot has reached the final target. + * + * \param motion_sequence The sequence of motion primitives to execute + */ + bool executeMotion(const std::vector>& motion_sequence); + + /** + * \brief Move the robot to a joint target. + * + * This function will move the robot to the given joint target. The robot will move with the given acceleration and + * velocity. The function will return once the robot has reached the target. + * + * \param target The joint target to move to. + * \param acceleration Joint acceleration of leading axis [rad/s^2] + * \param velocity Joint speed of leading axis [rad/s] + * \param time The time to reach the target. If set to 0, the robot will move with the given acceleration and + * velocity. + * \param blend_radius The blend radius to use for the motion. + * \return True if the robot has reached the target, false otherwise. + */ + bool moveJ(const urcl::vector6d_t& target, const double acceleration = 1.4, const double velocity = 1.04, + const double time = 0, const double blend_radius = 0); + + /** + * \brief Move the robot to a pose target. + * + * This function will move the robot to the given pose target. The robot will move with the given acceleration and + * velocity. The function will return once the robot has reached the target. + * + * \param target The pose target to move to. + * \param acceleration Tool acceleration [m/s^2] + * \param velocity Tool speed [m/s] + * \param time The time to reach the target. If set to 0, the robot will move with the given acceleration and + * velocity. + * \param blend_radius The blend radius to use for the motion. + * \return True if the robot has reached the target, false otherwise. + */ + bool moveL(const urcl::Pose& target, const double acceleration = 1.4, const double velocity = 1.04, + const double time = 0, const double blend_radius = 0); + +private: + void trajDoneCallback(const urcl::control::TrajectoryResult& result); + void trajDisconnectCallback(const int filedescriptor); + std::shared_ptr driver_; + std::atomic trajectory_running_ = false; + std::mutex trajectory_result_mutex_; + urcl::control::TrajectoryResult trajectory_result_; +}; +} // namespace urcl + +#endif // UR_CLIENT_LIBRARY_INSTRUCTION_EXECUTOR_H_INCLUDED \ No newline at end of file diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index e9c6395b..79ead088 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -28,6 +28,7 @@ #ifndef UR_CLIENT_LIBRARY_UR_UR_DRIVER_H_INCLUDED #define UR_CLIENT_LIBRARY_UR_UR_DRIVER_H_INCLUDED +#include #include #include "ur_client_library/rtde/rtde_client.h" @@ -38,11 +39,111 @@ #include "ur_client_library/ur/tool_communication.h" #include "ur_client_library/ur/version_information.h" #include "ur_client_library/ur/robot_receive_timeout.h" +#include "ur_client_library/primary/primary_client.h" #include "ur_client_library/primary/robot_message/version_message.h" #include "ur_client_library/rtde/rtde_writer.h" namespace urcl { +/*! + * \brief Structure for configuration parameters of a UrDriver object. + */ +struct UrDriverConfiguration +{ + std::string robot_ip; //!< IP-address under which the robot is reachable. + std::string script_file; //!< URScript file that should be sent to the robot. + std::string output_recipe_file; //!< Filename where the output recipe is stored in. + std::string input_recipe_file; //!< Filename where the input recipe is stored in. + + /*! + * \brief Function handle to a callback on program state changes. + * + * For this to work, the URScript program will have to send keepalive signals to the \p + * reverse_port. + */ + std::function handle_program_state; + bool headless_mode; //!< Parameter to control if the driver should be started in headless mode. + + std::unique_ptr tool_comm_setup = nullptr; //!< Configuration for using the tool communication. + + /*! + * \brief Port that will be opened by the driver to allow direct communication between the driver + * and the robot controller. + */ + uint32_t reverse_port = 50001; + + /*! \brief The driver will offer an interface to receive the program's URScript on this port. If + * the robot cannot connect to this port, `External Control` will stop immediately. + */ + uint32_t script_sender_port = 50002; + + /*! + * \brief Port used for sending trajectory points to the robot in case of trajectory forwarding. + */ + uint32_t trajectory_port = 50003; + + /*! + * \brief Port used for forwarding script commands to the robot. + * + * This interface supports a set of predefined commands. + * The script commands will be executed locally on the robot. + */ + uint32_t script_command_port = 50004; + + /*! + * \brief IP address that the reverse_port will get bound to. + * + * If not specified, the IP address of the interface that is used for connecting to the robot's RTDE port will be + * used. + */ + std::string reverse_ip = ""; + + /*! + * \brief Proportional gain for arm joints following target position, range [100,2000] + */ + int servoj_gain = 2000; + + /*! + * \brief Time [S], range [0.03,0.2] smoothens servoj calls with this lookahead time + */ + double servoj_lookahead_time = 0.03; + + /*! + * \brief Number of attempts to reconnect to sockets such as the primary or RTDE interface. + * + * If set to 0, the driver will try to reconnect indefinitely. + */ + size_t socket_reconnect_attempts = 0; + + /*! + * \brief Time in between connection attempts to sockets such as the primary or RTDE interface. + */ + std::chrono::milliseconds socket_reconnection_timeout = std::chrono::seconds(10); + + /*! + * \brief Number of attempts to initialize (given a successful socket connection) the RTDE interface. + * + * If set to 0, the driver will try to initialize the RTDE interface indefinitely. + */ + size_t rtde_initialization_attempts_ = 3; + + /*! + * \brief Time in between initialization attempts of the RTDE interface. + */ + std::chrono::milliseconds rtde_initialization_timeout_ = std::chrono::seconds(5); + + bool non_blocking_read = false; + + // TODO: Remove on 2027-05 + // The following parameters are considered deprecated and will be removed in May 2027. + /// @private + std::string calibration_checksum = ""; + /// @private + double force_mode_damping = 0.025; + /// @private + double force_mode_gain_scaling = 0.5; +}; + /*! * \brief This is the main class for interfacing the driver. * @@ -55,6 +156,25 @@ class UrDriver public: /*! * \brief Constructs a new UrDriver object. + * + * An RTDE connection to the robot will be established using the given recipe files. However, RTDE + * communication will not be started automatically, as this requires an external structure to read + * data from the RTDE client using the getDataPackage() method periodically. Once this is setup, + * please use the startRTDECommunication() method to actually start RTDE communication. + * + * \param config Configuration struct for the UrDriver. See it's documentation for details. + */ + explicit UrDriver(const UrDriverConfiguration& config) + { + init(config); + } + + /*! + * \brief Constructs a new UrDriver object. + * + * \deprecated Initializing a UrDriver object with an argument list is deprecated. Please use UrDriver(const + * UrDriverConfiguration& config) instead. This function will be removed in May 2027. + * * Upon initialization this class will check the calibration checksum reported from the robot and * compare it to a checksum given by the user. If the checksums don't match, the driver will output * an error message. This is critical if you want to do forward or inverse kinematics based on the @@ -93,15 +213,41 @@ class UrDriver * \param script_command_port Port used for forwarding script commands to the robot. The script commands will be * executed locally on the robot. */ + // Called sigA in tests + [[deprecated("Initializing a UrDriver object with an argument list is deprecated. Please use UrDriver(const " + "UrDriverConfiguration& config) instead. This function will be removed in May 2027.")]] UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& output_recipe_file, const std::string& input_recipe_file, std::function handle_program_state, bool headless_mode, std::unique_ptr tool_comm_setup, const uint32_t reverse_port = 50001, const uint32_t script_sender_port = 50002, int servoj_gain = 2000, double servoj_lookahead_time = 0.03, bool non_blocking_read = false, const std::string& reverse_ip = "", const uint32_t trajectory_port = 50003, - const uint32_t script_command_port = 50004); + const uint32_t script_command_port = 50004) + { + UrDriverConfiguration config; + config.robot_ip = robot_ip; + config.script_file = script_file; + config.output_recipe_file = output_recipe_file; + config.input_recipe_file = input_recipe_file; + config.handle_program_state = handle_program_state; + config.headless_mode = headless_mode; + config.tool_comm_setup = std::move(tool_comm_setup); + config.reverse_port = reverse_port; + config.script_sender_port = script_sender_port; + config.servoj_gain = servoj_gain; + config.servoj_lookahead_time = servoj_lookahead_time; + config.non_blocking_read = non_blocking_read; + config.reverse_ip = reverse_ip; + config.trajectory_port = trajectory_port; + config.script_command_port = script_command_port; + init(config); + } /*! * \brief Constructs a new UrDriver object. + * + * \deprecated Initializing a UrDriver object with an argument list is deprecated. Please use UrDriver(const + * UrDriverConfiguration& config) instead. This function will be removed in May 2027. + * * \param robot_ip IP-address under which the robot is reachable. * \param script_file URScript file that should be sent to the robot. * \param output_recipe_file Filename where the output recipe is stored in. @@ -128,28 +274,43 @@ class UrDriver * \param force_mode_damping The damping parameter used when the robot is in force mode, range [0,1] * \param force_mode_gain_scaling Scales the gain used when the robot is in force mode, range [0,2] (only e-series) */ - [[deprecated( - "Specifying the force mode damping factor and the force mode gain scaling factor at driver creation has " - "been deprecated. Force mode parameters should be specified with each activiation of force mode, and " - "can be set in the function call to start force mode.")]] UrDriver(const std::string& robot_ip, - const std::string& script_file, - const std::string& output_recipe_file, - const std::string& input_recipe_file, - std::function handle_program_state, - bool headless_mode, - std::unique_ptr tool_comm_setup, - const uint32_t reverse_port, - const uint32_t script_sender_port, - int servoj_gain, double servoj_lookahead_time, - bool non_blocking_read, - const std::string& reverse_ip, - const uint32_t trajectory_port, - const uint32_t script_command_port, - double force_mode_damping, - double force_mode_gain_scaling = 0.5); + // Called sigB in tests + [[deprecated("Initializing a UrDriver object with an argument list is deprecated. Please use UrDriver(const " + "UrDriverConfiguration& config) instead. This function will be removed in May 2027.")]] + UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& output_recipe_file, + const std::string& input_recipe_file, std::function handle_program_state, bool headless_mode, + std::unique_ptr tool_comm_setup, const uint32_t reverse_port, + const uint32_t script_sender_port, int servoj_gain, double servoj_lookahead_time, bool non_blocking_read, + const std::string& reverse_ip, const uint32_t trajectory_port, const uint32_t script_command_port, + double force_mode_damping, double force_mode_gain_scaling = 0.5) + { + UrDriverConfiguration config; + config.robot_ip = robot_ip; + config.script_file = script_file; + config.output_recipe_file = output_recipe_file; + config.input_recipe_file = input_recipe_file; + config.handle_program_state = handle_program_state; + config.headless_mode = headless_mode; + config.tool_comm_setup = std::move(tool_comm_setup); + config.reverse_port = reverse_port; + config.script_sender_port = script_sender_port; + config.servoj_gain = servoj_gain; + config.servoj_lookahead_time = servoj_lookahead_time; + config.non_blocking_read = non_blocking_read; + config.reverse_ip = reverse_ip; + config.trajectory_port = trajectory_port; + config.script_command_port = script_command_port; + config.force_mode_damping = force_mode_damping; + config.force_mode_gain_scaling = force_mode_gain_scaling; + init(config); + } /*! * \brief Constructs a new UrDriver object. + * + * \deprecated Initializing a UrDriver object with an argument list is deprecated. Please use UrDriver(const + * UrDriverConfiguration& config) instead. This function will be removed in May 2027. + * * \param robot_ip IP-address under which the robot is reachable. * \param script_file URScript file that should be sent to the robot. * \param output_recipe_file Filename where the output recipe is stored in. @@ -177,16 +338,44 @@ class UrDriver * \param force_mode_damping The damping parameter used when the robot is in force mode, range [0,1] * \param force_mode_gain_scaling Scales the gain used when the robot is in force mode, range [0,2] (only e-series) */ + // Called sigC in tests + [[deprecated("Initializing a UrDriver object with an argument list is deprecated. Please use UrDriver(const " + "UrDriverConfiguration& config) instead. This function will be removed in May 2027.")]] UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& output_recipe_file, const std::string& input_recipe_file, std::function handle_program_state, bool headless_mode, std::unique_ptr tool_comm_setup, const std::string& calibration_checksum, const uint32_t reverse_port = 50001, const uint32_t script_sender_port = 50002, int servoj_gain = 2000, double servoj_lookahead_time = 0.03, bool non_blocking_read = false, const std::string& reverse_ip = "", const uint32_t trajectory_port = 50003, const uint32_t script_command_port = 50004, - double force_mode_damping = 0.025, double force_mode_gain_scaling = 0.5); + double force_mode_damping = 0.025, double force_mode_gain_scaling = 0.5) + { + UrDriverConfiguration config; + config.robot_ip = robot_ip; + config.script_file = script_file; + config.output_recipe_file = output_recipe_file; + config.input_recipe_file = input_recipe_file; + config.handle_program_state = handle_program_state; + config.headless_mode = headless_mode; + config.calibration_checksum = calibration_checksum; + config.tool_comm_setup = std::move(tool_comm_setup); + config.reverse_port = reverse_port; + config.script_sender_port = script_sender_port; + config.servoj_gain = servoj_gain; + config.servoj_lookahead_time = servoj_lookahead_time; + config.non_blocking_read = non_blocking_read; + config.reverse_ip = reverse_ip; + config.trajectory_port = trajectory_port; + config.script_command_port = script_command_port; + config.force_mode_damping = force_mode_damping; + config.force_mode_gain_scaling = force_mode_gain_scaling; + init(config); + } /*! * \brief Constructs a new UrDriver object. * + * \deprecated Initializing a UrDriver object with an argument list is deprecated. Please use UrDriver(const + * UrDriverConfiguration& config) instead. This function will be removed in May 2027. + * * \param robot_ip IP-address under which the robot is reachable. * \param script_file URScript file that should be sent to the robot. * \param output_recipe_file Filename where the output recipe is stored in. @@ -213,6 +402,9 @@ class UrDriver * \param force_mode_damping The damping parameter used when the robot is in force mode, range [0,1] * \param force_mode_gain_scaling Scales the gain used when the robot is in force mode, range [0,2] (only e-series) */ + // Called sigD in tests + [[deprecated("Initializing a UrDriver object with an argument list is deprecated. Please use UrDriver(const " + "UrDriverConfiguration& config) instead. This function will be removed in May 2027.")]] UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& output_recipe_file, const std::string& input_recipe_file, std::function handle_program_state, bool headless_mode, const std::string& calibration_checksum = "", const uint32_t reverse_port = 50001, @@ -220,11 +412,26 @@ class UrDriver bool non_blocking_read = false, const std::string& reverse_ip = "", const uint32_t trajectory_port = 50003, const uint32_t script_command_port = 50004, double force_mode_damping = 0.025, double force_mode_gain_scaling = 0.5) - : UrDriver(robot_ip, script_file, output_recipe_file, input_recipe_file, handle_program_state, headless_mode, - std::unique_ptr{}, calibration_checksum, reverse_port, script_sender_port, servoj_gain, - servoj_lookahead_time, non_blocking_read, reverse_ip, trajectory_port, script_command_port, - force_mode_damping, force_mode_gain_scaling) { + UrDriverConfiguration config; + config.robot_ip = robot_ip; + config.script_file = script_file; + config.output_recipe_file = output_recipe_file; + config.input_recipe_file = input_recipe_file; + config.handle_program_state = handle_program_state; + config.headless_mode = headless_mode; + config.calibration_checksum = calibration_checksum; + config.reverse_port = reverse_port; + config.script_sender_port = script_sender_port; + config.servoj_gain = servoj_gain; + config.servoj_lookahead_time = servoj_lookahead_time; + config.non_blocking_read = non_blocking_read; + config.reverse_ip = reverse_ip; + config.trajectory_port = trajectory_port; + config.script_command_port = script_command_port; + config.force_mode_damping = force_mode_damping; + config.force_mode_gain_scaling = force_mode_gain_scaling; + init(config); } virtual ~UrDriver() = default; @@ -534,6 +741,15 @@ class UrDriver */ bool checkCalibration(const std::string& checksum); + /*! + * \brief Retrieves previously raised error codes from PrimaryClient. After calling this, recorded errors will be + * deleted. + * + * \returns list of error codes + * + */ + std::deque getErrorCodes(); + /*! * \brief Getter for the RTDE writer used to write to the robot's RTDE interface. * @@ -647,13 +863,26 @@ class UrDriver void resetRTDEClient(const std::string& output_recipe_filename, const std::string& input_recipe_filename, double target_frequency = 0.0, bool ignore_unavailable_outputs = false); + /*! + * \brief Starts the primary client + */ + void startPrimaryClientCommunication(); + + void registerTrajectoryInterfaceDisconnectedCallback(std::function fun) + { + trajectory_interface_->registerDisconnectionCallback(fun); + } + static std::string readScriptFile(const std::string& filename); -protected: - std::unique_ptr> primary_stream_; - std::unique_ptr> secondary_stream_; + void closeSecondaryStream() + { + secondary_stream_->close(); + } private: + void init(const UrDriverConfiguration& config); + /*! * \brief Reconnects the secondary stream used to send program to the robot. * @@ -668,10 +897,19 @@ class UrDriver comm::INotifier notifier_; std::unique_ptr rtde_client_; + std::unique_ptr primary_client_; std::unique_ptr reverse_interface_; std::unique_ptr trajectory_interface_; std::unique_ptr script_command_interface_; std::unique_ptr script_sender_; + std::unique_ptr> primary_stream_; + std::unique_ptr> secondary_stream_; + + size_t socket_connection_attempts_ = 0; + std::chrono::milliseconds socket_reconnection_timeout_ = std::chrono::milliseconds(10000); + + size_t rtde_initialization_attempts_ = 0; + std::chrono::milliseconds rtde_initialization_timeout_ = std::chrono::milliseconds(10000); double force_mode_gain_scale_factor_ = 0.5; double force_mode_damping_factor_ = 0.025; diff --git a/package.xml b/package.xml index f9d137cf..3ab8fd65 100644 --- a/package.xml +++ b/package.xml @@ -2,7 +2,7 @@ ur_client_library - 1.5.0 + 1.6.0 Standalone C++ library for accessing Universal Robots interfaces. This has been forked off the ur_robot_driver. Thomas Timm Andersen Simon Rasmussen diff --git a/resources/external_control.urscript b/resources/external_control.urscript index 2bc3ab93..8830ca8c 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -148,10 +148,8 @@ def set_servo_setpoint(q): end def extrapolate(): - diff = [cmd_servo_q[0] - cmd_servo_q_last[0], cmd_servo_q[1] - cmd_servo_q_last[1], cmd_servo_q[2] - cmd_servo_q_last[2], cmd_servo_q[3] - cmd_servo_q_last[3], cmd_servo_q[4] - cmd_servo_q_last[4], cmd_servo_q[5] - cmd_servo_q_last[5]] cmd_servo_q_last = cmd_servo_q - cmd_servo_q = [cmd_servo_q[0] + diff[0], cmd_servo_q[1] + diff[1], cmd_servo_q[2] + diff[2], cmd_servo_q[3] + diff[3], cmd_servo_q[4] + diff[4], cmd_servo_q[5] + diff[5]] - + cmd_servo_q = cmd_servo_q + (get_target_joint_speeds() * steptime) return cmd_servo_q end diff --git a/run_examples.sh b/run_examples.sh index 72edb232..2b91e3de 100755 --- a/run_examples.sh +++ b/run_examples.sh @@ -13,13 +13,14 @@ for file in $folder_path/*; do echo $file # Check if the file is executable if [[ -f "$file" && -x "$file" ]]; then + printf "\n#### Executing '$file'\n" # Execute the file ./"$file" $@ # Check the exit status exit_status=$? if [[ $exit_status -ne 0 ]]; then - echo "Execution of '$file' failed with exit status $exit_status." - exit 1 + echo "Execution of '$file' failed with exit status $exit_status." + exit 1 fi # Delay for 10 seconds to avoid too fast reconnects echo "Sleep 10" diff --git a/scripts/start_ursim.sh b/scripts/start_ursim.sh index 0a72f339..014dc315 100755 --- a/scripts/start_ursim.sh +++ b/scripts/start_ursim.sh @@ -51,7 +51,7 @@ help() If not specified, will fallback to ${PERSISTENT_BASE}/${ROBOT_SERIES}/urcaps" echo " -n Name of the docker container. Defaults to '$CONTAINER_NAME'" echo " -i IP address the container should get. Defaults to $IP_ADDRESS" - echo " -d Detached mode - start in backgound" + echo " -d Detached mode - start in background" echo " -f Specify port forwarding to use. Defaults to '$PORT_FORWARDING'. Set to empty string to disable port forwarding." echo " -h Print this Help." echo diff --git a/src/comm/tcp_server.cpp b/src/comm/tcp_server.cpp index f143fd28..cb58252f 100644 --- a/src/comm/tcp_server.cpp +++ b/src/comm/tcp_server.cpp @@ -37,7 +37,6 @@ #include #include - namespace urcl { namespace comm diff --git a/src/comm/tcp_socket.cpp b/src/comm/tcp_socket.cpp index bce8f963..778e5f16 100644 --- a/src/comm/tcp_socket.cpp +++ b/src/comm/tcp_socket.cpp @@ -27,8 +27,8 @@ #include #ifndef _WIN32 -#include -#include +# include +# include #endif #include "ur_client_library/log.h" @@ -38,12 +38,13 @@ namespace urcl { namespace comm { -TCPSocket::TCPSocket() : socket_fd_(INVALID_SOCKET), state_(SocketState::Invalid), reconnection_time_(std::chrono::seconds(10)) +TCPSocket::TCPSocket() + : socket_fd_(INVALID_SOCKET), state_(SocketState::Invalid), reconnection_time_(std::chrono::seconds(10)) { #ifdef _WIN32 WSAData data; ::WSAStartup(MAKEWORD(1, 1), &data); -#endif // _WIN32 +#endif // _WIN32 } TCPSocket::~TCPSocket() { @@ -121,25 +122,24 @@ bool TCPSocket::setup(const std::string& host, const int port, const size_t max_ freeaddrinfo(result); - if (max_num_tries > 0) + if (!connected) { - if (connect_counter++ >= max_num_tries) + state_ = SocketState::Invalid; + if (++connect_counter >= max_num_tries && max_num_tries > 0) { URCL_LOG_ERROR("Failed to establish connection for %s:%d after %d tries", host.c_str(), port, max_num_tries); - state_ = SocketState::Invalid; return false; } - } - - if (!connected) - { - state_ = SocketState::Invalid; - std::stringstream ss; - ss << "Failed to connect to robot on IP " << host_name - << ". Please check that the robot is booted and reachable on " << host_name << ". Retrying in " - << std::chrono::duration_cast>(reconnection_time_resolved).count() << " seconds"; - URCL_LOG_ERROR("%s", ss.str().c_str()); - std::this_thread::sleep_for(reconnection_time_resolved); + else + { + std::stringstream ss; + ss << "Failed to connect to robot on IP " << host_name << ":" << port + << ". Please check that the robot is booted and reachable on " << host_name << ". Retrying in " + << std::chrono::duration_cast>(reconnection_time_resolved).count() + << " seconds."; + URCL_LOG_ERROR("%s", ss.str().c_str()); + std::this_thread::sleep_for(reconnection_time_resolved); + } } } setupOptions(); @@ -191,7 +191,11 @@ bool TCPSocket::read(uint8_t* buf, const size_t buf_len, size_t& read) if (state_ != SocketState::Connected) return false; +#ifdef _WIN32 ssize_t res = ::recv(socket_fd_, reinterpret_cast(buf), static_cast(buf_len), 0); +#else + ssize_t res = ::recv(socket_fd_, buf, buf_len, 0); +#endif if (res == 0) { @@ -236,7 +240,8 @@ bool TCPSocket::write(const uint8_t* buf, const size_t buf_len, size_t& written) // handle partial sends while (written < buf_len) { - ssize_t sent = ::send(socket_fd_, reinterpret_cast(buf + written), static_cast(remaining), 0); + ssize_t sent = + ::send(socket_fd_, reinterpret_cast(buf + written), static_cast(remaining), 0); if (sent <= 0) { diff --git a/src/control/trajectory_point_interface.cpp b/src/control/trajectory_point_interface.cpp index ce93c31d..4c280395 100644 --- a/src/control/trajectory_point_interface.cpp +++ b/src/control/trajectory_point_interface.cpp @@ -29,11 +29,30 @@ #include #include #include +#include namespace urcl { namespace control { + +std::string trajectoryResultToString(const TrajectoryResult result) +{ + switch (result) + { + case TrajectoryResult::TRAJECTORY_RESULT_UNKNOWN: + return "UNKNOWN"; + case TrajectoryResult::TRAJECTORY_RESULT_SUCCESS: + return "SUCCESS"; + case TrajectoryResult::TRAJECTORY_RESULT_CANCELED: + return "CANCELED"; + case TrajectoryResult::TRAJECTORY_RESULT_FAILURE: + return "FAILURE"; + default: + throw std::invalid_argument("Illegal Trajectory result"); + } +} + TrajectoryPointInterface::TrajectoryPointInterface(uint32_t port) : ReverseInterface(port, [](bool foo) { return foo; }) { } @@ -197,8 +216,12 @@ void TrajectoryPointInterface::connectionCallback(const socket_t filedescriptor) void TrajectoryPointInterface::disconnectionCallback(const socket_t filedescriptor) { - URCL_LOG_DEBUG("Connection to trajectory interface dropped.", filedescriptor); + URCL_LOG_DEBUG("Connection to trajectory interface dropped."); client_fd_ = -1; + if (disconnection_callback_ != nullptr) + { + disconnection_callback_(filedescriptor); + } } void TrajectoryPointInterface::messageCallback(const socket_t filedescriptor, char* buffer, int nbytesrecv) diff --git a/src/example_robot_wrapper.cpp b/src/example_robot_wrapper.cpp new file mode 100644 index 00000000..8f405c5d --- /dev/null +++ b/src/example_robot_wrapper.cpp @@ -0,0 +1,289 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2025 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#include +#include +#include "ur_client_library/exceptions.h" +#include "ur_client_library/log.h" + +namespace urcl +{ +ExampleRobotWrapper::ExampleRobotWrapper(const std::string& robot_ip, const std::string& output_recipe_file, + const std::string& input_recipe_file, const bool headless_mode, + const std::string& autostart_program, const std::string& script_file) + : headless_mode_(headless_mode), autostart_program_(autostart_program) +{ + dashboard_client_ = std::make_shared(robot_ip); + + // Connect the robot Dashboard + if (!dashboard_client_->connect()) + { + URCL_LOG_ERROR("Could not connect to dashboard"); + } + + // In CI we the dashboard client times out for no obvious reason. Hence we increase the timeout + // here. + timeval tv; + tv.tv_sec = 10; + tv.tv_usec = 0; + dashboard_client_->setReceiveTimeout(tv); + + if (!initializeRobotWithDashboard()) + { + throw UrException("Could not initialize robot with dashboard"); + } + + UrDriverConfiguration driver_config; + driver_config.robot_ip = robot_ip; + driver_config.script_file = script_file; + driver_config.output_recipe_file = output_recipe_file; + driver_config.input_recipe_file = input_recipe_file; + driver_config.handle_program_state = + std::bind(&ExampleRobotWrapper::handleRobotProgramState, this, std::placeholders::_1); + driver_config.headless_mode = headless_mode; + ur_driver_ = std::make_shared(driver_config); + + if (!headless_mode && !std::empty(autostart_program)) + { + startRobotProgram(autostart_program); + } + + if (headless_mode | !std::empty(autostart_program)) + { + if (!waitForProgramRunning(500)) + { + throw UrException("Program did not start running. Is the robot in remote control?"); + } + } +} + +ExampleRobotWrapper::~ExampleRobotWrapper() +{ + if (rtde_communication_started_) + { + stopConsumingRTDEData(); + } +} + +bool ExampleRobotWrapper::clearProtectiveStop() +{ + std::string safety_status; + dashboard_client_->commandSafetyStatus(safety_status); + bool is_protective_stopped = safety_status.find("PROTECTIVE_STOP") != std::string::npos; + if (is_protective_stopped) + { + URCL_LOG_INFO("Robot is in protective stop, trying to release it"); + dashboard_client_->commandClosePopup(); + dashboard_client_->commandCloseSafetyPopup(); + if (!dashboard_client_->commandUnlockProtectiveStop()) + { + std::this_thread::sleep_for(std::chrono::seconds(5)); + if (!dashboard_client_->commandUnlockProtectiveStop()) + { + URCL_LOG_ERROR("Could not unlock protective stop"); + return false; + } + } + } + return true; +} + +bool ExampleRobotWrapper::initializeRobotWithDashboard() +{ + if (!clearProtectiveStop()) + { + URCL_LOG_ERROR("Could not clear protective stop"); + return false; + } + + // Stop program, if there is one running + if (!dashboard_client_->commandStop()) + { + URCL_LOG_ERROR("Could not send stop program command"); + return false; + } + + // Power it off + if (!dashboard_client_->commandPowerOff()) + { + URCL_LOG_ERROR("Could not send Power off command"); + return false; + } + + // Power it on + if (!dashboard_client_->commandPowerOn()) + { + URCL_LOG_ERROR("Could not send Power on command"); + return false; + } + + // Release the brakes + if (!dashboard_client_->commandBrakeRelease()) + { + URCL_LOG_ERROR("Could not send BrakeRelease command"); + return false; + } + + // Now the robot is ready to receive a program + URCL_LOG_INFO("Robot ready to start a program"); + robot_initialized_ = true; + return true; +} + +void ExampleRobotWrapper::handleRobotProgramState(bool program_running) +{ + // Print the text in green so we see it better + std::cout << "\033[1;32mProgram running: " << std::boolalpha << program_running << "\033[0m\n" << std::endl; + if (program_running) + { + std::lock_guard lk(program_running_mutex_); + program_running_ = program_running; + program_running_cv_.notify_one(); + } + else + { + std::lock_guard lk(program_not_running_mutex_); + program_running_ = program_running; + program_not_running_cv_.notify_one(); + } +} + +void ExampleRobotWrapper::startRTDECommununication(const bool consume_data) +{ + if (!rtde_communication_started_) + { + ur_driver_->startRTDECommunication(); + rtde_communication_started_ = true; + } + if (consume_data) + { + startConsumingRTDEData(); + } +} + +void ExampleRobotWrapper::startConsumingRTDEData() +{ + consume_rtde_packages_ = true; + rtde_consumer_thread_ = std::thread([this]() { + while (consume_rtde_packages_) + { + // Consume package to prevent pipeline overflow + std::lock_guard lk(read_package_mutex_); + data_pkg_ = ur_driver_->getDataPackage(); + } + }); +} + +void ExampleRobotWrapper::stopConsumingRTDEData() +{ + if (consume_rtde_packages_) + { + consume_rtde_packages_ = false; + if (rtde_consumer_thread_.joinable()) + { + rtde_consumer_thread_.join(); + } + } +} + +bool ExampleRobotWrapper::readDataPackage(std::unique_ptr& data_pkg) +{ + if (consume_rtde_packages_ == true) + { + URCL_LOG_ERROR("Unable to read packages while consuming, this should not happen!"); + return false; + } + std::lock_guard lk(read_package_mutex_); + data_pkg = ur_driver_->getDataPackage(); + if (data_pkg == nullptr) + { + URCL_LOG_ERROR("Timed out waiting for a new package from the robot"); + return false; + } + return true; +} + +bool ExampleRobotWrapper::waitForProgramRunning(int milliseconds) +{ + std::unique_lock lk(program_running_mutex_); + if (program_running_cv_.wait_for(lk, std::chrono::milliseconds(milliseconds)) == std::cv_status::no_timeout || + program_running_ == true) + { + return true; + } + return false; +} + +bool ExampleRobotWrapper::waitForProgramNotRunning(int milliseconds) +{ + std::unique_lock lk(program_not_running_mutex_); + if (program_not_running_cv_.wait_for(lk, std::chrono::milliseconds(milliseconds)) == std::cv_status::no_timeout || + program_running_ == false) + { + return true; + } + return false; +} + +bool ExampleRobotWrapper::startRobotProgram(const std::string& program_file_name) +{ + if (!dashboard_client_->commandLoadProgram(program_file_name)) + { + URCL_LOG_ERROR("Could not load program '%s'", program_file_name.c_str()); + return false; + } + + return dashboard_client_->commandPlay(); +} +bool ExampleRobotWrapper::resendRobotProgram() +{ + if (headless_mode_) + { + return ur_driver_->sendRobotProgram(); + } + return startRobotProgram(autostart_program_); +} + +bool ExampleRobotWrapper::isHealthy() const +{ + if (!robot_initialized_) + { + URCL_LOG_ERROR("Robot is not initialized"); + return false; + } + if (!program_running_) + { + URCL_LOG_ERROR("Robot program is not running"); + return false; + } + return true; +} + +} // namespace urcl diff --git a/src/helpers.cpp b/src/helpers.cpp index d08bd9cb..223ca431 100644 --- a/src/helpers.cpp +++ b/src/helpers.cpp @@ -33,13 +33,19 @@ #include #include +// clang-format off +// We want to keep the URL in one line to avoid formatting issues. This will make it easier to +// extract the URL for an automatic check. +const std::string RT_DOC_URL = "https://docs.universal-robots.com/Universal_Robots_ROS_Documentation/doc/ur_client_library/doc/real_time.html"; +// clang-format on + namespace urcl { bool setFiFoScheduling(pthread_t& thread, const int priority) { #ifdef _WIN32 return ::SetThreadPriority(thread, priority); -#else // _WIN32 +#else // _WIN32 struct sched_param params; params.sched_priority = priority; int ret = pthread_setschedparam(thread, SCHED_FIFO, ¶ms); @@ -51,8 +57,8 @@ bool setFiFoScheduling(pthread_t& thread, const int priority) { URCL_LOG_WARN("Your system/user seems not to be setup for FIFO scheduling. We recommend using a lowlatency " "kernel with FIFO scheduling. See " - "https://github.com/UniversalRobots/Universal_Robots_ROS_Driver/blob/master/ur_robot_driver/" - "doc/real_time.md for details."); + "%s for details.", + RT_DOC_URL.c_str()); break; } default: diff --git a/src/primary/primary_client.cpp b/src/primary/primary_client.cpp new file mode 100644 index 00000000..54b4872e --- /dev/null +++ b/src/primary/primary_client.cpp @@ -0,0 +1,95 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright © 2024-2025 Ocado Group +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#include +#include +#include + +namespace urcl +{ +namespace primary_interface +{ +PrimaryClient::PrimaryClient(const std::string& robot_ip, comm::INotifier& notifier) + : stream_(robot_ip, UR_PRIMARY_PORT) +{ + prod_.reset(new comm::URProducer(stream_, parser_)); + + consumer_.reset(new PrimaryConsumer()); + consumer_->setErrorCodeMessageCallback(std::bind(&PrimaryClient::errorMessageCallback, this, std::placeholders::_1)); + + // Configure multi consumer even though we only have one consumer as default, as this enables the user to add more + // consumers after the object has been created + std::vector>> consumers; + consumers.push_back(consumer_); + multi_consumer_.reset(new comm::MultiConsumer(consumers)); + + pipeline_.reset( + new comm::Pipeline(*prod_, multi_consumer_.get(), "PrimaryClient Pipeline", notifier_)); +} + +PrimaryClient::~PrimaryClient() +{ + URCL_LOG_INFO("Stopping primary client pipeline"); + pipeline_->stop(); +} + +void PrimaryClient::start(const size_t max_num_tries, const std::chrono::milliseconds reconnection_time) +{ + URCL_LOG_INFO("Starting primary client pipeline"); + pipeline_->init(max_num_tries, reconnection_time); + pipeline_->run(); +} + +void PrimaryClient::addPrimaryConsumer(std::shared_ptr> primary_consumer) +{ + multi_consumer_->addConsumer(primary_consumer); +} + +void PrimaryClient::removePrimaryConsumer(std::shared_ptr> primary_consumer) +{ + multi_consumer_->removeConsumer(primary_consumer); +} + +void PrimaryClient::errorMessageCallback(ErrorCode& code) +{ + std::lock_guard lock_guard(error_code_queue_mutex_); + error_code_queue_.push_back(code); +} + +std::deque PrimaryClient::getErrorCodes() +{ + std::lock_guard lock_guard(error_code_queue_mutex_); + std::deque error_codes; + error_codes = error_code_queue_; + error_code_queue_.clear(); + return error_codes; +} +} // namespace primary_interface +} // namespace urcl diff --git a/src/primary/robot_message/error_code_message.cpp b/src/primary/robot_message/error_code_message.cpp new file mode 100644 index 00000000..a52a482f --- /dev/null +++ b/src/primary/robot_message/error_code_message.cpp @@ -0,0 +1,61 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2020 FZI Forschungszentrum Informatik +// Created on behalf of Universal Robots A/S +// +// 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. +// -- END LICENSE BLOCK ------------------------------------------------ + +//---------------------------------------------------------------------- +/*!\file + * + * \author Felix Exner exner@fzi.de + * \date 2020-04-30 + * + */ +//---------------------------------------------------------------------- + +#include "ur_client_library/primary/robot_message/error_code_message.h" +#include "ur_client_library/primary/abstract_primary_consumer.h" + +namespace urcl +{ +namespace primary_interface +{ +bool ErrorCodeMessage::parseWith(comm::BinParser& bp) +{ + bp.parse(message_code_); + bp.parse(message_argument_); + int32_t report_level; + bp.parse(report_level); + report_level_ = static_cast(report_level); + bp.parse(data_type_); + bp.parse(data_); + bp.parseRemainder(text_); + + return true; // not really possible to check dynamic size packets +} + +bool ErrorCodeMessage::consumeWith(AbstractPrimaryConsumer& consumer) +{ + return consumer.consume(*this); +} + +std::string ErrorCodeMessage::toString() const +{ + std::stringstream ss; + ss << "C" << message_code_ << "A" << message_argument_; + return ss.str(); +} + +} // namespace primary_interface +} // namespace urcl diff --git a/src/rtde/rtde_client.cpp b/src/rtde/rtde_client.cpp index 2e05fad4..8667d466 100644 --- a/src/rtde/rtde_client.cpp +++ b/src/rtde/rtde_client.cpp @@ -28,7 +28,9 @@ #include "ur_client_library/rtde/rtde_client.h" #include "ur_client_library/exceptions.h" +#include "ur_client_library/log.h" #include +#include namespace urcl { @@ -74,26 +76,34 @@ RTDEClient::~RTDEClient() disconnect(); } -bool RTDEClient::init(const size_t max_num_tries, const std::chrono::milliseconds reconnection_time) +bool RTDEClient::init(const size_t max_connection_attempts, const std::chrono::milliseconds reconnection_timeout, + const size_t max_initialization_attempts, const std::chrono::milliseconds initialization_timeout) { + if (max_initialization_attempts <= 0) + { + throw UrException("The number of initialization attempts has to be greater than 0."); + } + if (client_state_ > ClientState::UNINITIALIZED) { return true; } unsigned int attempts = 0; - while (attempts < MAX_INITIALIZE_ATTEMPTS) + while (attempts < max_initialization_attempts) { - setupCommunication(max_num_tries, reconnection_time); + setupCommunication(max_connection_attempts, reconnection_timeout); if (client_state_ == ClientState::INITIALIZED) return true; - URCL_LOG_ERROR("Failed to initialize RTDE client, retrying in 10 seconds"); - std::this_thread::sleep_for(std::chrono::seconds(10)); - attempts++; + if (++attempts < max_initialization_attempts) + { + URCL_LOG_ERROR("Failed to initialize RTDE client, retrying in %d seconds", initialization_timeout.count() / 1000); + std::this_thread::sleep_for(initialization_timeout); + } } std::stringstream ss; - ss << "Failed to initialize RTDE client after " << MAX_INITIALIZE_ATTEMPTS << " attempts"; + ss << "Failed to initialize RTDE client after " << max_initialization_attempts << " attempts"; throw UrException(ss.str()); } @@ -241,7 +251,7 @@ void RTDEClient::queryURControlVersion() else { std::stringstream ss; - ss << "Did not receive protocol negotiation answer from robot. Message received instead: " << std::endl + ss << "Did not receive URControl version from robot. Message received instead: " << std::endl << package->toString() << ". Retrying..."; num_retries++; URCL_LOG_WARN("%s", ss.str().c_str()); @@ -256,7 +266,6 @@ void RTDEClient::queryURControlVersion() void RTDEClient::resetOutputRecipe(const std::vector new_recipe) { - prod_->teardownProducer(); disconnect(); output_recipe_.assign(new_recipe.begin(), new_recipe.end()); @@ -332,10 +341,11 @@ void RTDEClient::setupOutputs(const uint16_t protocol_version) if (!unavailable_variables.empty()) { std::stringstream error_message; - error_message << "The following variables are not recognized by the robot: "; - std::for_each(unavailable_variables.begin(), unavailable_variables.end(), - [&error_message](const std::string& variable_name) { error_message << variable_name << " "; }); - error_message << ". Either your output recipe contains errors " + error_message << "The following variables are not recognized by the robot:"; + std::for_each( + unavailable_variables.begin(), unavailable_variables.end(), + [&error_message](const std::string& variable_name) { error_message << "\n - '" << variable_name << "'"; }); + error_message << "\nEither your output recipe contains errors " "or the urcontrol version does not support " "them."; @@ -346,6 +356,7 @@ void RTDEClient::setupOutputs(const uint16_t protocol_version) // Some variables are not available so retry setting up the communication with a stripped-down output recipe resetOutputRecipe(available_variables); + return; } else { @@ -445,10 +456,16 @@ void RTDEClient::setupInputs() void RTDEClient::disconnect() { // If communication is started it should be paused before disconnecting - if (client_state_ > ClientState::UNINITIALIZED) + if (client_state_ == ClientState::RUNNING) + { + pause(); + } + if (client_state_ >= ClientState::INITIALIZING) { - sendPause(); pipeline_->stop(); + } + if (client_state_ > ClientState::UNINITIALIZED) + { stream_.disconnect(); } client_state_ = ClientState::UNINITIALIZED; diff --git a/src/ur/dashboard_client.cpp b/src/ur/dashboard_client.cpp index 75650688..b05b5450 100644 --- a/src/ur/dashboard_client.cpp +++ b/src/ur/dashboard_client.cpp @@ -26,6 +26,7 @@ */ //---------------------------------------------------------------------- +#include #include #include #include @@ -34,10 +35,9 @@ #include #ifndef _WIN32 -#include +# include #endif // !_WIN32 - using namespace std::chrono_literals; namespace urcl @@ -164,7 +164,7 @@ bool DashboardClient::sendRequest(const std::string& command, const std::string& bool ret = std::regex_match(response, std::regex(expected)); if (!ret) { - throw UrException("Expected: " + expected + ", but received: " + response); + URCL_LOG_WARN("Expected: \"%s\", but received: \"%s\"", expected.c_str(), response.c_str()); } return ret; } @@ -250,8 +250,12 @@ bool DashboardClient::commandBrakeRelease() bool DashboardClient::commandLoadProgram(const std::string& program_file_name) { assertVersion("5.0.0", "1.4", "load "); + // We load the program, which will fail if the program is not found or the requested file does + // not contain a valid program. Afterwards, we wait until the program state is stopped with a + // program named the same as the requested program. We cannot check the full file path here, but + // the important thing is that the program state is stopped. return sendRequest("load " + program_file_name + "", "(?:Loading program: ).*(?:" + program_file_name + ").*") && - waitForReply("programState", "STOPPED " + program_file_name); + waitForReply("programState", "STOPPED " + std::filesystem::path{ program_file_name }.filename().string()); } bool DashboardClient::commandLoadInstallation(const std::string& installation_file_name) diff --git a/src/ur/instruction_executor.cpp b/src/ur/instruction_executor.cpp new file mode 100644 index 00000000..9fab61d5 --- /dev/null +++ b/src/ur/instruction_executor.cpp @@ -0,0 +1,112 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2025 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#include "ur_client_library/ur/instruction_executor.h" +#include "ur_client_library/control/trajectory_point_interface.h" +void urcl::InstructionExecutor::trajDoneCallback(const urcl::control::TrajectoryResult& result) +{ + std::unique_lock lock(trajectory_result_mutex_); + trajectory_result_ = result; + trajectory_running_ = false; +} +void urcl::InstructionExecutor::trajDisconnectCallback(const int filedescriptor) +{ + URCL_LOG_INFO("Trajectory disconnect"); + std::unique_lock lock(trajectory_result_mutex_); + if (trajectory_running_) + { + trajectory_result_ = urcl::control::TrajectoryResult::TRAJECTORY_RESULT_FAILURE; + trajectory_running_ = false; + } +} +bool urcl::InstructionExecutor::executeMotion( + const std::vector>& motion_sequence) +{ + if (!driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, + motion_sequence.size())) + { + URCL_LOG_ERROR("Cannot send trajectory control command. No client connected?"); + std::unique_lock lock(trajectory_result_mutex_); + trajectory_result_ = urcl::control::TrajectoryResult::TRAJECTORY_RESULT_FAILURE; + return false; + } + + for (const auto& primitive : motion_sequence) + { + switch (primitive->type) + { + case control::MotionType::MOVEJ: + { + auto movej_primitive = std::static_pointer_cast(primitive); + driver_->writeTrajectoryPoint(movej_primitive->target_joint_configuration, primitive->acceleration, + primitive->velocity, false, primitive->duration.count(), primitive->blend_radius); + break; + } + case control::MotionType::MOVEL: + { + auto movel_primitive = std::static_pointer_cast(primitive); + urcl::vector6d_t pose_vec = { movel_primitive->target_pose.x, movel_primitive->target_pose.y, + movel_primitive->target_pose.z, movel_primitive->target_pose.rx, + movel_primitive->target_pose.ry, movel_primitive->target_pose.rz }; + driver_->writeTrajectoryPoint(pose_vec, primitive->acceleration, primitive->velocity, true, + primitive->duration.count(), primitive->blend_radius); + break; + } + default: + URCL_LOG_ERROR("Unsupported motion type"); + // The hardware will complain about missing trajectory points and return a failure for + // trajectory execution. Hence, we need to step into the running loop below. + } + } + trajectory_running_ = true; + + while (trajectory_running_) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP); + } + { + std::unique_lock lock(trajectory_result_mutex_); + URCL_LOG_INFO("Trajectory done with result %s", control::trajectoryResultToString(trajectory_result_).c_str()); + return trajectory_result_ == urcl::control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS; + } +} +bool urcl::InstructionExecutor::moveJ(const urcl::vector6d_t& target, const double acceleration, const double velocity, + const double time, const double blend_radius) +{ + return executeMotion({ std::make_shared( + target, blend_radius, std::chrono::milliseconds(static_cast(time * 1000)), acceleration, velocity) }); +} +bool urcl::InstructionExecutor::moveL(const urcl::Pose& target, const double acceleration, const double velocity, + const double time, const double blend_radius) +{ + return executeMotion({ std::make_shared( + target, blend_radius, std::chrono::milliseconds(static_cast(time * 1000)), acceleration, velocity) }); +} diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index 88a485fd..52fe45ed 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -52,38 +52,41 @@ static const std::string SCRIPT_COMMAND_PORT_REPLACE("{{SCRIPT_COMMAND_SERVER_PO static const std::string FORCE_MODE_SET_DAMPING_REPLACE("{{FORCE_MODE_SET_DAMPING_REPLACE}}"); static const std::string FORCE_MODE_SET_GAIN_SCALING_REPLACE("{{FORCE_MODE_SET_GAIN_SCALING_REPLACE}}"); -urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_file, - const std::string& output_recipe_file, const std::string& input_recipe_file, - std::function handle_program_state, bool headless_mode, - std::unique_ptr tool_comm_setup, const uint32_t reverse_port, - const uint32_t script_sender_port, int servoj_gain, double servoj_lookahead_time, - bool non_blocking_read, const std::string& reverse_ip, const uint32_t trajectory_port, - const uint32_t script_command_port) - : servoj_gain_(servoj_gain) - , servoj_lookahead_time_(servoj_lookahead_time) - , handle_program_state_(handle_program_state) - , robot_ip_(robot_ip) -{ +void UrDriver::init(const UrDriverConfiguration& config) +{ + robot_ip_ = config.robot_ip; + non_blocking_read_ = config.non_blocking_read; + servoj_gain_ = config.servoj_gain; + servoj_lookahead_time_ = config.servoj_lookahead_time; + handle_program_state_ = config.handle_program_state; + in_headless_mode_ = config.headless_mode; + socket_connection_attempts_ = config.socket_reconnect_attempts; + socket_reconnection_timeout_ = config.socket_reconnection_timeout; + rtde_initialization_attempts_ = config.rtde_initialization_attempts_; + rtde_initialization_timeout_ = config.rtde_initialization_timeout_; + URCL_LOG_DEBUG("Initializing urdriver"); URCL_LOG_DEBUG("Initializing RTDE client"); - rtde_client_.reset(new rtde_interface::RTDEClient(robot_ip_, notifier_, output_recipe_file, input_recipe_file)); + rtde_client_.reset( + new rtde_interface::RTDEClient(robot_ip_, notifier_, config.output_recipe_file, config.input_recipe_file)); primary_stream_.reset( new comm::URStream(robot_ip_, urcl::primary_interface::UR_PRIMARY_PORT)); secondary_stream_.reset( new comm::URStream(robot_ip_, urcl::primary_interface::UR_SECONDARY_PORT)); - secondary_stream_->connect(); + secondary_stream_->connect(socket_connection_attempts_, socket_reconnection_timeout_); + + primary_client_.reset(new urcl::primary_interface::PrimaryClient(robot_ip_, notifier_)); - non_blocking_read_ = non_blocking_read; get_packet_timeout_ = non_blocking_read_ ? 0 : 100; initRTDE(); - setupReverseInterface(reverse_port); + setupReverseInterface(config.reverse_port); // Figure out the ip automatically if the user didn't provide it - std::string local_ip = reverse_ip.empty() ? rtde_client_->getIP() : reverse_ip; + std::string local_ip = config.reverse_ip.empty() ? rtde_client_->getIP() : config.reverse_ip; - std::string prog = readScriptFile(script_file); + std::string prog = readScriptFile(config.script_file); while (prog.find(JOINT_STATE_REPLACE) != std::string::npos) { prog.replace(prog.find(JOINT_STATE_REPLACE), JOINT_STATE_REPLACE.length(), @@ -109,24 +112,25 @@ urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_ while (prog.find(SERVER_PORT_REPLACE) != std::string::npos) { - prog.replace(prog.find(SERVER_PORT_REPLACE), SERVER_PORT_REPLACE.length(), std::to_string(reverse_port)); + prog.replace(prog.find(SERVER_PORT_REPLACE), SERVER_PORT_REPLACE.length(), std::to_string(config.reverse_port)); } while (prog.find(TRAJECTORY_PORT_REPLACE) != std::string::npos) { - prog.replace(prog.find(TRAJECTORY_PORT_REPLACE), TRAJECTORY_PORT_REPLACE.length(), std::to_string(trajectory_port)); + prog.replace(prog.find(TRAJECTORY_PORT_REPLACE), TRAJECTORY_PORT_REPLACE.length(), + std::to_string(config.trajectory_port)); } while (prog.find(SCRIPT_COMMAND_PORT_REPLACE) != std::string::npos) { prog.replace(prog.find(SCRIPT_COMMAND_PORT_REPLACE), SCRIPT_COMMAND_PORT_REPLACE.length(), - std::to_string(script_command_port)); + std::to_string(config.script_command_port)); } robot_version_ = rtde_client_->getVersion(); std::stringstream begin_replace; - if (tool_comm_setup != nullptr) + if (config.tool_comm_setup != nullptr) { if (robot_version_.major < 5) { @@ -135,15 +139,15 @@ urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_ 5, robot_version_.major); } begin_replace << "set_tool_voltage(" - << static_cast::type>(tool_comm_setup->getToolVoltage()) << ")\n"; - begin_replace << "set_tool_communication(" << "True" << ", " << tool_comm_setup->getBaudRate() << ", " - << static_cast::type>(tool_comm_setup->getParity()) << ", " - << tool_comm_setup->getStopBits() << ", " << tool_comm_setup->getRxIdleChars() << ", " - << tool_comm_setup->getTxIdleChars() << ")"; + << static_cast::type>(config.tool_comm_setup->getToolVoltage()) + << ")\n"; + begin_replace << "set_tool_communication(" << "True" << ", " << config.tool_comm_setup->getBaudRate() << ", " + << static_cast::type>(config.tool_comm_setup->getParity()) << ", " + << config.tool_comm_setup->getStopBits() << ", " << config.tool_comm_setup->getRxIdleChars() << ", " + << config.tool_comm_setup->getTxIdleChars() << ")"; } prog.replace(prog.find(BEGIN_REPLACE), BEGIN_REPLACE.length(), begin_replace.str()); - in_headless_mode_ = headless_mode; if (in_headless_mode_) { full_robot_program_ = "stop program\n"; @@ -159,61 +163,34 @@ urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_ } else { - script_sender_.reset(new control::ScriptSender(script_sender_port, prog)); + script_sender_.reset(new control::ScriptSender(config.script_sender_port, prog)); URCL_LOG_DEBUG("Created script sender"); } - trajectory_interface_.reset(new control::TrajectoryPointInterface(trajectory_port)); - script_command_interface_.reset(new control::ScriptCommandInterface(script_command_port)); - - URCL_LOG_DEBUG("Initialization done"); -} + trajectory_interface_.reset(new control::TrajectoryPointInterface(config.trajectory_port)); + script_command_interface_.reset(new control::ScriptCommandInterface(config.script_command_port)); -urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_file, - const std::string& output_recipe_file, const std::string& input_recipe_file, - std::function handle_program_state, bool headless_mode, - std::unique_ptr tool_comm_setup, const uint32_t reverse_port, - const uint32_t script_sender_port, int servoj_gain, double servoj_lookahead_time, - bool non_blocking_read, const std::string& reverse_ip, const uint32_t trajectory_port, - const uint32_t script_command_port, double force_mode_damping, double force_mode_gain_scaling) - : UrDriver(robot_ip, script_file, output_recipe_file, input_recipe_file, handle_program_state, headless_mode, - std::move(tool_comm_setup), reverse_port, script_sender_port, servoj_gain, servoj_lookahead_time, - non_blocking_read, reverse_ip, trajectory_port, script_command_port) -{ - force_mode_damping_factor_ = force_mode_damping; - force_mode_gain_scale_factor_ = force_mode_gain_scaling; -} - -urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_file, - const std::string& output_recipe_file, const std::string& input_recipe_file, - std::function handle_program_state, bool headless_mode, - std::unique_ptr tool_comm_setup, const std::string& calibration_checksum, - const uint32_t reverse_port, const uint32_t script_sender_port, int servoj_gain, - double servoj_lookahead_time, bool non_blocking_read, const std::string& reverse_ip, - const uint32_t trajectory_port, const uint32_t script_command_port, double force_mode_damping, - double force_mode_gain_scaling) - : UrDriver(robot_ip, script_file, output_recipe_file, input_recipe_file, handle_program_state, headless_mode, - std::move(tool_comm_setup), reverse_port, script_sender_port, servoj_gain, servoj_lookahead_time, - non_blocking_read, reverse_ip, trajectory_port, script_command_port, force_mode_damping, - force_mode_gain_scaling) -{ - URCL_LOG_WARN("DEPRECATION NOTICE: Passing the calibration_checksum to the UrDriver's constructor has been " - "deprecated. Instead, use the checkCalibration(calibration_checksum) function separately. This " - "notice is for application developers using this library. If you are only using an application using " - "this library, you can ignore this message."); - if (checkCalibration(calibration_checksum)) - { - URCL_LOG_INFO("Calibration checked successfully."); - } - else + if (!std::empty(config.calibration_checksum)) { - URCL_LOG_ERROR("The calibration parameters of the connected robot don't match the ones from the given kinematics " - "config file. Please be aware that this can lead to critical inaccuracies of tcp positions. Use " - "the ur_calibration tool to extract the correct calibration from the robot and pass that into the " - "description. See " - "[https://github.com/UniversalRobots/Universal_Robots_ROS_Driver#extract-calibration-information] " - "for details."); + URCL_LOG_WARN("DEPRECATION NOTICE: Passing the calibration_checksum to the UrDriver's constructor has been " + "deprecated. Instead, use the checkCalibration(calibration_checksum) function separately. This " + "notice is for application developers using this library. If you are only using an application using " + "this library, you can ignore this message."); + if (checkCalibration(config.calibration_checksum)) + { + URCL_LOG_INFO("Calibration checked successfully."); + } + else + { + URCL_LOG_ERROR("The calibration parameters of the connected robot don't match the ones from the given kinematics " + "config file. Please be aware that this can lead to critical inaccuracies of tcp positions. Use " + "the ur_calibration tool to extract the correct calibration from the robot and pass that into the " + "description. See " + "[https://github.com/UniversalRobots/Universal_Robots_ROS_Driver#extract-calibration-information] " + "for details."); + } } + URCL_LOG_DEBUG("Initialization done"); } std::unique_ptr urcl::UrDriver::getDataPackage() @@ -692,7 +669,11 @@ void UrDriver::setKeepaliveCount(const uint32_t count) "set the " "read timeout in the write commands directly. This keepalive count will overwrite the timeout passed " "to the write functions."); +// TODO: Remove 2027-05 +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" reverse_interface_->setKeepaliveCount(count); +#pragma GCC diagnostic pop } void UrDriver::resetRTDEClient(const std::vector& output_recipe, @@ -714,7 +695,8 @@ void UrDriver::resetRTDEClient(const std::string& output_recipe_filename, const void UrDriver::initRTDE() { - if (!rtde_client_->init()) + if (!rtde_client_->init(socket_connection_attempts_, socket_reconnection_timeout_, rtde_initialization_attempts_, + rtde_initialization_timeout_)) { throw UrException("Initialization of RTDE client went wrong."); } @@ -727,4 +709,13 @@ void UrDriver::setupReverseInterface(const uint32_t reverse_port) reverse_interface_.reset(new control::ReverseInterface(reverse_port, handle_program_state_, step_time)); } +void UrDriver::startPrimaryClientCommunication() +{ + primary_client_->start(socket_connection_attempts_, socket_reconnection_timeout_); +} + +std::deque UrDriver::getErrorCodes() +{ + return primary_client_->getErrorCodes(); +} } // namespace urcl diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index dc3bd87f..e4eda80a 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -31,15 +31,73 @@ if (INTEGRATION_TESTS) gtest_add_tests(TARGET dashboard_client_tests ) - add_executable(spline_tests test_spline_interpolation.cpp) - target_link_libraries(spline_tests PRIVATE ur_client_library::urcl GTest::gtest_main) - gtest_add_tests(TARGET spline_tests + # Spline tests + add_executable(spline_tests_urcap test_spline_interpolation.cpp) + target_link_libraries(spline_tests_urcap PRIVATE ur_client_library::urcl GTest::gtest_main) + gtest_add_tests(TARGET spline_tests_urcap WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + EXTRA_ARGS --headless false + TEST_SUFFIX _urcap + ) + add_executable(spline_tests_headless test_spline_interpolation.cpp) + target_link_libraries(spline_tests_headless PRIVATE ur_client_library::urcl GTest::gtest_main) + gtest_add_tests(TARGET spline_tests_headless + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + EXTRA_ARGS --headless true + TEST_SUFFIX _headless + ) + + # UrDriver tests + add_executable(ur_driver_tests_urcap test_ur_driver.cpp) + target_link_libraries(ur_driver_tests_urcap PRIVATE ur_client_library::urcl GTest::gtest_main) + gtest_add_tests(TARGET ur_driver_tests_urcap + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + EXTRA_ARGS --headless false + TEST_SUFFIX _urcap + ) + add_executable(ur_driver_tests_headless test_ur_driver.cpp) + target_link_libraries(ur_driver_tests_headless PRIVATE ur_client_library::urcl GTest::gtest_main) + gtest_add_tests(TARGET ur_driver_tests_headless + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + EXTRA_ARGS --headless true + TEST_SUFFIX _headless + ) + + # InstructionExecutor tests + add_executable(instruction_executor_test_urcap test_instruction_executor.cpp) + target_link_libraries(instruction_executor_test_urcap PRIVATE ur_client_library::urcl GTest::gtest_main) + gtest_add_tests(TARGET instruction_executor_test_urcap + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + EXTRA_ARGS --headless false + TEST_SUFFIX _urcap + ) + add_executable(instruction_executor_test_headless test_instruction_executor.cpp) + target_link_libraries(instruction_executor_test_headless PRIVATE ur_client_library::urcl GTest::gtest_main) + gtest_add_tests(TARGET instruction_executor_test_headless + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + EXTRA_ARGS --headless true + TEST_SUFFIX _headless + ) + + # PrimaryClient tests + add_executable(primary_client_test_urcap test_primary_client.cpp) + target_link_libraries(primary_client_test_urcap PRIVATE ur_client_library::urcl GTest::gtest_main) + gtest_add_tests(TARGET primary_client_test_urcap + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + EXTRA_ARGS --headless false + TEST_SUFFIX _urcap + ) + add_executable(primary_client_test_headless test_primary_client.cpp) + target_link_libraries(primary_client_test_headless PRIVATE ur_client_library::urcl GTest::gtest_main) + gtest_add_tests(TARGET primary_client_test_headless + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + EXTRA_ARGS --headless true + TEST_SUFFIX _headless ) - add_executable(ur_driver_tests test_ur_driver.cpp) - target_link_libraries(ur_driver_tests PRIVATE ur_client_library::urcl GTest::gtest_main) - gtest_add_tests(TARGET ur_driver_tests + add_executable(ur_driver_deprecated_constructor_test test_deprecated_ur_driver_construction.cpp) + target_link_libraries(ur_driver_deprecated_constructor_test PRIVATE ur_client_library::urcl GTest::gtest_main) + gtest_add_tests(TARGET ur_driver_deprecated_constructor_test WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} ) else() diff --git a/tests/resources/dockerursim/programs/cb3/external_control.urp b/tests/resources/dockerursim/programs/cb3/external_control.urp new file mode 100644 index 00000000..812ba285 Binary files /dev/null and b/tests/resources/dockerursim/programs/cb3/external_control.urp differ diff --git a/tests/resources/dockerursim/programs/e-series/external_control.urp b/tests/resources/dockerursim/programs/e-series/external_control.urp new file mode 100644 index 00000000..4139fd18 Binary files /dev/null and b/tests/resources/dockerursim/programs/e-series/external_control.urp differ diff --git a/tests/test_dashboard_client.cpp b/tests/test_dashboard_client.cpp index 8b67ff84..5990e461 100644 --- a/tests/test_dashboard_client.cpp +++ b/tests/test_dashboard_client.cpp @@ -235,6 +235,36 @@ TEST_F(DashboardClientTest, connect_non_running_robot) EXPECT_LT(elapsed, 2 * comm::TCPSocket::DEFAULT_RECONNECTION_TIME); } +TEST_F(DashboardClientTest, non_expected_result_returns_correctly) +{ + ASSERT_TRUE(dashboard_client_->connect()); + EXPECT_TRUE(dashboard_client_->commandPowerOff()); + + // We will not get this answer + EXPECT_FALSE(dashboard_client_->sendRequest("brake release", "non-existing-response")); + + // A non-matching answer should throw an exception for this call + EXPECT_THROW(dashboard_client_->sendRequestString("brake release", "non-existing-response"), UrException); + + // Waiting for a non-matching answer should return false + // Internally we wait 100ms between each attempt, hence the 300ms wait time + EXPECT_FALSE( + dashboard_client_->waitForReply("brake_release", "non-existing-response", std::chrono::milliseconds(300))); +} + +TEST_F(DashboardClientTest, connecting_twice_returns_false) +{ + ASSERT_TRUE(dashboard_client_->connect()); + EXPECT_FALSE(dashboard_client_->connect()); +} + +TEST_F(DashboardClientTest, load_program_in_subdir_works) +{ + ASSERT_TRUE(dashboard_client_->connect()); + + ASSERT_TRUE(dashboard_client_->commandLoadProgram("/ursim/programs/wait_program.urp")); +} + int main(int argc, char* argv[]) { ::testing::InitGoogleTest(&argc, argv); diff --git a/tests/test_deprecated_ur_driver_construction.cpp b/tests/test_deprecated_ur_driver_construction.cpp new file mode 100644 index 00000000..08b55e65 --- /dev/null +++ b/tests/test_deprecated_ur_driver_construction.cpp @@ -0,0 +1,109 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2025 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#include +#include +#include "ur_client_library/ur/ur_driver.h" + +const std::string SCRIPT_FILE = "../resources/external_control.urscript"; +const std::string OUTPUT_RECIPE = "resources/rtde_output_recipe.txt"; +const std::string INPUT_RECIPE = "resources/rtde_input_recipe.txt"; +const std::string CALIBRATION_CHECKSUM = "calib_12788084448423163542"; +std::string g_ROBOT_IP = "192.168.56.101"; +bool g_HEADLESS = true; + +void handleRobotProgramState(bool program_running) +{ + std::cout << "\033[1;32mProgram running: " << std::boolalpha << program_running << "\033[0m\n" << std::endl; +} + +TEST(UrDriverTestDeprecatedConstructor, sigA) +{ + std::unique_ptr tool_comm_setup; + auto driver = std::make_shared(g_ROBOT_IP, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, + std::bind(&handleRobotProgramState, std::placeholders::_1), g_HEADLESS, + std::move(tool_comm_setup)); + driver->checkCalibration(CALIBRATION_CHECKSUM); + auto version = driver->getVersion(); + ASSERT_TRUE(version.major > 0); +} + +TEST(UrDriverTestDeprecatedConstructor, sigB) +{ + std::unique_ptr tool_comm_setup; + auto driver = std::make_shared( + g_ROBOT_IP, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, std::bind(&handleRobotProgramState, std::placeholders::_1), + g_HEADLESS, std::move(tool_comm_setup), 50001, 50002, 2000, 0.03, false, "", 50003, 50004, 0.025, 0.5); + driver->checkCalibration(CALIBRATION_CHECKSUM); + auto version = driver->getVersion(); + ASSERT_TRUE(version.major > 0); +} + +TEST(UrDriverTestDeprecatedConstructor, sigC) +{ + std::unique_ptr tool_comm_setup; + auto driver = std::make_shared(g_ROBOT_IP, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, + std::bind(&handleRobotProgramState, std::placeholders::_1), g_HEADLESS, + std::move(tool_comm_setup), CALIBRATION_CHECKSUM); + auto version = driver->getVersion(); + ASSERT_TRUE(version.major > 0); +} + +TEST(UrDriverTestDeprecatedConstructor, sigD) +{ + std::unique_ptr tool_comm_setup; + auto driver = std::make_shared(g_ROBOT_IP, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, + std::bind(&handleRobotProgramState, std::placeholders::_1), g_HEADLESS, + CALIBRATION_CHECKSUM); + auto version = driver->getVersion(); + ASSERT_TRUE(version.major > 0); +} + +int main(int argc, char* argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + + for (int i = 0; i < argc; i++) + { + if (std::string(argv[i]) == "--robot_ip" && i + 1 < argc) + { + g_ROBOT_IP = argv[i + 1]; + break; + } + if (std::string(argv[i]) == "--headless" && i + 1 < argc) + { + std::string headless = argv[i + 1]; + g_HEADLESS = headless == "true" || headless == "1" || headless == "True" || headless == "TRUE"; + break; + } + } + + return RUN_ALL_TESTS(); +} diff --git a/tests/test_instruction_executor.cpp b/tests/test_instruction_executor.cpp new file mode 100644 index 00000000..aadbc2b2 --- /dev/null +++ b/tests/test_instruction_executor.cpp @@ -0,0 +1,282 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2025 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#include +#include +#include +#include "ur_client_library/ur/instruction_executor.h" +#include "ur_client_library/control/motion_primitives.h" + +#include + +using namespace urcl; +using namespace urcl::control; + +const std::string SCRIPT_FILE = "../resources/external_control.urscript"; +const std::string OUTPUT_RECIPE = "resources/rtde_output_recipe.txt"; +const std::string INPUT_RECIPE = "resources/rtde_input_recipe.txt"; +const std::string CALIBRATION_CHECKSUM = "calib_12788084448423163542"; +std::string ROBOT_IP = "192.168.56.101"; +bool g_HEADLESS = true; + +std::unique_ptr g_my_robot; + +class InstructionExecutorTest : public ::testing::Test +{ +protected: + std::unique_ptr executor_; + + static void SetUpTestSuite() + { + // Setup driver + g_my_robot = std::make_unique(ROBOT_IP, OUTPUT_RECIPE, INPUT_RECIPE, g_HEADLESS, + "external_control.urp", SCRIPT_FILE); + } + void SetUp() override + { + executor_ = std::make_unique(g_my_robot->ur_driver_); + g_my_robot->clearProtectiveStop(); + // Make sure script is running on the robot + if (!g_my_robot->waitForProgramRunning()) + { + g_my_robot->resendRobotProgram(); + ASSERT_TRUE(g_my_robot->waitForProgramRunning()); + } + } +}; + +TEST_F(InstructionExecutorTest, execute_motion_sequence_success) +{ + std::vector> motion_sequence{ + std::make_shared(urcl::vector6d_t{ -1.57, -1.57, 0, 0, 0, 0 }, 0.1, + std::chrono::seconds(5)), + std::make_shared(urcl::vector6d_t{ -1.57, -1.6, 1.6, -0.7, 0.7, 0.2 }, 0.1, + std::chrono::seconds(5)), + + std::make_shared(urcl::Pose{ -0.203, 0.263, 0.559, 0.68, -1.083, -2.076 }, 0.1, + std::chrono::seconds(2)), + std::make_shared(urcl::Pose{ -0.203, 0.463, 0.559, 0.68, -1.083, -2.076 }, 0.1, + std::chrono::seconds(2)), + }; + ASSERT_TRUE(executor_->executeMotion(motion_sequence)); +} + +TEST_F(InstructionExecutorTest, execute_movej_success) +{ + // default parametrization + ASSERT_TRUE(executor_->moveJ({ -1.57, -1.57, 0, 0, 0, 0 })); + ASSERT_TRUE(executor_->moveJ({ -1.57, -1.6, 1.6, -0.7, 0.7, 0.2 })); + + // acceleration & velocity parametrization + auto start = std::chrono::steady_clock::now(); + ASSERT_TRUE(executor_->moveJ({ -1.57, -1.57, 0, 0, 0, 0 }, 3.4, 3.1, 0)); + auto end = std::chrono::steady_clock::now(); + auto duration = std::chrono::duration_cast(end - start); + // With this parametrization execution should take about 1.4 seconds plus some overhead. We test + // time parametrization below with a motion time of 2.5 seconds, so this is the upper bound to + // distinguish between the two. This large range is necessary, as the actual overhead is not + // known. + ASSERT_GT(duration.count(), 1400); + ASSERT_LT(duration.count(), 2500); + + start = std::chrono::steady_clock::now(); + ASSERT_TRUE(executor_->moveJ({ -1.57, -1.6, 1.6, -0.7, 0.7, 0.2 }, 3.4, 3.1, 0)); + end = std::chrono::steady_clock::now(); + duration = std::chrono::duration_cast(end - start); + ASSERT_GT(duration.count(), 1400); + ASSERT_LT(duration.count(), 2500); + + // time parametrization + start = std::chrono::steady_clock::now(); + ASSERT_TRUE(executor_->moveJ({ -1.57, -1.57, 0, 0, 0, 0 }, 3.4, 3.1, 3)); // 3 seconds + end = std::chrono::steady_clock::now(); + duration = std::chrono::duration_cast(end - start); + ASSERT_GE(duration.count(), 3000); + + start = std::chrono::steady_clock::now(); + ASSERT_TRUE(executor_->moveJ({ -1.57, -1.6, 1.6, -0.7, 0.7, 0.2 }, 3.4, 3.1, 2.5)); // 2.5 seconds + end = std::chrono::steady_clock::now(); + duration = std::chrono::duration_cast(end - start); + ASSERT_GE(duration.count(), 2500); +} + +TEST_F(InstructionExecutorTest, execute_movel_success) +{ + // move to a feasible starting pose + ASSERT_TRUE(executor_->moveJ({ -1.57, -1.6, 1.6, -0.7, 0.7, 0.2 })); + ASSERT_TRUE(executor_->moveL({ -0.203, 0.263, 0.559, 0.68, -1.083, -2.076 })); + ASSERT_TRUE(executor_->moveL({ -0.203, 0.463, 0.559, 0.68, -1.083, -2.076 })); + + // acceleration & velocity parametrization + auto start = std::chrono::steady_clock::now(); + ASSERT_TRUE(executor_->moveL({ -0.203, 0.263, 0.559, 0.68, -1.083, -2.076 }, 2.5, 2.5)); + auto end = std::chrono::steady_clock::now(); + auto duration = std::chrono::duration_cast(end - start); + // With this parametrization execution should take about 0.6 seconds plus some overhead. We test + // time parametrization below with a motion time of 2.0 seconds, so this is the upper bound to + // distinguish between the two. This large range is necessary, as the actual overhead is not + // known. + ASSERT_GT(duration.count(), 500); + ASSERT_LT(duration.count(), 2000); + + start = std::chrono::steady_clock::now(); + ASSERT_TRUE(executor_->moveL({ -0.203, 0.463, 0.559, 0.68, -1.083, -2.076 }, 2.5, 2.5)); + end = std::chrono::steady_clock::now(); + duration = std::chrono::duration_cast(end - start); + ASSERT_GT(duration.count(), 500); + ASSERT_LT(duration.count(), 2000); + + // time parametrization + start = std::chrono::steady_clock::now(); + ASSERT_TRUE(executor_->moveL({ -0.203, 0.263, 0.559, 0.68, -1.083, -2.076 }, 2.5, 2.5, 2.0)); + end = std::chrono::steady_clock::now(); + duration = std::chrono::duration_cast(end - start); + ASSERT_GT(duration.count(), 2000); + + start = std::chrono::steady_clock::now(); + ASSERT_TRUE(executor_->moveL({ -0.203, 0.463, 0.559, 0.68, -1.083, -2.076 }, 2.5, 1.5, 2.2)); + end = std::chrono::steady_clock::now(); + duration = std::chrono::duration_cast(end - start); + ASSERT_GT(duration.count(), 2200); +} + +TEST_F(InstructionExecutorTest, sending_commands_without_reverse_interface_connected_fails) +{ + g_my_robot->dashboard_client_->commandStop(); + ASSERT_TRUE(g_my_robot->waitForProgramNotRunning(1000)); + + ASSERT_FALSE(executor_->moveJ({ -1.57, -1.6, 1.6, -0.7, 0.7, 0.2 })); + ASSERT_FALSE(executor_->moveL({ -0.203, 0.263, 0.559, 0.68, -1.083, -2.076 })); + std::vector> motion_sequence{ + std::make_shared(urcl::vector6d_t{ -1.57, -1.57, 0, 0, 0, 0 }, 0.1, + std::chrono::seconds(5)), + std::make_shared(urcl::vector6d_t{ -1.57, -1.6, 1.6, -0.7, 0.7, 0.2 }, 0.1, + std::chrono::seconds(5)), + + std::make_shared(urcl::Pose{ -0.203, 0.263, 0.559, 0.68, -1.083, -2.076 }, 0.1, + std::chrono::seconds(2)), + std::make_shared(urcl::Pose{ -0.203, 0.463, 0.559, 0.68, -1.083, -2.076 }, 0.1, + std::chrono::seconds(2)), + }; + ASSERT_FALSE(executor_->executeMotion(motion_sequence)); + + // Disconnect mid-motion + g_my_robot->resendRobotProgram(); + ASSERT_TRUE(g_my_robot->waitForProgramRunning(1000)); + + // move to first pose + ASSERT_TRUE(executor_->moveJ({ -1.57, -1.57, 0, 0, 0, 0 })); + // move to second pose asynchronoysly + auto motion_thread = std::thread([&]() { ASSERT_FALSE(executor_->moveJ({ -1.57, -1.6, 1.6, -0.7, 0.7, 0.2 })); }); + + std::this_thread::sleep_for(std::chrono::milliseconds(300)); + g_my_robot->dashboard_client_->commandStop(); + ASSERT_TRUE(g_my_robot->waitForProgramNotRunning(1000)); + motion_thread.join(); +} + +TEST_F(InstructionExecutorTest, sending_illegal_motion_type_fails) +{ + auto primitive = std::make_shared(); + primitive->type = urcl::control::MotionType::UNKNOWN; + std::vector> motion_sequence{ primitive }; + ASSERT_FALSE(executor_->executeMotion(motion_sequence)); +} + +TEST_F(InstructionExecutorTest, unfeasible_movej_target_results_in_failure) +{ + // move to a feasible starting pose + ASSERT_TRUE(executor_->moveJ({ -1.57, -1.6, 1.6, -0.7, 0.7, 0.2 })); + + // move to an unfeasible pose + ASSERT_FALSE(executor_->moveJ({ -123, 0, 0, 0, 0, 0 })); +} + +TEST_F(InstructionExecutorTest, unfeasible_movel_target_results_in_failure) +{ + // move to a feasible starting pose + ASSERT_TRUE(executor_->moveJ({ -1.57, -1.6, 1.6, -0.7, 0.7, 0.2 })); + + // move to an unfeasible pose + ASSERT_FALSE(executor_->moveL({ -10.203, 0.263, 0.559, 0.68, -1.083, -2.076 }, 1.4, 1.04, 0.1)); +} + +TEST_F(InstructionExecutorTest, unfeasible_sequence_targets_results_in_failure) +{ + std::vector> motion_sequence{ + std::make_shared(urcl::vector6d_t{ -1.57, -1.57, 0, 0, 0, 0 }, 0.1), + std::make_shared(urcl::vector6d_t{ -157, -1.6, 1.6, -0.7, 0.7, 0.2 }, 0.1), + + }; + ASSERT_FALSE(executor_->executeMotion(motion_sequence)); +} + +TEST_F(InstructionExecutorTest, unfeasible_times_succeeds) +{ + // Unfeasible time constraints should simply result in speed scaling slowing down the robot. + + // move to a feasible starting pose + ASSERT_TRUE(executor_->moveJ({ -1.57, -1.6, 1.6, -0.7, 0.7, 0.2 })); + // Request going to another pose unfeasibly fast + ASSERT_TRUE(executor_->moveJ({ 1.57, -1.6, 1.6, -0.7, 0.7, 0.2 }, 1.4, 1.4, 0.1)); + + // move to a feasible starting pose + ASSERT_TRUE(executor_->moveJ({ -1.57, -1.6, 1.6, -0.7, 0.7, 0.2 })); + // Request going to another pose unfeasibly fast + ASSERT_TRUE(executor_->moveL({ -0.203, 0.263, 0.559, 0.68, -1.083, -2.076 }, 1.4, 1.04, 0.1)); +} + +TEST_F(InstructionExecutorTest, empty_sequence_succeeds) +{ + std::vector> motion_sequence{}; + ASSERT_TRUE(executor_->executeMotion(motion_sequence)); +} + +int main(int argc, char* argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + + for (int i = 0; i < argc; i++) + { + if (std::string(argv[i]) == "--robot_ip" && i + 1 < argc) + { + ROBOT_IP = argv[i + 1]; + break; + } + if (std::string(argv[i]) == "--headless" && i + 1 < argc) + { + std::string headless = argv[i + 1]; + g_HEADLESS = headless == "true" || headless == "1" || headless == "True" || headless == "TRUE"; + break; + } + } + + return RUN_ALL_TESTS(); +} diff --git a/tests/test_primary_client.cpp b/tests/test_primary_client.cpp new file mode 100644 index 00000000..45d6fcce --- /dev/null +++ b/tests/test_primary_client.cpp @@ -0,0 +1,82 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2025 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#include + +#include +#include +#include + +using namespace urcl; + +std::string g_ROBOT_IP = "192.168.56.101"; + +class PrimaryClientTest : public ::testing::Test +{ +protected: + void SetUp() override + { + client_ = std::make_unique(g_ROBOT_IP, notifier_); + } + + std::unique_ptr client_; + comm::INotifier notifier_; +}; + +TEST_F(PrimaryClientTest, start_communication_succeeds) +{ + EXPECT_NO_THROW(client_->start()); +} + +TEST_F(PrimaryClientTest, add_and_remove_consumer) +{ + auto calibration_consumer = std::make_shared("test"); + + client_->addPrimaryConsumer(calibration_consumer); + + EXPECT_NO_THROW(client_->start()); + + auto start_time = std::chrono::system_clock::now(); + const auto timeout = std::chrono::seconds(5); + while (!calibration_consumer->isChecked() && std::chrono::system_clock::now() - start_time < timeout) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + EXPECT_TRUE(calibration_consumer->isChecked()); + + client_->removePrimaryConsumer(calibration_consumer); +} + +int main(int argc, char* argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} diff --git a/tests/test_reverse_interface.cpp b/tests/test_reverse_interface.cpp index 0d356c4b..73466cfd 100644 --- a/tests/test_reverse_interface.cpp +++ b/tests/test_reverse_interface.cpp @@ -428,7 +428,10 @@ TEST_F(ReverseIntefaceTest, deprecated_set_keep_alive_count) // Test that it works to set the keepalive count using the deprecated function int keep_alive_count = 10; +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" reverse_interface_->setKeepaliveCount(keep_alive_count); +#pragma GCC diagnostic pop int32_t expected_read_timeout = 20 * keep_alive_count; urcl::vector6d_t pos = { 0, 0, 0, 0, 0, 0 }; diff --git a/tests/test_rtde_client.cpp b/tests/test_rtde_client.cpp index a3683b63..2831d6b4 100644 --- a/tests/test_rtde_client.cpp +++ b/tests/test_rtde_client.cpp @@ -211,7 +211,7 @@ TEST_F(RTDEClientTest, set_target_frequency) data_pkg->getData("timestamp", second_time_stamp); // There should be 1 second between each timestamp - EXPECT_EQ(second_time_stamp - first_time_stamp, 1); + EXPECT_NEAR(second_time_stamp - first_time_stamp, 1, 1e-6); client_->pause(); } @@ -380,7 +380,7 @@ TEST_F(RTDEClientTest, check_all_rtde_output_variables_exist) client_.reset(new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, exhaustive_output_recipe_file_, input_recipe_file_, 0.0, true)); - EXPECT_NO_THROW(client_->init()); + EXPECT_TRUE(client_->init()); client_->start(); // Test that we can receive and parse the timestamp from the received package to prove the setup was successful diff --git a/tests/test_spline_interpolation.cpp b/tests/test_spline_interpolation.cpp index 39eec3d7..1d789355 100644 --- a/tests/test_spline_interpolation.cpp +++ b/tests/test_spline_interpolation.cpp @@ -31,6 +31,7 @@ #include #include +#include #include #include #include @@ -50,34 +51,9 @@ const std::string OUTPUT_RECIPE = "resources/rtde_output_recipe_spline.txt"; const std::string INPUT_RECIPE = "resources/rtde_input_recipe.txt"; const std::string CALIBRATION_CHECKSUM = "calib_12788084448423163542"; std::string g_ROBOT_IP = "192.168.56.101"; +bool g_HEADLESS = true; -std::unique_ptr g_ur_driver; -std::unique_ptr g_dashboard_client; - -bool g_program_running; -std::condition_variable g_program_not_running_cv; -std::mutex g_program_not_running_mutex; -std::condition_variable g_program_running_cv; -std::mutex g_program_running_mutex; - -// Helper functions for the driver -void handleRobotProgramState(bool program_running) -{ - // Print the text in green so we see it better - std::cout << "\033[1;32mProgram running: " << std::boolalpha << program_running << "\033[0m\n" << std::endl; - if (program_running) - { - std::lock_guard lk(g_program_running_mutex); - g_program_running = program_running; - g_program_running_cv.notify_one(); - } - else - { - std::lock_guard lk(g_program_not_running_mutex); - g_program_running = program_running; - g_program_not_running_cv.notify_one(); - } -} +std::unique_ptr g_my_robot; bool g_trajectory_running; control::TrajectoryResult g_trajectory_result; @@ -87,29 +63,6 @@ void handleTrajectoryState(control::TrajectoryResult state) g_trajectory_running = false; } -bool g_rtde_read_thread_running = false; -bool g_consume_rtde_packages = false; -std::mutex g_read_package_mutex; -std::thread g_rtde_read_thread; - -void rtdeConsumeThread() -{ - while (g_rtde_read_thread_running) - { - // Consume package to prevent pipeline overflow - if (g_consume_rtde_packages == true) - { - std::lock_guard lk(g_read_package_mutex); - std::unique_ptr data_pkg; - data_pkg = g_ur_driver->getDataPackage(); - } - else - { - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - } - } -} - int sign(double val) { return (0.0 < val) - (val < 0.0); @@ -126,21 +79,6 @@ class SplineInterpolationTest : public ::testing::Test protected: static void SetUpTestSuite() { - g_dashboard_client.reset(new DashboardClient(g_ROBOT_IP)); - ASSERT_TRUE(g_dashboard_client->connect()); - - // Make robot ready for test - timeval tv; - tv.tv_sec = 10; - tv.tv_usec = 0; - g_dashboard_client->setReceiveTimeout(tv); - - // Stop running program if there is one - ASSERT_TRUE(g_dashboard_client->commandStop()); - - // if the robot is not powered on and ready - ASSERT_TRUE(g_dashboard_client->commandBrakeRelease()); - // Add splineTimerTraveled to output registers to check for correct computation on test side std::ifstream in_file(SCRIPT_FILE); std::string prog((std::istreambuf_iterator(in_file)), (std::istreambuf_iterator())); @@ -159,40 +97,19 @@ class SplineInterpolationTest : public ::testing::Test out_file.close(); // Setup driver - std::unique_ptr tool_comm_setup; - const bool headless = true; - try - { - g_ur_driver.reset(new UrDriver(g_ROBOT_IP, SPLINE_SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, - &handleRobotProgramState, headless, std::move(tool_comm_setup), - CALIBRATION_CHECKSUM)); - } - catch (UrException& exp) - { - std::cout << "caught exception " << exp.what() << " while launch driver, retrying once in 10 seconds" - << std::endl; - std::this_thread::sleep_for(std::chrono::seconds(10)); - g_ur_driver.reset(new UrDriver(g_ROBOT_IP, SPLINE_SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, - &handleRobotProgramState, headless, std::move(tool_comm_setup), - CALIBRATION_CHECKSUM)); - } + g_my_robot = std::make_unique(g_ROBOT_IP, OUTPUT_RECIPE, INPUT_RECIPE, g_HEADLESS, + "external_control.urp", SPLINE_SCRIPT_FILE); - g_ur_driver->registerTrajectoryDoneCallback(&handleTrajectoryState); + g_my_robot->startRTDECommununication(true); - g_ur_driver->startRTDECommunication(); - // Setup rtde read thread - g_rtde_read_thread_running = true; - g_rtde_read_thread = std::thread(rtdeConsumeThread); + g_my_robot->ur_driver_->registerTrajectoryDoneCallback(&handleTrajectoryState); } static void TearDownTestSuite() { // Set target speed scaling to 100% as one test change this value - g_ur_driver->getRTDEWriter().sendSpeedSlider(1); + g_my_robot->ur_driver_->getRTDEWriter().sendSpeedSlider(1); - g_rtde_read_thread_running = false; - g_rtde_read_thread.join(); - g_dashboard_client->disconnect(); // Remove temporary file again std::remove(SPLINE_SCRIPT_FILE.c_str()); } @@ -200,51 +117,56 @@ class SplineInterpolationTest : public ::testing::Test void TearDown() { // Set target speed scaling to 100% as one test change this value - g_ur_driver->getRTDEWriter().sendSpeedSlider(1); + g_my_robot->ur_driver_->getRTDEWriter().sendSpeedSlider(1); } void SetUp() { step_time_ = 0.002; - if (g_ur_driver->getVersion().major < 5) + if (g_my_robot->ur_driver_->getVersion().major < 5) { step_time_ = 0.008; } - // Make sure script is running on the robot - if (g_program_running == false) + std::string safety_status; + g_my_robot->dashboard_client_->commandSafetyStatus(safety_status); + bool is_protective_stopped = safety_status.find("PROTECTIVE_STOP") != std::string::npos; + if (is_protective_stopped) { - g_consume_rtde_packages = true; - g_ur_driver->sendRobotProgram(); - ASSERT_TRUE(waitForProgramRunning(1000)); + // We forced a protective stop above. Some versions require waiting 5 seconds before releasing + // the protective stop. + std::this_thread::sleep_for(std::chrono::seconds(5)); + g_my_robot->dashboard_client_->commandCloseSafetyPopup(); + ASSERT_TRUE(g_my_robot->dashboard_client_->commandUnlockProtectiveStop()); } - g_consume_rtde_packages = false; + ASSERT_TRUE(g_my_robot->isHealthy()); } void sendIdle() { - ASSERT_TRUE(g_ur_driver->writeKeepalive(RobotReceiveTimeout::sec())); + ASSERT_TRUE(g_my_robot->ur_driver_->writeKeepalive(RobotReceiveTimeout::sec())); } void sendTrajectory(const std::vector& s_pos, const std::vector& s_vel, const std::vector& s_acc, const std::vector& s_time) { - ASSERT_TRUE(g_ur_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP)); + ASSERT_TRUE(g_my_robot->ur_driver_->writeTrajectoryControlMessage( + urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP)); // Send trajectory to robot for execution - ASSERT_TRUE(g_ur_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, - s_pos.size())); + ASSERT_TRUE(g_my_robot->ur_driver_->writeTrajectoryControlMessage( + urcl::control::TrajectoryControlMessage::TRAJECTORY_START, s_pos.size())); for (size_t i = 0; i < s_pos.size(); i++) { // QUINTIC if (s_pos.size() == s_acc.size()) { - g_ur_driver->writeTrajectorySplinePoint(s_pos[i], s_vel[i], s_acc[i], s_time[i]); + g_my_robot->ur_driver_->writeTrajectorySplinePoint(s_pos[i], s_vel[i], s_acc[i], s_time[i]); } // CUBIC else { - g_ur_driver->writeTrajectorySplinePoint(s_pos[i], s_vel[i], s_time[i]); + g_my_robot->ur_driver_->writeTrajectorySplinePoint(s_pos[i], s_vel[i], s_time[i]); } } } @@ -324,12 +246,13 @@ class SplineInterpolationTest : public ::testing::Test while (trajectory_started == false) { std::unique_ptr data_pkg; - readDataPackage(data_pkg); + g_my_robot->readDataPackage(data_pkg); double spline_travel_time = 0.0; data_pkg->getData("output_double_register_1", spline_travel_time); // Keep connection alive - ASSERT_TRUE(g_ur_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP)); + ASSERT_TRUE(g_my_robot->ur_driver_->writeTrajectoryControlMessage( + urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP)); if (std::abs(spline_travel_time - 0.0) < 0.01) { return; @@ -345,44 +268,6 @@ class SplineInterpolationTest : public ::testing::Test } } - bool waitForProgramRunning(int milliseconds = 100) - { - std::unique_lock lk(g_program_running_mutex); - if (g_program_running_cv.wait_for(lk, std::chrono::milliseconds(milliseconds)) == std::cv_status::no_timeout || - g_program_running == true) - { - return true; - } - return false; - } - - bool waitForProgramNotRunning(int milliseconds = 100) - { - std::unique_lock lk(g_program_not_running_mutex); - if (g_program_not_running_cv.wait_for(lk, std::chrono::milliseconds(milliseconds)) == std::cv_status::no_timeout || - g_program_running == false) - { - return true; - } - return false; - } - - void readDataPackage(std::unique_ptr& data_pkg) - { - if (g_consume_rtde_packages == true) - { - URCL_LOG_ERROR("Unable to read packages while consuming, this should not happen!"); - GTEST_FAIL(); - } - std::lock_guard lk(g_read_package_mutex); - data_pkg = g_ur_driver->getDataPackage(); - if (data_pkg == nullptr) - { - URCL_LOG_ERROR("Timed out waiting for a new package from the robot"); - GTEST_FAIL(); - } - } - void writeTrajectoryToFile(const char* filename, std::vector time_vec, std::vector expected_positions, std::vector actual_positions, @@ -452,8 +337,10 @@ class SplineInterpolationTest : public ::testing::Test TEST_F(SplineInterpolationTest, cubic_spline_with_end_point_velocity) { + g_my_robot->stopConsumingRTDEData(); std::unique_ptr data_pkg; - readDataPackage(data_pkg); + g_my_robot->readDataPackage(data_pkg); + ASSERT_TRUE(data_pkg != nullptr); urcl::vector6d_t joint_positions, joint_velocities, joint_acc; ASSERT_TRUE(data_pkg->getData("target_q", joint_positions)); @@ -491,14 +378,15 @@ TEST_F(SplineInterpolationTest, cubic_spline_with_end_point_velocity) double plot_time = 0.0; while (g_trajectory_running) { - readDataPackage(data_pkg); + g_my_robot->readDataPackage(data_pkg); ASSERT_TRUE(data_pkg->getData("target_q", joint_positions)); ASSERT_TRUE(data_pkg->getData("target_qd", joint_velocities)); ASSERT_TRUE(data_pkg->getData("target_qdd", joint_acc)); ASSERT_TRUE(data_pkg->getData("speed_scaling", speed_scaling)); // Keep connection alive - ASSERT_TRUE(g_ur_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP)); + ASSERT_TRUE(g_my_robot->ur_driver_->writeTrajectoryControlMessage( + urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP)); // Read spline travel time from the robot double spline_travel_time = 0.0; @@ -544,7 +432,7 @@ TEST_F(SplineInterpolationTest, cubic_spline_with_end_point_velocity) EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS, g_trajectory_result); // Make sure the velocity is zero when the trajectory has finished - readDataPackage(data_pkg); + g_my_robot->readDataPackage(data_pkg); ASSERT_TRUE(data_pkg->getData("target_qd", joint_velocities)); for (unsigned int i = 0; i < 6; ++i) { @@ -562,16 +450,17 @@ TEST_F(SplineInterpolationTest, cubic_spline_with_end_point_velocity) TEST_F(SplineInterpolationTest, quintic_spline_with_end_point_velocity_with_speedscaling) { + g_my_robot->stopConsumingRTDEData(); // Set speed scaling to 25% to test interpolation with speed scaling active const unsigned int reduse_factor(4); - g_ur_driver->getRTDEWriter().sendSpeedSlider(1.0 / reduse_factor); + g_my_robot->ur_driver_->getRTDEWriter().sendSpeedSlider(1.0 / reduse_factor); std::unique_ptr data_pkg; - readDataPackage(data_pkg); + g_my_robot->readDataPackage(data_pkg); // Align timestep sendIdle(); - readDataPackage(data_pkg); + g_my_robot->readDataPackage(data_pkg); urcl::vector6d_t joint_positions, joint_velocities, joint_acc; ASSERT_TRUE(data_pkg->getData("target_q", joint_positions)); @@ -616,7 +505,7 @@ TEST_F(SplineInterpolationTest, quintic_spline_with_end_point_velocity_with_spee const double eps_acc_change(1e-15); while (g_trajectory_running) { - readDataPackage(data_pkg); + g_my_robot->readDataPackage(data_pkg); ASSERT_TRUE(data_pkg->getData("target_q", joint_positions)); ASSERT_TRUE(data_pkg->getData("target_qd", joint_velocities)); ASSERT_TRUE(data_pkg->getData("target_qdd", joint_acc)); @@ -634,7 +523,8 @@ TEST_F(SplineInterpolationTest, quintic_spline_with_end_point_velocity_with_spee double time_left = s_time[0] - spline_travel_time; // Keep connection alive - ASSERT_TRUE(g_ur_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP)); + ASSERT_TRUE(g_my_robot->ur_driver_->writeTrajectoryControlMessage( + urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP)); // Ensure that we follow the joint trajectory urcl::vector6d_t expected_joint_positions, change_acc; @@ -708,7 +598,7 @@ TEST_F(SplineInterpolationTest, quintic_spline_with_end_point_velocity_with_spee EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS, g_trajectory_result); // Make sure the velocity is zero when the trajectory has finished - readDataPackage(data_pkg); + g_my_robot->readDataPackage(data_pkg); ASSERT_TRUE(data_pkg->getData("target_qd", joint_velocities)); for (unsigned int i = 0; i < 6; ++i) { @@ -727,8 +617,9 @@ TEST_F(SplineInterpolationTest, quintic_spline_with_end_point_velocity_with_spee TEST_F(SplineInterpolationTest, spline_interpolation_cubic) { + g_my_robot->stopConsumingRTDEData(); std::unique_ptr data_pkg; - readDataPackage(data_pkg); + g_my_robot->readDataPackage(data_pkg); urcl::vector6d_t joint_positions, joint_velocities, joint_acc; ASSERT_TRUE(data_pkg->getData("target_q", joint_positions)); ASSERT_TRUE(data_pkg->getData("target_qd", joint_velocities)); @@ -776,7 +667,7 @@ TEST_F(SplineInterpolationTest, spline_interpolation_cubic) double plot_time = 0.0; while (g_trajectory_running) { - readDataPackage(data_pkg); + g_my_robot->readDataPackage(data_pkg); ASSERT_TRUE(data_pkg->getData("target_q", joint_positions)); ASSERT_TRUE(data_pkg->getData("target_qd", joint_velocities)); ASSERT_TRUE(data_pkg->getData("target_qdd", joint_acc)); @@ -789,7 +680,8 @@ TEST_F(SplineInterpolationTest, spline_interpolation_cubic) spline_travel_time = (spline_travel_time == 0 ? spline_travel_time : spline_travel_time - step_time_); // Keep connection alive - ASSERT_TRUE(g_ur_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP)); + ASSERT_TRUE(g_my_robot->ur_driver_->writeTrajectoryControlMessage( + urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP)); if (old_spline_travel_time > spline_travel_time) { @@ -830,8 +722,9 @@ TEST_F(SplineInterpolationTest, spline_interpolation_cubic) TEST_F(SplineInterpolationTest, spline_interpolation_quintic) { + g_my_robot->stopConsumingRTDEData(); std::unique_ptr data_pkg; - readDataPackage(data_pkg); + g_my_robot->readDataPackage(data_pkg); urcl::vector6d_t joint_positions, joint_velocities, joint_acc; ASSERT_TRUE(data_pkg->getData("target_q", joint_positions)); @@ -887,7 +780,7 @@ TEST_F(SplineInterpolationTest, spline_interpolation_quintic) g_trajectory_running = true; while (g_trajectory_running) { - readDataPackage(data_pkg); + g_my_robot->readDataPackage(data_pkg); ASSERT_TRUE(data_pkg->getData("target_q", joint_positions)); ASSERT_TRUE(data_pkg->getData("target_qd", joint_velocities)); ASSERT_TRUE(data_pkg->getData("target_qdd", joint_acc)); @@ -900,7 +793,8 @@ TEST_F(SplineInterpolationTest, spline_interpolation_quintic) spline_travel_time = (spline_travel_time == 0 ? spline_travel_time : spline_travel_time - step_time_); // Keep connection alive - ASSERT_TRUE(g_ur_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP)); + ASSERT_TRUE(g_my_robot->ur_driver_->writeTrajectoryControlMessage( + urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP)); if (old_spline_travel_time > spline_travel_time) { @@ -940,14 +834,15 @@ TEST_F(SplineInterpolationTest, spline_interpolation_quintic) TEST_F(SplineInterpolationTest, zero_time_trajectory_cubic_spline) { + g_my_robot->stopConsumingRTDEData(); std::unique_ptr data_pkg; - readDataPackage(data_pkg); + g_my_robot->readDataPackage(data_pkg); urcl::vector6d_t joint_positions_before; ASSERT_TRUE(data_pkg->getData("target_q", joint_positions_before)); // Start consuming rtde packages to avoid pipeline overflows while testing that the control script aborts correctly - g_consume_rtde_packages = true; + g_my_robot->startConsumingRTDEData(); // Create illegal trajectory std::vector s_pos, s_vel; @@ -969,19 +864,19 @@ TEST_F(SplineInterpolationTest, zero_time_trajectory_cubic_spline) // Send illegal trajectory to the robot sendTrajectory(s_pos, s_vel, std::vector(), s_time); - g_ur_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP, -1, - RobotReceiveTimeout::off()); + g_my_robot->ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP, -1, + RobotReceiveTimeout::off()); // When an illegal trajectory is send to the robot, the control script should keep running but the trajectory result // should be canceled. - ASSERT_FALSE(waitForProgramNotRunning(1000)); + ASSERT_FALSE(g_my_robot->waitForProgramNotRunning(1000)); EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_CANCELED, g_trajectory_result); // Stop consuming rtde packages - g_consume_rtde_packages = false; + g_my_robot->stopConsumingRTDEData(); // Ensure that the robot hasn't moved - readDataPackage(data_pkg); + g_my_robot->readDataPackage(data_pkg); urcl::vector6d_t joint_positions_after; ASSERT_TRUE(data_pkg->getData("target_q", joint_positions_after)); for (unsigned int i = 0; i < 6; ++i) @@ -1006,10 +901,11 @@ TEST_F(SplineInterpolationTest, zero_time_trajectory_cubic_spline) urcl::vector6d_t joint_positions; while (g_trajectory_running) { - readDataPackage(data_pkg); + g_my_robot->readDataPackage(data_pkg); ASSERT_TRUE(data_pkg->getData("target_q", joint_positions)); // Keep connection alive - ASSERT_TRUE(g_ur_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP)); + ASSERT_TRUE(g_my_robot->ur_driver_->writeTrajectoryControlMessage( + urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP)); } EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS, g_trajectory_result); @@ -1022,14 +918,15 @@ TEST_F(SplineInterpolationTest, zero_time_trajectory_cubic_spline) TEST_F(SplineInterpolationTest, zero_time_trajectory_quintic_spline) { + g_my_robot->stopConsumingRTDEData(); std::unique_ptr data_pkg; - readDataPackage(data_pkg); + g_my_robot->readDataPackage(data_pkg); urcl::vector6d_t joint_positions_before; ASSERT_TRUE(data_pkg->getData("target_q", joint_positions_before)); // Start consuming rtde packages to avoid pipeline overflows while testing - g_consume_rtde_packages = true; + g_my_robot->startConsumingRTDEData(); // Create illegal trajectory std::vector s_pos, s_vel, s_acc; @@ -1047,19 +944,19 @@ TEST_F(SplineInterpolationTest, zero_time_trajectory_quintic_spline) // Send illegal trajectory to the robot sendTrajectory(s_pos, s_vel, s_acc, s_time); - g_ur_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP, -1, - RobotReceiveTimeout::off()); + g_my_robot->ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP, -1, + RobotReceiveTimeout::off()); // When an illegal trajectory is send to the robot, the control script should keep running but the trajectory result // should be canceled - ASSERT_FALSE(waitForProgramNotRunning(1000)); + ASSERT_FALSE(g_my_robot->waitForProgramNotRunning(1000)); EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_CANCELED, g_trajectory_result); // Stop consuming rtde packages - g_consume_rtde_packages = false; + g_my_robot->stopConsumingRTDEData(); // Ensure that the robot hasn't moved - readDataPackage(data_pkg); + g_my_robot->readDataPackage(data_pkg); urcl::vector6d_t joint_positions_after; ASSERT_TRUE(data_pkg->getData("target_q", joint_positions_after)); for (unsigned int i = 0; i < 6; ++i) @@ -1085,10 +982,11 @@ TEST_F(SplineInterpolationTest, zero_time_trajectory_quintic_spline) urcl::vector6d_t joint_positions; while (g_trajectory_running) { - readDataPackage(data_pkg); + g_my_robot->readDataPackage(data_pkg); ASSERT_TRUE(data_pkg->getData("target_q", joint_positions)); // Keep connection alive - ASSERT_TRUE(g_ur_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP)); + ASSERT_TRUE(g_my_robot->ur_driver_->writeTrajectoryControlMessage( + urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP)); } EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS, g_trajectory_result); @@ -1101,14 +999,15 @@ TEST_F(SplineInterpolationTest, zero_time_trajectory_quintic_spline) TEST_F(SplineInterpolationTest, physically_unfeasible_trajectory_cubic_spline) { + g_my_robot->stopConsumingRTDEData(); std::unique_ptr data_pkg; - readDataPackage(data_pkg); + g_my_robot->readDataPackage(data_pkg); urcl::vector6d_t joint_positions_before; ASSERT_TRUE(data_pkg->getData("target_q", joint_positions_before)); // Start consuming rtde packages to avoid pipeline overflows while testing that the control script aborts correctly - g_consume_rtde_packages = true; + g_my_robot->startConsumingRTDEData(); // Create a trajectory that cannot be executed within the robots limits std::vector s_pos, s_vel; @@ -1125,19 +1024,19 @@ TEST_F(SplineInterpolationTest, physically_unfeasible_trajectory_cubic_spline) // Send unfeasible trajectory to the robot sendTrajectory(s_pos, s_vel, std::vector(), s_time); - g_ur_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP, -1, - RobotReceiveTimeout::off()); + g_my_robot->ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP, -1, + RobotReceiveTimeout::off()); // When an unfeasible trajectory is send to the robot, the control script should keep running but the trajectory // result should be canceled - ASSERT_FALSE(waitForProgramNotRunning(1000)); + ASSERT_FALSE(g_my_robot->waitForProgramNotRunning(1000)); EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_CANCELED, g_trajectory_result); // Stop consuming rtde packages - g_consume_rtde_packages = false; + g_my_robot->stopConsumingRTDEData(); // Ensure that the robot hasn't moved - readDataPackage(data_pkg); + g_my_robot->readDataPackage(data_pkg); urcl::vector6d_t joint_positions_after; ASSERT_TRUE(data_pkg->getData("target_q", joint_positions_after)); for (unsigned int i = 0; i < 6; ++i) @@ -1148,14 +1047,15 @@ TEST_F(SplineInterpolationTest, physically_unfeasible_trajectory_cubic_spline) TEST_F(SplineInterpolationTest, physically_unfeasible_trajectory_quintic_spline) { + g_my_robot->stopConsumingRTDEData(); std::unique_ptr data_pkg; - readDataPackage(data_pkg); + g_my_robot->readDataPackage(data_pkg); urcl::vector6d_t joint_positions_before; ASSERT_TRUE(data_pkg->getData("target_q", joint_positions_before)); // Start consuming rtde packages to avoid pipeline overflows while testing that the control script aborts correctly - g_consume_rtde_packages = true; + g_my_robot->startConsumingRTDEData(); // Create a trajectory that cannot be executed within the robots limits std::vector s_pos, s_vel, s_acc; @@ -1173,19 +1073,19 @@ TEST_F(SplineInterpolationTest, physically_unfeasible_trajectory_quintic_spline) // Send unfeasible trajectory to the robot sendTrajectory(s_pos, s_vel, s_acc, s_time); - g_ur_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP, -1, - RobotReceiveTimeout::off()); + g_my_robot->ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP, -1, + RobotReceiveTimeout::off()); // When an unfeasible trajectory is send to the robot, the control script should keep running but the trajectory // result should be canceled - ASSERT_FALSE(waitForProgramNotRunning(1000)); + ASSERT_FALSE(g_my_robot->waitForProgramNotRunning(1000)); EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_CANCELED, g_trajectory_result); // Stop consuming rtde packages - g_consume_rtde_packages = false; + g_my_robot->stopConsumingRTDEData(); // Ensure that the robot hasn't moved - readDataPackage(data_pkg); + g_my_robot->readDataPackage(data_pkg); urcl::vector6d_t joint_positions_after; ASSERT_TRUE(data_pkg->getData("target_q", joint_positions_after)); for (unsigned int i = 0; i < 6; ++i) @@ -1205,6 +1105,12 @@ int main(int argc, char* argv[]) g_ROBOT_IP = argv[i + 1]; break; } + if (std::string(argv[i]) == "--headless" && i + 1 < argc) + { + std::string headless = argv[i + 1]; + g_HEADLESS = headless == "true" || headless == "1" || headless == "True" || headless == "TRUE"; + break; + } } return RUN_ALL_TESTS(); diff --git a/tests/test_ur_driver.cpp b/tests/test_ur_driver.cpp index 1c236c3f..cd00b01d 100644 --- a/tests/test_ur_driver.cpp +++ b/tests/test_ur_driver.cpp @@ -32,6 +32,7 @@ #include #include +#include using namespace urcl; @@ -40,188 +41,30 @@ const std::string OUTPUT_RECIPE = "resources/rtde_output_recipe.txt"; const std::string INPUT_RECIPE = "resources/rtde_input_recipe.txt"; const std::string CALIBRATION_CHECKSUM = "calib_12788084448423163542"; std::string g_ROBOT_IP = "192.168.56.101"; +bool g_HEADLESS = true; -class TestableUrDriver : public UrDriver -{ -public: - TestableUrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& output_recipe_file, - const std::string& input_recipe_file, std::function handle_program_state, - bool headless_mode, std::unique_ptr tool_comm_setup, - const std::string& calibration_checksum) - : UrDriver(robot_ip, script_file, output_recipe_file, input_recipe_file, handle_program_state, headless_mode, - std::move(tool_comm_setup), calibration_checksum) - { - } - void closeSecondaryStream() - { - secondary_stream_->close(); - } -}; - -std::unique_ptr g_ur_driver; -std::unique_ptr g_dashboard_client; - -bool g_program_running; -std::condition_variable g_program_not_running_cv; -std::mutex g_program_not_running_mutex; -std::condition_variable g_program_running_cv; -std::mutex g_program_running_mutex; - -// Helper functions for the driver -void handleRobotProgramState(bool program_running) -{ - // Print the text in green so we see it better - std::cout << "\033[1;32mProgram running: " << std::boolalpha << program_running << "\033[0m\n" << std::endl; - if (program_running) - { - std::lock_guard lk(g_program_running_mutex); - g_program_running = program_running; - g_program_running_cv.notify_one(); - } - else - { - std::lock_guard lk(g_program_not_running_mutex); - g_program_running = program_running; - g_program_not_running_cv.notify_one(); - } -} - -bool g_rtde_read_thread_running = false; -bool g_consume_rtde_packages = false; -std::mutex g_read_package_mutex; -std::thread g_rtde_read_thread; - -void rtdeConsumeThread() -{ - while (g_rtde_read_thread_running) - { - // Consume package to prevent pipeline overflow - if (g_consume_rtde_packages == true) - { - std::lock_guard lk(g_read_package_mutex); - std::unique_ptr data_pkg; - data_pkg = g_ur_driver->getDataPackage(); - } - else - { - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - } - } -} +std::unique_ptr g_my_robot; class UrDriverTest : public ::testing::Test { protected: static void SetUpTestSuite() { - g_dashboard_client.reset(new DashboardClient(g_ROBOT_IP)); - ASSERT_TRUE(g_dashboard_client->connect()); - - // Make robot ready for test - timeval tv; - tv.tv_sec = 10; - tv.tv_usec = 0; - g_dashboard_client->setReceiveTimeout(tv); - - // Stop running program if there is one - ASSERT_TRUE(g_dashboard_client->commandStop()); - - // if the robot is not powered on and ready - ASSERT_TRUE(g_dashboard_client->commandBrakeRelease()); - // Setup driver - std::unique_ptr tool_comm_setup; - const bool headless = true; - try - { - g_ur_driver.reset(new TestableUrDriver(g_ROBOT_IP, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, - &handleRobotProgramState, headless, std::move(tool_comm_setup), - CALIBRATION_CHECKSUM)); - } - catch (UrException& exp) - { - std::cout << "caught exception " << exp.what() << " while launch driver, retrying once in 10 seconds" - << std::endl; - std::this_thread::sleep_for(std::chrono::seconds(10)); - g_ur_driver.reset(new TestableUrDriver(g_ROBOT_IP, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, - &handleRobotProgramState, headless, std::move(tool_comm_setup), - CALIBRATION_CHECKSUM)); - } - g_ur_driver->startRTDECommunication(); - // Setup rtde read thread - g_rtde_read_thread_running = true; - g_rtde_read_thread = std::thread(rtdeConsumeThread); - } + g_my_robot = std::make_unique(g_ROBOT_IP, OUTPUT_RECIPE, INPUT_RECIPE, g_HEADLESS, + "external_control.urp", SCRIPT_FILE); - void SetUp() - { - step_time_ = 0.002; - if (g_ur_driver->getVersion().major < 5) - { - step_time_ = 0.008; - } - // Make sure script is running on the robot - if (g_program_running == false) - { - g_consume_rtde_packages = true; - g_ur_driver->sendRobotProgram(); - ASSERT_TRUE(waitForProgramRunning(1000)); - } - g_consume_rtde_packages = false; + g_my_robot->startRTDECommununication(true); } - static void TearDownTestSuite() - { - g_rtde_read_thread_running = false; - g_rtde_read_thread.join(); - g_dashboard_client->disconnect(); - } - - void readDataPackage(std::unique_ptr& data_pkg) - { - if (g_consume_rtde_packages == true) - { - URCL_LOG_ERROR("Unable to read packages while consuming, this should not happen!"); - GTEST_FAIL(); - } - std::lock_guard lk(g_read_package_mutex); - data_pkg = g_ur_driver->getDataPackage(); - if (data_pkg == nullptr) - { - URCL_LOG_ERROR("Timed out waiting for a new package from the robot"); - GTEST_FAIL(); - } - } - - bool waitForProgramRunning(int milliseconds = 100) - { - std::unique_lock lk(g_program_running_mutex); - if (g_program_running_cv.wait_for(lk, std::chrono::milliseconds(milliseconds)) == std::cv_status::no_timeout || - g_program_running == true) - { - return true; - } - return false; - } - - bool waitForProgramNotRunning(int milliseconds = 100) + void SetUp() { - std::unique_lock lk(g_program_not_running_mutex); - if (g_program_not_running_cv.wait_for(lk, std::chrono::milliseconds(milliseconds)) == std::cv_status::no_timeout || - g_program_running == false) - { - return true; - } - return false; + ASSERT_TRUE(g_my_robot->isHealthy()); } - - // Robot step time - double step_time_; }; TEST_F(UrDriverTest, read_non_existing_script_file) { - g_consume_rtde_packages = true; const std::string non_existing_script_file = ""; EXPECT_THROW(UrDriver::readScriptFile(non_existing_script_file), UrException); } @@ -229,9 +72,8 @@ TEST_F(UrDriverTest, read_non_existing_script_file) TEST_F(UrDriverTest, read_existing_script_file) { #ifdef _WIN32 -#define mkstemp _mktemp_s +# define mkstemp _mktemp_s #endif - g_consume_rtde_packages = true; char existing_script_file[] = "urscript.XXXXXX"; int fd = mkstemp(existing_script_file); if (fd == -1) @@ -248,80 +90,75 @@ TEST_F(UrDriverTest, read_existing_script_file) TEST_F(UrDriverTest, robot_receive_timeout) { - g_consume_rtde_packages = true; - // Robot program should time out after the robot receive timeout, whether it takes exactly 200 ms is not so important vector6d_t zeros = { 0, 0, 0, 0, 0, 0 }; - g_ur_driver->writeJointCommand(zeros, comm::ControlMode::MODE_IDLE, RobotReceiveTimeout::millisec(200)); - EXPECT_TRUE(waitForProgramNotRunning(400)); + g_my_robot->ur_driver_->writeJointCommand(zeros, comm::ControlMode::MODE_IDLE, RobotReceiveTimeout::millisec(200)); + EXPECT_TRUE(g_my_robot->waitForProgramNotRunning(400)); // Start robot program - g_ur_driver->sendRobotProgram(); - EXPECT_TRUE(waitForProgramRunning(1000)); + g_my_robot->resendRobotProgram(); + EXPECT_TRUE(g_my_robot->waitForProgramRunning(1000)); // Robot program should time out after the robot receive timeout, whether it takes exactly 200 ms is not so important - g_ur_driver->writeFreedriveControlMessage(control::FreedriveControlMessage::FREEDRIVE_NOOP, - RobotReceiveTimeout::millisec(200)); - EXPECT_TRUE(waitForProgramNotRunning(400)); + g_my_robot->ur_driver_->writeFreedriveControlMessage(control::FreedriveControlMessage::FREEDRIVE_NOOP, + RobotReceiveTimeout::millisec(200)); + EXPECT_TRUE(g_my_robot->waitForProgramNotRunning(400)); // Start robot program - g_ur_driver->sendRobotProgram(); - EXPECT_TRUE(waitForProgramRunning(1000)); + g_my_robot->resendRobotProgram(); + EXPECT_TRUE(g_my_robot->waitForProgramRunning(1000)); // Robot program should time out after the robot receive timeout, whether it takes exactly 200 ms is not so important - g_ur_driver->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_NOOP, -1, - RobotReceiveTimeout::millisec(200)); - EXPECT_TRUE(waitForProgramNotRunning(400)); + g_my_robot->ur_driver_->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_NOOP, -1, + RobotReceiveTimeout::millisec(200)); + EXPECT_TRUE(g_my_robot->waitForProgramNotRunning(400)); // Start robot program - g_ur_driver->sendRobotProgram(); - EXPECT_TRUE(waitForProgramRunning(1000)); + g_my_robot->resendRobotProgram(); + EXPECT_TRUE(g_my_robot->waitForProgramRunning(1000)); // Robot program should time out after the robot receive timeout, whether it takes exactly 200 ms is not so important - g_ur_driver->writeKeepalive(RobotReceiveTimeout::millisec(200)); - EXPECT_TRUE(waitForProgramNotRunning(400)); + g_my_robot->ur_driver_->writeKeepalive(RobotReceiveTimeout::millisec(200)); + EXPECT_TRUE(g_my_robot->waitForProgramNotRunning(400)); } TEST_F(UrDriverTest, robot_receive_timeout_off) { - g_consume_rtde_packages = true; - // Program should keep running when setting receive timeout off - g_ur_driver->writeKeepalive(RobotReceiveTimeout::off()); - EXPECT_FALSE(waitForProgramNotRunning(1000)); + g_my_robot->ur_driver_->writeKeepalive(RobotReceiveTimeout::off()); + EXPECT_FALSE(g_my_robot->waitForProgramNotRunning(1000)); // Program should keep running when setting receive timeout off - g_ur_driver->writeFreedriveControlMessage(control::FreedriveControlMessage::FREEDRIVE_NOOP, - RobotReceiveTimeout::off()); - EXPECT_FALSE(waitForProgramNotRunning(1000)); + g_my_robot->ur_driver_->writeFreedriveControlMessage(control::FreedriveControlMessage::FREEDRIVE_NOOP, + RobotReceiveTimeout::off()); + EXPECT_FALSE(g_my_robot->waitForProgramNotRunning(1000)); // Program should keep running when setting receive timeout off - g_ur_driver->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_NOOP, -1, - RobotReceiveTimeout::off()); - EXPECT_FALSE(waitForProgramNotRunning(1000)); + g_my_robot->ur_driver_->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_NOOP, -1, + RobotReceiveTimeout::off()); + EXPECT_FALSE(g_my_robot->waitForProgramNotRunning(1000)); // It shouldn't be possible to set robot receive timeout off, when dealing with realtime commands vector6d_t zeros = { 0, 0, 0, 0, 0, 0 }; - g_ur_driver->writeJointCommand(zeros, comm::ControlMode::MODE_SPEEDJ, RobotReceiveTimeout::off()); - EXPECT_TRUE(waitForProgramNotRunning(400)); + g_my_robot->ur_driver_->writeJointCommand(zeros, comm::ControlMode::MODE_SPEEDJ, RobotReceiveTimeout::off()); + EXPECT_TRUE(g_my_robot->waitForProgramNotRunning(400)); } TEST_F(UrDriverTest, stop_robot_control) { - g_consume_rtde_packages = true; - vector6d_t zeros = { 0, 0, 0, 0, 0, 0 }; - g_ur_driver->writeJointCommand(zeros, comm::ControlMode::MODE_IDLE, RobotReceiveTimeout::off()); + g_my_robot->ur_driver_->writeJointCommand(zeros, comm::ControlMode::MODE_IDLE, RobotReceiveTimeout::off()); // Make sure that we can stop the robot control, when robot receive timeout has been set off - g_ur_driver->stopControl(); - EXPECT_TRUE(waitForProgramNotRunning(400)); + g_my_robot->ur_driver_->stopControl(); + EXPECT_TRUE(g_my_robot->waitForProgramNotRunning(400)); } TEST_F(UrDriverTest, target_outside_limits_servoj) { + g_my_robot->stopConsumingRTDEData(); std::unique_ptr data_pkg; - readDataPackage(data_pkg); + g_my_robot->readDataPackage(data_pkg); urcl::vector6d_t joint_positions_before; ASSERT_TRUE(data_pkg->getData("actual_q", joint_positions_before)); @@ -331,11 +168,12 @@ TEST_F(UrDriverTest, target_outside_limits_servoj) joint_target[5] -= 2.5; // Send unfeasible targets to the robot - readDataPackage(data_pkg); - g_ur_driver->writeJointCommand(joint_target, comm::ControlMode::MODE_SERVOJ, RobotReceiveTimeout::millisec(200)); + g_my_robot->readDataPackage(data_pkg); + g_my_robot->ur_driver_->writeJointCommand(joint_target, comm::ControlMode::MODE_SERVOJ, + RobotReceiveTimeout::millisec(200)); // Ensure that the robot didn't move - readDataPackage(data_pkg); + g_my_robot->readDataPackage(data_pkg); urcl::vector6d_t joint_positions; ASSERT_TRUE(data_pkg->getData("actual_q", joint_positions)); for (unsigned int i = 0; i < 6; ++i) @@ -344,15 +182,16 @@ TEST_F(UrDriverTest, target_outside_limits_servoj) } // Make sure the program is stopped - g_consume_rtde_packages = true; - g_ur_driver->stopControl(); - waitForProgramNotRunning(1000); + g_my_robot->startConsumingRTDEData(); + g_my_robot->ur_driver_->stopControl(); + g_my_robot->waitForProgramNotRunning(1000); } TEST_F(UrDriverTest, target_outside_limits_pose) { + g_my_robot->stopConsumingRTDEData(); std::unique_ptr data_pkg; - readDataPackage(data_pkg); + g_my_robot->readDataPackage(data_pkg); urcl::vector6d_t tcp_pose_before; ASSERT_TRUE(data_pkg->getData("actual_TCP_pose", tcp_pose_before)); @@ -362,11 +201,12 @@ TEST_F(UrDriverTest, target_outside_limits_pose) tcp_target[2] += 0.3; // Send unfeasible targets to the robot - readDataPackage(data_pkg); - g_ur_driver->writeJointCommand(tcp_target, comm::ControlMode::MODE_POSE, RobotReceiveTimeout::millisec(200)); + g_my_robot->readDataPackage(data_pkg); + g_my_robot->ur_driver_->writeJointCommand(tcp_target, comm::ControlMode::MODE_POSE, + RobotReceiveTimeout::millisec(200)); // Ensure that the robot didn't move - readDataPackage(data_pkg); + g_my_robot->readDataPackage(data_pkg); urcl::vector6d_t tcp_pose; ASSERT_TRUE(data_pkg->getData("actual_TCP_pose", tcp_pose)); for (unsigned int i = 0; i < 6; ++i) @@ -375,29 +215,71 @@ TEST_F(UrDriverTest, target_outside_limits_pose) } // Make sure the program is stopped - g_consume_rtde_packages = true; - g_ur_driver->stopControl(); - waitForProgramNotRunning(1000); + g_my_robot->startConsumingRTDEData(); + g_my_robot->ur_driver_->stopControl(); + g_my_robot->waitForProgramNotRunning(1000); } TEST_F(UrDriverTest, send_robot_program_retry_on_failure) { - // Start robot program - g_ur_driver->sendRobotProgram(); - EXPECT_TRUE(waitForProgramRunning(1000)); - // Check that sendRobotProgram is robust to the secondary stream being disconnected. This is what happens when // switching from Remote to Local and back to Remote mode for example. - g_ur_driver->closeSecondaryStream(); + g_my_robot->ur_driver_->closeSecondaryStream(); + + std::this_thread::sleep_for(std::chrono::milliseconds(100)); - EXPECT_TRUE(g_ur_driver->sendRobotProgram()); + EXPECT_TRUE(g_my_robot->resendRobotProgram()); + + EXPECT_TRUE(g_my_robot->waitForProgramRunning()); } TEST_F(UrDriverTest, reset_rtde_client) { + g_my_robot->stopConsumingRTDEData(); double target_frequency = 50; - g_ur_driver->resetRTDEClient(OUTPUT_RECIPE, INPUT_RECIPE, target_frequency); - ASSERT_EQ(g_ur_driver->getControlFrequency(), target_frequency); + g_my_robot->ur_driver_->resetRTDEClient(OUTPUT_RECIPE, INPUT_RECIPE, target_frequency); + ASSERT_EQ(g_my_robot->ur_driver_->getControlFrequency(), target_frequency); +} + +TEST_F(UrDriverTest, read_error_code) +{ + g_my_robot->ur_driver_->startPrimaryClientCommunication(); + // Wait until we actually received a package + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + std::stringstream cmd; + cmd << "sec setup():" << std::endl << " protective_stop()" << std::endl << "end"; + EXPECT_TRUE(g_my_robot->ur_driver_->sendScript(cmd.str())); + + auto error_codes = g_my_robot->ur_driver_->getErrorCodes(); + while (error_codes.size() == 0) + { + error_codes = g_my_robot->ur_driver_->getErrorCodes(); + } + + ASSERT_EQ(error_codes.size(), 1); + // Check whether it is a "A protective stop was triggered" + // https://www.universal-robots.com/manuals/EN/HTML/SW5_21/Content/prod-err-codes/topics/CODE_209.html + ASSERT_EQ(error_codes.at(0).message_code, 209); + ASSERT_EQ(error_codes.at(0).message_argument, 0); + + // Wait for after PSTOP before clearing it + std::this_thread::sleep_for(std::chrono::seconds(6)); + + EXPECT_TRUE(g_my_robot->dashboard_client_->commandCloseSafetyPopup()); + EXPECT_TRUE(g_my_robot->dashboard_client_->commandUnlockProtectiveStop()); +} + +TEST(UrDriverInitTest, setting_connection_limits_works_correctly) +{ + UrDriverConfiguration config; + config.socket_reconnect_attempts = 1; + config.socket_reconnection_timeout = std::chrono::milliseconds(200); + config.robot_ip = "192.168.56.100"; // That IP address should not exist on the test network + config.input_recipe_file = INPUT_RECIPE; + config.output_recipe_file = OUTPUT_RECIPE; + config.headless_mode = g_HEADLESS; + + EXPECT_THROW(UrDriver ur_driver(config), UrException); } // TODO we should add more tests for the UrDriver class. @@ -413,6 +295,12 @@ int main(int argc, char* argv[]) g_ROBOT_IP = argv[i + 1]; break; } + if (std::string(argv[i]) == "--headless" && i + 1 < argc) + { + std::string headless = argv[i + 1]; + g_HEADLESS = headless == "true" || headless == "1" || headless == "True" || headless == "TRUE"; + break; + } } return RUN_ALL_TESTS();