diff --git a/CMakeLists.txt b/CMakeLists.txt index 1226dfc3..42e89c13 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -85,6 +85,7 @@ else() src/ur/dashboard_client.cpp src/ur/dashboard_client_implementation_g5.cpp src/ur/dashboard_client_implementation_x.cpp + src/primary/primary_client.cpp PROPERTIES COMPILE_OPTIONS "-Wno-maybe-uninitialized") endif() diff --git a/doc/architecture.rst b/doc/architecture.rst index fe608dbf..fced81da 100644 --- a/doc/architecture.rst +++ b/doc/architecture.rst @@ -10,6 +10,7 @@ well as a couple of standalone modules to directly use subsets of the library's :maxdepth: 1 architecture/dashboard_client + architecture/primary_client architecture/reverse_interface architecture/rtde_client architecture/script_command_interface diff --git a/doc/architecture/primary_client.rst b/doc/architecture/primary_client.rst new file mode 100644 index 00000000..9bbbaf01 --- /dev/null +++ b/doc/architecture/primary_client.rst @@ -0,0 +1,42 @@ +:github_url: https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/doc/architecture/primary_client.rst + +.. _primary_client: + +PrimaryClient +============= + +The Primary Client serves as an interface to the robot's `primary interface `_, present on port 30001. +The ``PrimaryClient`` class supports, among other things, sending URScript code for execution on the robot through the primary interface. Currently it offers two methods of script execution: ``sendScript`` and ``sendScriptBlocking``. + +Script execution without feedback +--------------------------------- +Method signature: + +.. code-block:: c++ + + bool sendScript(std::string program); + +The ``sendScript`` method will accept valid URScript code, and send it to the robot through the primary interface. This is a non-blocking method, as it will return as soon as the program has been transferred to the robot. It returns true when the program is successfully transferred to the robot, and false otherwise. +There is no feedback on whether the program is actually executed on the robot. + +Script execution with feedback +------------------------------ +Method signature: + +.. code-block:: c++ + + bool sendScriptBlocking( + std::string program, + std::string script_name = "", + std::chrono::milliseconds timeout = std::chrono::seconds(1), + bool fail_on_warnings = true + ); + +| The ``sendScriptBlocking`` method will also accept valid URScript code, but blocks until the execution result of the given program is available. +| Prior to transferring the program it will first check that the robot is in a state where it can execute programs, if not it returns false. +| If the robot is ready, the program is then transferred, and the method will wait for the robot to report that the program has either started, finished or encountered an error. +| If the program has not started within the given ``timeout``, the method returns false. +| If the robot encounters an error or runtime exception during program execution the method also returns false. +| If ``fail_on_warnings`` is true, it will also return false, if the robot reports a warning during program execution. +| The method only returns true if the program is successfully executed on the robot. +| This method also accepts secondary programs, but no feedback is available for those, so it will behave similarly to the ``sendScript`` method in those cases, except for the pre-transfer checks. diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index b7da3567..8a12e14e 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -11,6 +11,10 @@ add_executable(primary_pipeline_example primary_pipeline.cpp) target_link_libraries(primary_pipeline_example ur_client_library::urcl) +add_executable(send_script_blocking + send_script_blocking.cpp) +target_link_libraries(send_script_blocking ur_client_library::urcl) + add_executable(primary_pipeline_calibration_example primary_pipeline_calibration.cpp) target_link_libraries(primary_pipeline_calibration_example ur_client_library::urcl) diff --git a/examples/send_script_blocking.cpp b/examples/send_script_blocking.cpp new file mode 100644 index 00000000..30f0bfa0 --- /dev/null +++ b/examples/send_script_blocking.cpp @@ -0,0 +1,68 @@ +#include +#include +#include + +using namespace urcl; + +std::string DEFAULT_ROBOT_IP = "192.168.56.101"; + +int main(int argc, char* argv[]) +{ + // Set the loglevel to info to print info logs + 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]); + } + auto notif = comm::INotifier(); + auto client = primary_interface::PrimaryClient(robot_ip, notif); + client.start(10); + std::cout << "Client connected" << std::endl; + + // --------------- INITIALIZATION END ------------------- + + // Make sure the robot is running + client.commandBrakeRelease(); + + if (!client.safetyModeAllowsExecution()) + { + std::cout << "Robot is not in a safety state where script execution is possible. Exiting." << std::endl; + return 0; + } + + // The sendScriptBlocking accepts script code, and will return true or false, + // depending on whether the script is successfully executed + const std::string fully_defined_script = R"""( +# This is a fully defined script, function definition and all +# All comments in this script will be stripped before sending the script to the robot + +# Any whitespace-only lines will also be removed +def example_fun(): + movej([0,-0.75,0,0,0,0]) + sleep(0.1) + movel([0,0,-1.5,0,0,0], t=5) +end)"""; + + if (client.sendScriptBlocking(fully_defined_script)) + { + // The function definition can also be omitted + // A function name will then be auto generated + client.sendScriptBlocking(R"(textmsg("Successful program execution"))"); + } + // A script-function name can also be passed to the method + // A timeout can also be given to limit the wait for the passed function to start. If timeout = 0, it will + // wait indefinitely. + client.sendScriptBlocking(R"(textmsg("hello"))", "cool_function_name", std::chrono::milliseconds(0)); + // There is no feedback on secondary programs, so it will return successful as soon as the script is sent to the + // robot (Behavior is the same the sendScript function, except that robot state is checked before script is sent) + // Note that secondary scripts have to be "fully defined" by the user. + std::string secondary_script = R"( +sec sec_script(): + textmsg("Named secondary program") +end +)"; + client.sendScriptBlocking(secondary_script); +} \ No newline at end of file diff --git a/include/ur_client_library/exceptions.h b/include/ur_client_library/exceptions.h index 9554a80c..c3c46e8f 100644 --- a/include/ur_client_library/exceptions.h +++ b/include/ur_client_library/exceptions.h @@ -319,5 +319,22 @@ class RTDEInputConflictException : public UrException std::string key_; std::string message_; }; + +class ScriptCodeSyntaxException : public UrException +{ +public: + explicit ScriptCodeSyntaxException() = delete; + + explicit ScriptCodeSyntaxException(const std::string& text) : std::runtime_error(text) + { + } + + virtual ~ScriptCodeSyntaxException() = default; + + virtual const char* what() const noexcept override + { + return std::runtime_error::what(); + } +}; } // namespace urcl #endif // ifndef UR_CLIENT_LIBRARY_EXCEPTIONS_H_INCLUDED diff --git a/include/ur_client_library/primary/abstract_primary_consumer.h b/include/ur_client_library/primary/abstract_primary_consumer.h index 9bc558f7..8a199d9d 100644 --- a/include/ur_client_library/primary/abstract_primary_consumer.h +++ b/include/ur_client_library/primary/abstract_primary_consumer.h @@ -37,6 +37,8 @@ #include "ur_client_library/primary/robot_state/robot_mode_data.h" #include "ur_client_library/primary/robot_state/configuration_data.h" #include "ur_client_library/primary/robot_message/safety_mode_message.h" +#include "ur_client_library/primary/robot_message/key_message.h" +#include "ur_client_library/primary/robot_message/runtime_exception_message.h" namespace urcl { @@ -81,6 +83,8 @@ class AbstractPrimaryConsumer : public comm::IConsumer virtual bool consume(RobotModeData& pkg) = 0; virtual bool consume(ConfigurationData& pkg) = 0; virtual bool consume(SafetyModeMessage& pkg) = 0; + virtual bool consume(KeyMessage& pkg) = 0; + virtual bool consume(RuntimeExceptionMessage& pkg) = 0; private: /* data */ diff --git a/include/ur_client_library/primary/primary_client.h b/include/ur_client_library/primary/primary_client.h index d8001adf..db4619a1 100644 --- a/include/ur_client_library/primary/primary_client.h +++ b/include/ur_client_library/primary/primary_client.h @@ -47,6 +47,21 @@ namespace urcl { namespace primary_interface { + +enum ScriptTypes +{ + DEF = 0, + SEC = 1, +}; + +struct ScriptInfo +{ + std::string script_name; + std::string script_code; + ScriptTypes script_type; + ScriptInfo(std::string name, std::string code, ScriptTypes type) + : script_name(name), script_code(code), script_type(type) {}; +}; class PrimaryClient { public: @@ -88,6 +103,33 @@ class PrimaryClient */ bool sendScript(const std::string& program); + /*! + * \brief Send a custom script program to the robot, and wait for the execution result. + * + * The given code must be valid according the UR Scripting Manual. The given script code will be automatically wrapped + * in a function definition, if it is not already. Secondary programs can also be passed to this function, but must be + * fully defined as a secondary program when calling. Secondary programs create no feedback, so this function will + * return true as soon as the program is uploaded successfully to the robot (same as the sendScript function). + * + * \param program URScript code that shall be executed by the robot. + * + * \param script_name Name of the script to be executed. This will be ignored, if the given script already defines a + * function name. The script name will be used in log messages in both the client library and in the robot logs. If no + * name is defined in any way, the script will be given a generic, but unique, name. + * + * \param timeout Amount of time to allow before the robot must have confirmed that the script has been started. If + * timeout is 0, it will be ignored. Default value: 1 second + * + * \param fail_on_warnings Whether or not the function should report a failure, if the robot reports a warning-level + * error during execution. Default true + * + * \throw urcl::ScriptCodeSyntaxException if the given script code has syntax errors, which are checked here. + * + * \returns true on successful execution of the script, false otherwise + */ + bool sendScriptBlocking(const std::string& program, std::string script_name = "", + std::chrono::milliseconds timeout = std::chrono::seconds(1), bool fail_on_warnings = true); + bool checkCalibration(const std::string& checksum); /*! @@ -272,6 +314,12 @@ class PrimaryClient */ RobotSeries getRobotSeries(); + /* \brief Check if the current safety mode allows for script execution + * + * Safety modes allowing for execution are: NORMAL, REDUCED, RECOVERY, UNDEFINED_SAFETY_MODE + */ + bool safetyModeAllowsExecution(); + private: /*! * \brief Reconnects the primary stream used to send program to the robot. @@ -284,6 +332,11 @@ class PrimaryClient // The function is called whenever an error code message is received void errorMessageCallback(ErrorCode& code); + void keyMessageCallback(KeyMessage& msg); + void runtimeExceptionCallback(RuntimeExceptionMessage& msg); + + ScriptInfo prepare_script(std::string script, std::string script_name); + std::vector strip_comments_and_whitespace(std::vector script_lines); PrimaryParser parser_; std::shared_ptr consumer_; @@ -297,6 +350,12 @@ class PrimaryClient std::mutex error_code_queue_mutex_; std::deque error_code_queue_; + + std::mutex key_message_queue_mutex_; + std::deque key_message_queue_; + + std::mutex runtime_exception_mutex_; + std::shared_ptr latest_runtime_exception_; }; } // namespace primary_interface diff --git a/include/ur_client_library/primary/primary_consumer.h b/include/ur_client_library/primary/primary_consumer.h index 824bbf18..ffb42e89 100644 --- a/include/ur_client_library/primary/primary_consumer.h +++ b/include/ur_client_library/primary/primary_consumer.h @@ -32,6 +32,7 @@ #include "ur_client_library/primary/robot_state/robot_mode_data.h" #include "ur_client_library/ur/datatypes.h" #include "ur_client_library/ur/version_information.h" +#include "ur_client_library/primary/robot_message/key_message.h" #include #include @@ -196,6 +197,34 @@ class PrimaryConsumer : public AbstractPrimaryConsumer error_code_message_callback_ = callback_function; } + virtual bool consume(KeyMessage& pkg) override + { + if (key_message_callback_ != nullptr) + { + key_message_callback_(pkg); + } + return true; + } + + void setKeyMessageCallback(std::function callback_function) + { + key_message_callback_ = callback_function; + } + + virtual bool consume(RuntimeExceptionMessage& pkg) override + { + if (runtime_exception_callback_ != nullptr) + { + runtime_exception_callback_(pkg); + } + return true; + } + + void setRuntimeExceptionCallback(std::function callback_function) + { + runtime_exception_callback_ = callback_function; + } + /*! * \brief Get the kinematics info * @@ -258,6 +287,8 @@ class PrimaryConsumer : public AbstractPrimaryConsumer private: std::function error_code_message_callback_; + std::function key_message_callback_; + std::function runtime_exception_callback_; std::shared_ptr kinematics_info_; std::mutex robot_mode_mutex_; std::shared_ptr robot_mode_; diff --git a/src/primary/primary_client.cpp b/src/primary/primary_client.cpp index 9a2ac564..6017cc95 100644 --- a/src/primary/primary_client.cpp +++ b/src/primary/primary_client.cpp @@ -36,6 +36,7 @@ #include #include +#include namespace urcl { namespace primary_interface @@ -48,6 +49,9 @@ PrimaryClient::PrimaryClient(const std::string& robot_ip, [[maybe_unused]] comm: consumer_.reset(new PrimaryConsumer()); consumer_->setErrorCodeMessageCallback(std::bind(&PrimaryClient::errorMessageCallback, this, std::placeholders::_1)); + consumer_->setKeyMessageCallback(std::bind(&PrimaryClient::keyMessageCallback, this, std::placeholders::_1)); + consumer_->setRuntimeExceptionCallback( + std::bind(&PrimaryClient::runtimeExceptionCallback, 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 @@ -94,6 +98,18 @@ void PrimaryClient::errorMessageCallback(ErrorCode& code) error_code_queue_.push_back(code); } +void PrimaryClient::keyMessageCallback(KeyMessage& msg) +{ + std::lock_guard lock_guard(key_message_queue_mutex_); + key_message_queue_.push_back(msg); +} + +void PrimaryClient::runtimeExceptionCallback(RuntimeExceptionMessage& msg) +{ + std::scoped_lock lock(runtime_exception_mutex_); + latest_runtime_exception_ = std::make_shared(msg); +} + std::deque PrimaryClient::getErrorCodes() { std::lock_guard lock_guard(error_code_queue_mutex_); @@ -103,12 +119,314 @@ std::deque PrimaryClient::getErrorCodes() return error_codes; } +bool PrimaryClient::safetyModeAllowsExecution() +{ + SafetyMode mode = getSafetyMode(); + switch (mode) + { + case SafetyMode::NORMAL: + return true; + + case SafetyMode::REDUCED: + return true; + + case SafetyMode::RECOVERY: + return true; + + // Safety mode might be unknown, as it is only updated on changes. + case SafetyMode::UNDEFINED_SAFETY_MODE: + return true; + + default: + return false; + } +} + +bool PrimaryClient::sendScriptBlocking(const std::string& program, std::string script_name, + std::chrono::milliseconds timeout, bool fail_on_warnings) +{ + ScriptInfo script_info = prepare_script(program, script_name); + + RobotMode robot_mode = getRobotMode(); + while (robot_mode == RobotMode::UNKNOWN) + { + URCL_LOG_INFO("Robot mode not received yet, waiting for it to be received."); + std::chrono::milliseconds update_period(100); + std::this_thread::sleep_for(update_period); + robot_mode = getRobotMode(); + } + + if (robot_mode != RobotMode::RUNNING) + { + URCL_LOG_ERROR("Robot is not running, cannot execute script."); + std::stringstream ss; + ss << "Robot is in mode: " << urcl::robotModeString(robot_mode) << " (" << int(robot_mode) << ")"; + URCL_LOG_ERROR(ss.str().c_str()); + return false; + } + + if (!safetyModeAllowsExecution()) + { + URCL_LOG_ERROR("Robot safety mode does not allow for script execution, cannot execute script."); + std::stringstream ss; + ss << "Robot safety mode is: " << safetyModeString(getSafetyMode()) << " (" << unsigned(getSafetyMode()) << ")"; + URCL_LOG_ERROR(ss.str().c_str()); + return false; + } + + { + std::scoped_lock lock(runtime_exception_mutex_); + latest_runtime_exception_ = nullptr; + } + + bool script_sent = sendScript(script_info.script_code); + if (!script_sent) + { + URCL_LOG_ERROR("Script could not be sent."); + return false; + } + // No feedback from secondary programs, so we assume success + if (script_info.script_type == ScriptTypes::SEC) + { + return true; + } + + const auto script_start_time = std::chrono::system_clock::now(); + // Ignore start delay if it is 0 + bool script_started = timeout == std::chrono::milliseconds(0) ? true : false; + while (true) + { + { + std::scoped_lock lock(runtime_exception_mutex_); + if (latest_runtime_exception_ != nullptr) + { + URCL_LOG_ERROR("Runtime exception occured during script execution. Runtime exception type: %s", + latest_runtime_exception_->text_.c_str()); + std::stringstream ss; + ss << "Exception occured at line " << latest_runtime_exception_->line_number_ << ", column " + << latest_runtime_exception_->column_number_ << "\n"; + // Debug print for the user + auto script_lines = splitString(script_info.script_code, "\n"); + for (int i = 0; i < static_cast(script_lines.size()); i++) + { + if (!script_lines[i].empty()) + { + ss << script_lines[i] << "\n"; + } + if (i == latest_runtime_exception_->line_number_ - 1) + { + for (int j = 0; j < latest_runtime_exception_->column_number_ - 1; j++) + { + ss << " "; + } + ss << "^\n"; + } + } + URCL_LOG_ERROR(ss.str().c_str()); + return false; + } + } + + auto errors = getErrorCodes(); + if (errors.size() > 0) + { + bool is_error = false; + bool is_warning = false; + bool is_read_only = false; + for (auto error : errors) + { + if (error.report_level == ReportLevel::VIOLATION || error.report_level == ReportLevel::FAULT) + { + URCL_LOG_ERROR("Robot error code with severity VIOLATION or FAULT received during script execution. Robot " + "error code: %s", + error.to_string.c_str()); + is_error = true; + } + if (error.report_level == ReportLevel::WARNING) + { + URCL_LOG_ERROR("Robot error code with severity WARNING received during script execution. Robot " + "error code: %s", + error.to_string.c_str()); + is_warning = true; + } + if (error.message_code == 210) + { + is_error = true; + is_read_only = true; + } + } + if (is_error) + { + if (!is_read_only) + { + commandStop(); + } + else + { + URCL_LOG_ERROR("Script cannot be executed since primary client is connected to a read-only primary " + "interface. If you have switched from local to remote mode recently, try reconnecting the " + "primary client and send the script code again."); + } + URCL_LOG_ERROR("Script execution failed due to error code(s) received from robot."); + return false; + } + if (is_warning && fail_on_warnings) + { + return false; + } + } + + // Copy out key messages + std::deque key_messages; + { + std::scoped_lock lock(key_message_queue_mutex_); + for (auto msg : key_message_queue_) + { + key_messages.push_back(msg); + } + key_message_queue_.clear(); + } + if (key_messages.size() > 0) + { + for (auto message : key_messages) + { + if (message.title_ == "PROGRAM_XXX_STOPPED" && message.text_ == script_info.script_name) + { + URCL_LOG_INFO("Script with name %s executed successfully", script_info.script_name.c_str()); + return true; + } + else if (!script_started && message.title_ == "PROGRAM_XXX_STARTED" && message.text_ == script_info.script_name) + { + URCL_LOG_INFO("Script with name %s started", script_info.script_name.c_str()); + script_started = true; + } + } + } + auto current_time = std::chrono::system_clock::now(); + auto elapsed_time = std::chrono::duration_cast(current_time - script_start_time); + + if (!script_started && elapsed_time > timeout) + { + // Should this stop the running program? + URCL_LOG_ERROR("Script %s not started within timeout", script_info.script_name.c_str()); + return false; + } + std::chrono::milliseconds wait_period(10); + std::this_thread::sleep_for(wait_period); + } +} + +std::vector PrimaryClient::strip_comments_and_whitespace(std::vector split_script) +{ + std::vector stripped_script; + for (auto line : split_script) + { + for (auto c : line) + { + if (!isspace(c)) + { + if (c == '#') + { + break; + } + else + { + stripped_script.push_back(line); + break; + } + } + } + } + return stripped_script; +} + +ScriptInfo PrimaryClient::prepare_script(std::string script, std::string script_name) +{ + // Split the given script in to separate lines + std::vector split_script = splitString(script, "\n"); + + // Remove all comments and white-space-only lines + std::vector stripped_script = strip_comments_and_whitespace(split_script); + + // Use given scipt name or create one + int64_t current_time = + std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()) + .count(); + std::string actual_script_name = script_name.empty() ? "script_" + std::to_string(current_time) : script_name; + ScriptTypes actual_script_type = urcl::primary_interface::ScriptTypes::DEF; + // Is the script wrapped in a function definition? If not add one + if (stripped_script[0].substr(0, 4).find("def ") == script.npos && + stripped_script[0].substr(0, 4).find("sec ") == script.npos) + { + std::string definition = "def " + actual_script_name + "():"; + std::string end = "end"; + // Add indentation to the existing script code + for (std::size_t i = 0; i < stripped_script.size(); i++) + { + stripped_script[i] = " " + stripped_script[i]; + } + // Add function definition and end statement to the stripped script lines vector + stripped_script.insert(stripped_script.begin(), definition); + stripped_script.push_back(end); + } + // Otherwise extract script name and type from function + else + { + size_t name_end = stripped_script[0].find("("); + actual_script_name = stripped_script[0].substr(4, name_end - 4); + if (stripped_script[0].find("def") != stripped_script[0].npos) + { + actual_script_type = ScriptTypes::DEF; + } + else + { + actual_script_type = ScriptTypes::SEC; + } + } + + // Validate script_name + static const std::regex valid_name(R"(^[A-Za-z_][A-Za-z0-9_]*$)"); + if (!std::regex_match(actual_script_name, valid_name)) + { + throw urcl::ScriptCodeSyntaxException("Invalid script name: '" + script_name + + "'. Can only contain letters, numbers and underscores. First character " + "must " + "be a letter or " + "underscore."); + } + + // Limit script name length to 31, to ensure backwards compatibility + if (actual_script_name.size() > 31) + { + actual_script_name = actual_script_name.substr(0, 31); + URCL_LOG_WARN("Given script name was too long, and has been truncated. New script name is: %s", + actual_script_name.c_str()); + } + if (stripped_script.back().find("end") == script.npos) + { + throw urcl::ScriptCodeSyntaxException("Script contains either function definition or secondary process " + "definition, " + "but no 'end' term. Script is invalid."); + } + + // Concatenate all the script lines in to the final script + std::string prepared_script = ""; + for (auto line : stripped_script) + { + prepared_script.append(line + "\n"); + } + + // Return final script code as well as the name of the script as it will be exectuted + return ScriptInfo(actual_script_name, prepared_script, actual_script_type); +} + bool PrimaryClient::sendScript(const std::string& program) { // urscripts (snippets) must end with a newline, or otherwise the controller's runtime will // not execute them. To avoid problems, we always just append a newline here, even if // there may already be one. - auto program_with_newline = program + '\n'; + + auto program_with_newline = program + "\n"; size_t len = program_with_newline.size(); const uint8_t* data = reinterpret_cast(program_with_newline.c_str()); diff --git a/tests/test_primary_client.cpp b/tests/test_primary_client.cpp index cf129bc8..64a93df7 100644 --- a/tests/test_primary_client.cpp +++ b/tests/test_primary_client.cpp @@ -409,6 +409,110 @@ TEST_F(PrimaryClientTest, test_read_safety_mode) EXPECT_EQ(client_->getSafetyMode(), urcl::SafetyMode::NORMAL); } +TEST_F(PrimaryClientTest, test_send_script_blocking_happy_path) +{ + EXPECT_NO_THROW(client_->start()); + EXPECT_NO_THROW(client_->commandPowerOff()); + EXPECT_NO_THROW(client_->commandBrakeRelease()); + + const std::string fully_defined_script = "def test_fun():\n" + " textmsg(\"still running\")\n" + " sleep(0.1)\n" + " sync()\n" + "end"; + EXPECT_TRUE(client_->sendScriptBlocking(fully_defined_script)); + + const std::string part_defined_script = "textmsg(\"still running\")\n" + "sleep(0.1)\n" + "sync()\n"; + EXPECT_TRUE(client_->sendScriptBlocking(part_defined_script)); + EXPECT_TRUE(client_->sendScriptBlocking(part_defined_script, "test_def")); + std::string sec_script = "sec test_sec():\n textmsg(\"Still running\")\nend"; + EXPECT_TRUE(client_->sendScriptBlocking(sec_script, "test_sec")); +} + +TEST_F(PrimaryClientTest, test_send_script_blocking_fails_on_nonrunning_robot) +{ + EXPECT_NO_THROW(client_->start()); + EXPECT_NO_THROW(client_->commandPowerOff()); + EXPECT_FALSE(client_->sendScriptBlocking("textmsg(\"Still running\")")); + EXPECT_NO_THROW(client_->commandPowerOn()); + EXPECT_FALSE(client_->sendScriptBlocking("textmsg(\"Still running\")")); + EXPECT_NO_THROW(client_->commandBrakeRelease()); + EXPECT_TRUE(client_->sendScriptBlocking("textmsg(\"Still running\")")); +} + +TEST_F(PrimaryClientTest, test_send_script_blocking_fails_on_bad_safety_mode) +{ + EXPECT_NO_THROW(client_->start()); + EXPECT_NO_THROW(client_->commandPowerOff()); + EXPECT_NO_THROW(client_->commandBrakeRelease()); + ASSERT_TRUE(client_->safetyModeAllowsExecution()); + + EXPECT_FALSE(client_->sendScriptBlocking("protective_stop()")); + EXPECT_FALSE(client_->sendScriptBlocking("textmsg(\"Still running\")")); + EXPECT_NO_THROW(client_->commandUnlockProtectiveStop()); + EXPECT_TRUE(client_->sendScriptBlocking("textmsg(\"Still running\")")); +} + +TEST_F(PrimaryClientTest, test_send_script_blocking_throw_on_malformed_scripts) +{ + EXPECT_NO_THROW(client_->start()); + const std::string script_no_end = "def test_fun():\n" + " textmsg(\"testing\")"; + EXPECT_THROW(client_->sendScriptBlocking(script_no_end), urcl::ScriptCodeSyntaxException); + const std::string script_bad_name = "def 7_eight_9():\n" + " textmsg(\"testing\")\n" + "end"; + EXPECT_THROW(client_->sendScriptBlocking(script_bad_name), urcl::ScriptCodeSyntaxException); + EXPECT_THROW(client_->sendScriptBlocking("textmsg(\"testing\")", "0_errors"), urcl::ScriptCodeSyntaxException); +} + +TEST_F(PrimaryClientTest, test_send_script_blocking_fail_on_runtime_exception) +{ + EXPECT_NO_THROW(client_->start()); + EXPECT_NO_THROW(client_->commandPowerOff()); + EXPECT_NO_THROW(client_->commandBrakeRelease()); + // Non-invertible goal, should throw runtime exception + EXPECT_FALSE(client_->sendScriptBlocking("movej(p[10,0,0,0,0,0])")); +} + +TEST_F(PrimaryClientTest, test_send_script_blocking_fail_on_robot_errors) +{ + EXPECT_NO_THROW(client_->start()); + EXPECT_NO_THROW(client_->commandPowerOff()); + EXPECT_NO_THROW(client_->commandBrakeRelease()); + // Impossible movement, will trigger a warning and protective stop + EXPECT_FALSE(client_->sendScriptBlocking("movel(p[10,0,0,0,0,0])")); + // reset the robot + client_->commandUnlockProtectiveStop(); + EXPECT_TRUE(client_->sendScriptBlocking("movej([0,0,0,0,0,0])")); +} + +TEST_F(PrimaryClientTest, test_send_script_blocking_fail_on_bad_script) +{ + EXPECT_NO_THROW(client_->start()); + EXPECT_NO_THROW(client_->commandPowerOff()); + EXPECT_NO_THROW(client_->commandBrakeRelease()); + + EXPECT_FALSE(client_->sendScriptBlocking("non_existing_func()")); + + const std::string script_code = "def illegal_fun():\n" + " calldoesntexist()\n" + "end"; + + EXPECT_FALSE(client_->sendScriptBlocking(script_code)); +} + +// TEST_F(PrimaryClientTest, test_send_script_blocking_ignore_warnings) +// { +// EXPECT_NO_THROW(client_->start()); +// EXPECT_NO_THROW(client_->commandPowerOff()); +// EXPECT_NO_THROW(client_->commandBrakeRelease()); +// // Impossible movement, will trigger an error and protective stop +// EXPECT_TRUE(client_->sendScriptBlocking("movel(p[10,0,0,0,0,0])", "", std::chrono::seconds(1), false)); +// } + int main(int argc, char* argv[]) { ::testing::InitGoogleTest(&argc, argv);