diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index e4f4ba561..fccb28eb0 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -15,22 +15,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 +30,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: diff --git a/CMakeLists.txt b/CMakeLists.txt index 3c41f99b4..306df182d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -39,6 +39,7 @@ add_library(urcl SHARED 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/examples/freedrive_example.cpp b/examples/freedrive_example.cpp index 53466cd40..e19a0d0b0 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 freedrive command. Is the robot in remote control?"); exit(1); } } @@ -89,86 +80,34 @@ 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->waitForProgramRunning()) { - URCL_LOG_ERROR("Could not send Power off command"); + URCL_LOG_ERROR("External Control script not running."); 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; } + + URCL_LOG_INFO("Stopping freedrive mode"); sendFreedriveMessageOrDie(control::FreedriveControlMessage::FREEDRIVE_STOP); } 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 000000000..297b32fb7 --- /dev/null +++ b/include/ur_client_library/example_robot_wrapper.h @@ -0,0 +1,184 @@ +// -- 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. + */ + void 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); + + 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 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/scripts/start_ursim.sh b/scripts/start_ursim.sh index 0a72f3391..014dc315a 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/example_robot_wrapper.cpp b/src/example_robot_wrapper.cpp new file mode 100644 index 000000000..ea3582a4a --- /dev/null +++ b/src/example_robot_wrapper.cpp @@ -0,0 +1,213 @@ +// -- 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 + +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"); + } + initializeRobotWithDashboard(); + + std::unique_ptr tool_comm_setup; + ur_driver_ = + std::make_shared(robot_ip, script_file, output_recipe_file, input_recipe_file, + std::bind(&ExampleRobotWrapper::handleRobotProgramState, this, std::placeholders::_1), + headless_mode, std::move(tool_comm_setup)); + + if (!headless_mode && !std::empty(autostart_program)) + { + startRobotProgram(autostart_program); + } +} + +ExampleRobotWrapper::~ExampleRobotWrapper() +{ + if (rtde_communication_started_) + { + stopConsumingRTDEData(); + } +} +void ExampleRobotWrapper::initializeRobotWithDashboard() +{ + // // Stop program, if there is one running + if (!dashboard_client_->commandStop()) + { + URCL_LOG_ERROR("Could not send stop program command"); + } + + // Power it off + if (!dashboard_client_->commandPowerOff()) + { + URCL_LOG_ERROR("Could not send Power off command"); + } + + // Power it on + if (!dashboard_client_->commandPowerOn()) + { + URCL_LOG_ERROR("Could not send Power on command"); + } + + // Release the brakes + if (!dashboard_client_->commandBrakeRelease()) + { + URCL_LOG_ERROR("Could not send BrakeRelease command"); + } + + // Now the robot is ready to receive a program + URCL_LOG_INFO("Robot ready to start a program"); +} + +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_); +} + +} // namespace urcl diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index fc48318f6..3a27051dc 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -31,22 +31,52 @@ 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 ) - 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 + # 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 ) - add_executable(instruction_executor_test test_instruction_executor.cpp) - target_link_libraries(instruction_executor_test PRIVATE ur_client_library::urcl GTest::gtest_main) - gtest_add_tests(TARGET instruction_executor_test + # 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 ) else() message(STATUS "Skipping integration tests.") 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 000000000..812ba2856 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 000000000..4139fd183 Binary files /dev/null and b/tests/resources/dockerursim/programs/e-series/external_control.urp differ diff --git a/tests/test_instruction_executor.cpp b/tests/test_instruction_executor.cpp index cf75e9813..5d8aeb641 100644 --- a/tests/test_instruction_executor.cpp +++ b/tests/test_instruction_executor.cpp @@ -29,11 +29,13 @@ // -- END LICENSE BLOCK ------------------------------------------------ #include +#include #include -#include "ur_client_library/ur/dashboard_client.h" #include "ur_client_library/ur/instruction_executor.h" #include "ur_client_library/control/motion_primitives.h" +#include + using namespace urcl; using namespace urcl::control; @@ -42,56 +44,9 @@ 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; -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; - -std::unique_ptr g_dashboard_client; -std::shared_ptr g_ur_driver; - -// 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 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; -} - -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; -} +std::unique_ptr g_my_robot; class InstructionExecutorTest : public ::testing::Test { @@ -100,57 +55,29 @@ class InstructionExecutorTest : public ::testing::Test static void SetUpTestSuite() { - g_dashboard_client.reset(new DashboardClient(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 UrDriver(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 UrDriver(ROBOT_IP, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, &handleRobotProgramState, - headless, std::move(tool_comm_setup), CALIBRATION_CHECKSUM)); - } + 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_ur_driver); + executor_ = std::make_unique(g_my_robot->ur_driver_); std::string safety_status; - g_dashboard_client->commandSafetyStatus(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) { // 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_dashboard_client->commandCloseSafetyPopup(); - ASSERT_TRUE(g_dashboard_client->commandUnlockProtectiveStop()); + g_my_robot->dashboard_client_->commandCloseSafetyPopup(); + ASSERT_TRUE(g_my_robot->dashboard_client_->commandUnlockProtectiveStop()); } // Make sure script is running on the robot - if (g_program_running == false) + if (!g_my_robot->waitForProgramRunning()) { - g_ur_driver->sendRobotProgram(); - ASSERT_TRUE(waitForProgramRunning(1000)); + g_my_robot->resendRobotProgram(); + ASSERT_TRUE(g_my_robot->waitForProgramRunning()); } } }; @@ -252,8 +179,8 @@ TEST_F(InstructionExecutorTest, execute_movel_success) TEST_F(InstructionExecutorTest, sending_commands_without_reverse_interface_connected_fails) { - g_dashboard_client->commandStop(); - ASSERT_TRUE(waitForProgramNotRunning(1000)); + 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 })); @@ -271,8 +198,8 @@ TEST_F(InstructionExecutorTest, sending_commands_without_reverse_interface_conne ASSERT_FALSE(executor_->executeMotion(motion_sequence)); // Disconnect mid-motion - g_ur_driver->sendRobotProgram(); - ASSERT_TRUE(waitForProgramRunning(1000)); + 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 })); @@ -280,8 +207,8 @@ TEST_F(InstructionExecutorTest, sending_commands_without_reverse_interface_conne 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_dashboard_client->commandStop(); - ASSERT_TRUE(waitForProgramNotRunning(1000)); + g_my_robot->dashboard_client_->commandStop(); + ASSERT_TRUE(g_my_robot->waitForProgramNotRunning(1000)); motion_thread.join(); } @@ -353,6 +280,12 @@ int main(int argc, char* argv[]) 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_rtde_client.cpp b/tests/test_rtde_client.cpp index a3683b638..ead20bbe3 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(); } diff --git a/tests/test_spline_interpolation.cpp b/tests/test_spline_interpolation.cpp index 39eec3d7c..e75d4c510 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) + 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) { - step_time_ = 0.008; + // 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()); } // Make sure script is running on the robot - if (g_program_running == false) + if (!g_my_robot->waitForProgramRunning()) { - g_consume_rtde_packages = true; - g_ur_driver->sendRobotProgram(); - ASSERT_TRUE(waitForProgramRunning(1000)); + g_my_robot->resendRobotProgram(); + ASSERT_TRUE(g_my_robot->waitForProgramRunning()); } - g_consume_rtde_packages = false; } 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 8ec2b989d..b6fc1a354 100644 --- a/tests/test_ur_driver.cpp +++ b/tests/test_ur_driver.cpp @@ -31,8 +31,10 @@ #include #include +#include #define private public #include +#include using namespace urcl; @@ -41,176 +43,41 @@ 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; -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 UrDriver(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 UrDriver(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); + + g_my_robot->startRTDECommununication(true); } 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) + if (!g_my_robot->waitForProgramRunning()) { - g_consume_rtde_packages = true; - g_ur_driver->sendRobotProgram(); - ASSERT_TRUE(waitForProgramRunning(1000)); + g_my_robot->resendRobotProgram(); + ASSERT_TRUE(g_my_robot->waitForProgramRunning()); } - g_consume_rtde_packages = false; - } - - 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) - { - 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; - } - - // 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); } TEST_F(UrDriverTest, read_existing_script_file) { - g_consume_rtde_packages = true; char existing_script_file[] = "urscript.XXXXXX"; int fd = mkstemp(existing_script_file); if (fd == -1) @@ -227,80 +94,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)); @@ -310,11 +172,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) @@ -323,15 +186,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)); @@ -341,11 +205,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) @@ -354,29 +219,30 @@ 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->secondary_stream_->close(); + g_my_robot->ur_driver_->secondary_stream_->close(); + + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + EXPECT_TRUE(g_my_robot->resendRobotProgram()); - EXPECT_TRUE(g_ur_driver->sendRobotProgram()); + 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); } // TODO we should add more tests for the UrDriver class. @@ -392,6 +258,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();