diff --git a/CMakeLists.txt b/CMakeLists.txt index f882aa57f..0ebb6d344 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -87,6 +87,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 fe608dbfe..fced81da0 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 000000000..57510d265 --- /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. Note: protective stops are reported as warnings by the robot. +| 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/doc/examples.rst b/doc/examples.rst index dd90634c9..fcfa352a4 100644 --- a/doc/examples.rst +++ b/doc/examples.rst @@ -26,6 +26,7 @@ may be running forever until manually stopped. examples/external_fts_through_rtde examples/script_command_interface examples/script_sender + examples/send_script examples/spline_example examples/tool_contact_example examples/direct_torque_control diff --git a/doc/examples/send_script.rst b/doc/examples/send_script.rst new file mode 100644 index 000000000..a38948556 --- /dev/null +++ b/doc/examples/send_script.rst @@ -0,0 +1,99 @@ +:github_url: https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/doc/examples/send_script.rst + +.. _send_script_example: + +Send script example +=================== + +This example shows how to send arbitrary URScript code to the robot using the +:ref:`primary_client`. It demonstrates both the blocking variant (``sendScriptBlocking``), which +waits for execution feedback, and the non-blocking variant (``sendScript``), which only confirms +that the script has been forwarded to the robot. + +The full source code can be found in `send_script.cpp `_. + +Setting up the primary client +----------------------------- + +The example connects to the robot's primary interface by creating a ``PrimaryClient``. After +starting the client, the robot's brakes are released so that motion scripts can actually run, and +the safety state is checked before any script is sent: + +.. literalinclude:: ../../examples/send_script.cpp + :language: c++ + :caption: examples/send_script.cpp + :linenos: + :lineno-match: + :start-at: auto notif = comm::INotifier(); + :end-at: } + +Sending scripts with execution feedback +--------------------------------------- + +The ``sendScriptBlocking`` function uploads URScript code to the robot and waits until the robot +reports the result of the execution. The given code can be a fully defined script (with its own +``def ... end`` block) or a snippet that will automatically be wrapped into a function on the +client side. Comments and whitespace-only lines are stripped before the script is sent. + +.. literalinclude:: ../../examples/send_script.cpp + :language: c++ + :caption: examples/send_script.cpp + :linenos: + :lineno-match: + :start-at: const std::string fully_defined_script + :end-at: client.sendScriptBlocking(fully_defined_script) + +If you don't provide a function definition, the library wraps the snippet in one for you. You can +optionally pass a ``script_name`` (used in log messages on both the client and the robot) and a +``start_timeout`` that limits how long the call waits for the robot to confirm that the script has +started. A timeout of ``0`` means "wait indefinitely": + +.. literalinclude:: ../../examples/send_script.cpp + :language: c++ + :caption: examples/send_script.cpp + :linenos: + :lineno-match: + :start-at: client.sendScriptBlocking(R"(textmsg("Successful program execution"))"); + :end-at: client.sendScriptBlocking(R"(textmsg("hello"))", "cool_function_name", std::chrono::milliseconds(0)); + +Secondary programs can also be uploaded through ``sendScriptBlocking``. Since the robot does not +report execution feedback for secondary programs, the call returns as soon as the script has been +accepted. Note that secondary programs must be *fully defined* by the user (``sec ... end``): + +.. literalinclude:: ../../examples/send_script.cpp + :language: c++ + :caption: examples/send_script.cpp + :linenos: + :lineno-match: + :start-at: std::string secondary_script + :end-at: client.sendScriptBlocking(secondary_script); + +Reporting bad script code +------------------------- + +When a script contains errors (e.g. a typo or an undefined symbol), ``sendScriptBlocking`` will +report this back to the caller. The example sends a script that uses an undefined variable +``current_pos`` instead of ``current_pose``, and logs the result: + +.. literalinclude:: ../../examples/send_script.cpp + :language: c++ + :caption: examples/send_script.cpp + :linenos: + :lineno-match: + :start-at: const std::string bad_script_code + :end-before: // We can also send script code without any checks + +Sending scripts without feedback +-------------------------------- + +For situations where execution feedback is not needed, ``sendScript`` can be +used. It returns ``true`` as soon as the script has been transferred to the robot. The library +performs no further checks, so faulty script code will *not* be reported back here: + +.. literalinclude:: ../../examples/send_script.cpp + :language: c++ + :caption: examples/send_script.cpp + :linenos: + :lineno-match: + :start-at: // We can also send script code without any checks + :end-at: } diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index b7da3567d..b273c2998 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 + send_script.cpp) +target_link_libraries(send_script 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.cpp b/examples/send_script.cpp new file mode 100644 index 000000000..a832fcfa1 --- /dev/null +++ b/examples/send_script.cpp @@ -0,0 +1,96 @@ +#include +#include + +using namespace urcl; + +std::string g_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 = g_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); + + // --------------- INITIALIZATION END ------------------- + + // Make sure the robot is running + client.commandBrakeRelease(); + + if (!client.safetyModeAllowsExecution()) + { + URCL_LOG_ERROR("Robot is not in a safety state where script execution is possible. Exiting."); + return 1; + } + + // 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,-1.2,1.2,-0.1,1.57,0]) + sleep(0.1) + current_pose = get_target_tcp_pose() + relative_move = p[0,-0.1,0,0,0,0] + movel(pose_trans(current_pose, relative_move), t=1) +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); + + // Sending wrong script code will result in a clear error + const std::string bad_script_code = R"""( +def bad_code(): + current_pose = get_target_tcp_pose() + movel(current_pos) # note pose vs pos +end)"""; + URCL_LOG_INFO("Sending bad script code..."); + bool success = client.sendScriptBlocking(bad_script_code); + { + std::stringstream ss; + ss << "Execution of bad code successful? " << std::boolalpha << success; + URCL_LOG_INFO("%s", ss.str().c_str()); + } + + // We can also send script code without any checks + URCL_LOG_INFO("Executing motion without feedback"); + client.sendScript("movej([0.1,-0.9,0.9,0,0,0])"); + // But we won't know when that is done or even if our code was correct. + // E.g. sending the bad script here will not give us any information + // The return value will only tell us that the script code has been sent to the robot. + URCL_LOG_INFO("Sending bad script code without feedback..."); + success = client.sendScript(bad_script_code); + { + std::stringstream ss; + ss << "Bad code sent to robot successfully? " << std::boolalpha << success; + URCL_LOG_INFO("%s", ss.str().c_str()); + } +} diff --git a/include/ur_client_library/exceptions.h b/include/ur_client_library/exceptions.h index 9554a80ce..c3c46e8fc 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 fdb545dd9..3abd7272a 100644 --- a/include/ur_client_library/primary/abstract_primary_consumer.h +++ b/include/ur_client_library/primary/abstract_primary_consumer.h @@ -38,6 +38,8 @@ #include "ur_client_library/primary/robot_state/configuration_data.h" #include "ur_client_library/primary/robot_state/masterboard_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 { @@ -83,6 +85,8 @@ class AbstractPrimaryConsumer : public comm::IConsumer virtual bool consume(ConfigurationData& pkg) = 0; virtual bool consume(MasterboardData& 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 27ed2f33c..f4f1b110c 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 class 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,36 @@ 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 start_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. + * \throw urcl::UrException if the stop command cannot be sent to the robot. + * \throw urcl::TimeoutException if the robot doesn't stop the program within the given timeout. + * + * \returns true on successful execution of the script, false otherwise + */ + bool sendScriptBlocking(const std::string& program, std::string script_name = "", + std::chrono::milliseconds start_timeout = std::chrono::seconds(1), + bool fail_on_warnings = true); + bool checkCalibration(const std::string& checksum); /*! @@ -286,6 +331,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. @@ -298,6 +349,12 @@ 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); + std::string truncate_script_name(std::string candidate_name); PrimaryParser parser_; std::shared_ptr consumer_; @@ -311,6 +368,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 74b4d7dd8..a140ceded 100644 --- a/include/ur_client_library/primary/primary_consumer.h +++ b/include/ur_client_library/primary/primary_consumer.h @@ -33,6 +33,7 @@ #include "ur_client_library/primary/robot_state/masterboard_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 @@ -210,6 +211,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 * @@ -293,6 +322,8 @@ class PrimaryConsumer : public AbstractPrimaryConsumer private: std::function error_code_message_callback_; + std::function key_message_callback_; + std::function runtime_exception_callback_; std::mutex kinematics_info_mutex_; std::unique_ptr kinematics_info_; std::mutex robot_mode_mutex_; diff --git a/include/ur_client_library/primary/robot_message/runtime_exception_message.h b/include/ur_client_library/primary/robot_message/runtime_exception_message.h index 89c0b622b..b1d3b0616 100644 --- a/include/ur_client_library/primary/robot_message/runtime_exception_message.h +++ b/include/ur_client_library/primary/robot_message/runtime_exception_message.h @@ -99,8 +99,8 @@ class RuntimeExceptionMessage : public RobotMessage */ virtual std::string toString() const; - int32_t line_number_; - int32_t column_number_; + uint32_t line_number_; + uint32_t column_number_; std::string text_; }; } // namespace primary_interface diff --git a/src/primary/primary_client.cpp b/src/primary/primary_client.cpp index 9a2ac564e..a316c2da0 100644 --- a/src/primary/primary_client.cpp +++ b/src/primary/primary_client.cpp @@ -36,6 +36,8 @@ #include #include +#include +#include namespace urcl { namespace primary_interface @@ -48,6 +50,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 +99,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 +120,389 @@ std::deque PrimaryClient::getErrorCodes() return error_codes; } +bool PrimaryClient::safetyModeAllowsExecution() +{ + SafetyMode mode = getSafetyMode(); + switch (mode) + { + case SafetyMode::NORMAL: + case SafetyMode::REDUCED: + case SafetyMode::RECOVERY: + // 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(); + std::chrono::milliseconds robot_mode_timeout(1000); + auto start = std::chrono::system_clock::now(); + while (robot_mode == RobotMode::UNKNOWN) + { + auto now = std::chrono::system_clock::now(); + if (std::chrono::duration_cast(now - start).count() > robot_mode_timeout.count()) + { + URCL_LOG_ERROR("Robot mode not received within %lld ms, exiting.", robot_mode_timeout.count()); + return false; + } + 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; + } + // Clear runtime exception + { + std::scoped_lock lock(runtime_exception_mutex_); + latest_runtime_exception_ = nullptr; + } + // Clear existing error codes + getErrorCodes(); + // Clear key messages + { + std::scoped_lock lock(key_message_queue_mutex_); + key_message_queue_.clear(); + } + + 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; + // Error codes and key messages are produced by the same pipeline thread, but they live in two + // separate queues that are drained independently in this loop. A warning ErrorCode and the + // PROGRAM_XXX_STOPPED KeyMessage may therefore be visible to consecutive iterations rather than + // to the same one. To avoid returning success before such a "straggler" warning/error has been + // observed, we don't return immediately when STOPPED is seen. Instead we record that fact and + // keep draining the error / runtime exception queues for a short grace period. + bool program_stopped = false; + std::chrono::system_clock::time_point program_stopped_time; + const std::chrono::milliseconds post_stop_drain_period(100); + 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"); + size_t line_count = script_lines.size(); + size_t line_number_width = std::to_string(line_count).size(); + for (size_t i = 0; i < line_count; i++) + { + if (!script_lines[i].empty()) + { + ss << std::setw(line_number_width) << (i + 1) << ": " << script_lines[i] << "\n"; + } + if (static_cast(i) == latest_runtime_exception_->line_number_ - 1) + { + uint32_t output_column = + latest_runtime_exception_->column_number_ - 1 + (static_cast(line_number_width) + 2); + for (uint32_t j = 0; j < output_column; j++) + { + ss << " "; + } + ss << "^<--- here\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_WARN("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) + { + // C210 means that the primary client is connected to a read-only primary interface, + // which means that scripts cannot be executed. We check for this error code to give the + // user a more specific error message in this case. + 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) + { + if (!program_stopped) + { + URCL_LOG_DEBUG("Script with name %s reported as stopped. Draining residual error / " + "runtime exception messages for up to %lld ms before reporting success.", + script_info.script_name.c_str(), static_cast(post_stop_drain_period.count())); + program_stopped = true; + program_stopped_time = std::chrono::system_clock::now(); + // STOPPED implies the script was started, otherwise the controller could not have + // stopped it. This avoids a spurious "not started within timeout" failure if the + // STARTED message was never observed in this loop. + script_started = 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; + } + } + } + + // After STOPPED has been observed, give the pipeline a short grace period to deliver any + // warning / fault / violation error codes that may have been parsed just before STOPPED but + // ended up in the error queue after this iteration's getErrorCodes() snapshot. Only declare + // success once that grace period elapsed without any reportable issue. + if (program_stopped) + { + const auto now = std::chrono::system_clock::now(); + if (now - program_stopped_time >= post_stop_drain_period) + { + URCL_LOG_INFO("Script with name %s executed successfully", script_info.script_name.c_str()); + return true; + } + } + else + { + const auto current_time = std::chrono::system_clock::now(); + const auto elapsed_time = std::chrono::duration_cast(current_time - script_start_time); + + if (!script_started && elapsed_time > timeout) + { + 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; +} + +std::string PrimaryClient::truncate_script_name(const std::string candidate_name) +{ + std::string final_name = candidate_name; + // Limit script name length to 31, to ensure backwards compatibility + if (final_name.size() > 31) + { + final_name = final_name.substr(0, 31); + URCL_LOG_WARN("Given script name was too long, and has been truncated. New script name is: %s", final_name.c_str()); + } + + return final_name; +} + +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); + + if (stripped_script.size() == 0) + { + throw urcl::ScriptCodeSyntaxException("Script is empty after stripping comments and whitespace."); + } + + // Use given script name or create one + int64_t current_time = + std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()) + .count(); + // Assign name according to inputs + 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) + { + // Check that the final name is not too long + actual_script_name = truncate_script_name(actual_script_name); + 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("("); + if (name_end == stripped_script[0].npos) + { + throw urcl::ScriptCodeSyntaxException("Function definition detected in script, but a '(' could not be found. " + "Definition is invalid."); + } + std::string name_in_script = stripped_script[0].substr(4, name_end - 4); + if (stripped_script[0].substr(0, 4).find("def ") != stripped_script[0].npos) + { + actual_script_type = ScriptTypes::DEF; + } + else + { + actual_script_type = ScriptTypes::SEC; + } + // Check that the script name is not too long, replace it, if it is + actual_script_name = truncate_script_name(name_in_script); + if (actual_script_name.size() != name_in_script.size()) + { + stripped_script[0].replace(stripped_script[0].find(name_in_script), name_in_script.size(), actual_script_name); + } + } + + // 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: '" + actual_script_name + + "'. Can only contain letters, numbers and underscores. First character " + "must be a letter or underscore."); + } + + if (stripped_script.back().substr(0, 3).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 1b99eafa0..b0dfcf52b 100644 --- a/tests/test_primary_client.cpp +++ b/tests/test_primary_client.cpp @@ -426,6 +426,134 @@ 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); + const std::string comments_only = "#only\n#comments\n\n\n#and\n#whitespace"; + EXPECT_THROW(client_->sendScriptBlocking(comments_only), urcl::ScriptCodeSyntaxException); + const std::string script_no_paren = "def test_fun:\n" + " textmsg(\"testing\")" + "end"; + EXPECT_THROW(client_->sendScriptBlocking(script_no_paren), 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 + ASSERT_NO_THROW(client_->commandUnlockProtectiveStop()); + EXPECT_TRUE(client_->sendScriptBlocking("movej([0.5,-0.5,0.5,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()); + // Trigger protective stop (warning level error code) + EXPECT_TRUE(client_->sendScriptBlocking("protective_stop()", "", std::chrono::milliseconds(1000), false)); + // reset the robot + ASSERT_NO_THROW(client_->commandUnlockProtectiveStop()); +} + +TEST_F(PrimaryClientTest, test_send_script_blocking_replace_long_names) +{ + EXPECT_NO_THROW(client_->start()); + EXPECT_NO_THROW(client_->commandPowerOff()); + EXPECT_NO_THROW(client_->commandBrakeRelease()); + const std::string name = "this_is_a_very_long_script_name_that_should_be_truncated"; + EXPECT_TRUE(client_->sendScriptBlocking("textmsg(\"Still running\")", name)); + const std::string long_name_script = "def " + name + + "():\n" + " textmsg(\"still running\")\n" + " sleep(0.1)\n" + " sync()\n" + "end"; + EXPECT_TRUE(client_->sendScriptBlocking(long_name_script)); +} + int main(int argc, char* argv[]) { ::testing::InitGoogleTest(&argc, argv);