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..a413c648a 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,394 @@ 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"; + // Line and column numbers should always be 1-based, but we check that they are greater + // than 0 just to be sure before using them for indexing in the debug print below + if (latest_runtime_exception_->line_number_ > 0 && latest_runtime_exception_->column_number_ > 0) + { + // 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/CMakeLists.txt b/tests/CMakeLists.txt index a7abb4909..dd562a730 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -17,6 +17,9 @@ FetchContent_MakeAvailable(googletest) add_executable(fake_rtde_server fake_rtde_server.cpp fake_rtde_server_main.cpp) target_link_libraries(fake_rtde_server PRIVATE ur_client_library::urcl) +add_executable(fake_primary_server fake_primary_server.cpp fake_primary_server_main.cpp) +target_link_libraries(fake_primary_server PRIVATE ur_client_library::urcl) + include(GoogleTest) option(INTEGRATION_TESTS "Build the integration tests that require a running robot / URSim" OFF) @@ -118,7 +121,7 @@ if (INTEGRATION_TESTS) TEST_SUFFIX _headless ) - add_executable(primary_client_test_headless test_primary_client.cpp) + add_executable(primary_client_test_headless test_primary_client.cpp fake_primary_server.cpp) target_link_libraries(primary_client_test_headless PRIVATE ur_client_library::urcl GTest::gtest_main) gtest_add_tests(TARGET primary_client_test_headless WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} @@ -144,6 +147,11 @@ target_link_libraries(primary_parser_tests PRIVATE ur_client_library::urcl GTest gtest_add_tests(TARGET primary_parser_tests ) +add_executable(fake_primary_server_tests test_fake_primary_server.cpp fake_primary_server.cpp) +target_link_libraries(fake_primary_server_tests PRIVATE ur_client_library::urcl GTest::gtest_main) +gtest_add_tests(TARGET fake_primary_server_tests +) + add_executable(rtde_data_package_tests test_rtde_data_package.cpp) diff --git a/tests/fake_primary_server.cpp b/tests/fake_primary_server.cpp new file mode 100644 index 000000000..76c8981ea --- /dev/null +++ b/tests/fake_primary_server.cpp @@ -0,0 +1,308 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2026 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 "fake_primary_server.h" + +#include + +#include "ur_client_library/comm/package_serializer.h" +#include "ur_client_library/log.h" + +namespace urcl +{ + +FakePrimaryServer::FakePrimaryServer(const int port) : server_(port) +{ + server_.setMessageCallback(std::bind(&FakePrimaryServer::messageCallback, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3)); + server_.setConnectCallback(std::bind(&FakePrimaryServer::connectionCallback, this, std::placeholders::_1)); + server_.setDisconnectCallback(std::bind(&FakePrimaryServer::disconnectionCallback, this, std::placeholders::_1)); + server_.start(); +} + +FakePrimaryServer::~FakePrimaryServer() +{ + server_.shutdown(); +} + +size_t FakePrimaryServer::getClientCount() const +{ + std::lock_guard lock(clients_mutex_); + return clients_.size(); +} + +bool FakePrimaryServer::waitForClient(const std::chrono::milliseconds timeout) +{ + std::unique_lock lock(clients_mutex_); + return clients_cv_.wait_for(lock, timeout, [this]() { return !clients_.empty(); }); +} + +void FakePrimaryServer::connectionCallback(const socket_t filedescriptor) +{ + { + std::lock_guard lock(clients_mutex_); + clients_.push_back(filedescriptor); + } + clients_cv_.notify_all(); + URCL_LOG_INFO("Client connected to fake primary server on FD %d", filedescriptor); +} + +void FakePrimaryServer::disconnectionCallback(const socket_t filedescriptor) +{ + { + std::lock_guard lock(clients_mutex_); + clients_.erase(std::remove(clients_.begin(), clients_.end(), filedescriptor), clients_.end()); + } + URCL_LOG_INFO("Client disconnected from fake primary server on FD %d", filedescriptor); +} + +void FakePrimaryServer::messageCallback([[maybe_unused]] const socket_t filedescriptor, char* buffer, int nbytesrecv) +{ + std::string received(buffer, static_cast(nbytesrecv)); + std::function cb; + { + std::lock_guard lock(script_mutex_); + last_received_script_ = received; + cb = script_callback_; + } + if (cb) + { + cb(received); + } +} + +bool FakePrimaryServer::sendRaw(const uint8_t* data, size_t size) +{ + std::vector clients_snapshot; + { + std::lock_guard lock(clients_mutex_); + clients_snapshot = clients_; + } + if (clients_snapshot.empty()) + { + URCL_LOG_WARN("Fake primary server has no connected clients to send to."); + return false; + } + bool all_ok = true; + for (const socket_t fd : clients_snapshot) + { + size_t written = 0; + if (!server_.write(fd, data, size, written) || written != size) + { + URCL_LOG_ERROR("Failed to write %zu bytes to client FD %d (wrote %zu)", size, fd, written); + all_ok = false; + } + } + return all_ok; +} + +std::vector FakePrimaryServer::buildPrimaryPackage(primary_interface::RobotPackageType type, + const std::vector& body) +{ + const int32_t total_size = static_cast(sizeof(int32_t) + sizeof(uint8_t) + body.size()); + std::vector packet(total_size); + size_t offset = 0; + offset += comm::PackageSerializer::serialize(packet.data() + offset, total_size); + offset += comm::PackageSerializer::serialize(packet.data() + offset, static_cast(type)); + std::copy(body.begin(), body.end(), packet.begin() + offset); + return packet; +} + +std::vector FakePrimaryServer::buildRobotMessageBody(uint64_t timestamp, uint8_t source, + primary_interface::RobotMessagePackageType message_type, + const std::vector& payload) +{ + std::vector body(sizeof(uint64_t) + sizeof(uint8_t) + sizeof(uint8_t) + payload.size()); + size_t offset = 0; + offset += comm::PackageSerializer::serialize(body.data() + offset, timestamp); + offset += comm::PackageSerializer::serialize(body.data() + offset, source); + offset += comm::PackageSerializer::serialize(body.data() + offset, static_cast(message_type)); + std::copy(payload.begin(), payload.end(), body.begin() + offset); + return body; +} + +std::vector FakePrimaryServer::buildRobotStateBody(const std::vector& sub_packages) +{ + std::vector body; + for (const auto& sub : sub_packages) + { + const uint32_t sub_size = static_cast(sizeof(uint32_t) + sizeof(uint8_t) + sub.second.size()); + const size_t old_size = body.size(); + body.resize(old_size + sub_size); + size_t offset = old_size; + offset += comm::PackageSerializer::serialize(body.data() + offset, sub_size); + offset += comm::PackageSerializer::serialize(body.data() + offset, static_cast(sub.first)); + std::copy(sub.second.begin(), sub.second.end(), body.begin() + offset); + } + return body; +} + +bool FakePrimaryServer::sendRobotMessage(primary_interface::RobotMessagePackageType message_type, + const std::vector& payload, uint64_t timestamp, uint8_t source) +{ + std::vector body = buildRobotMessageBody(timestamp, source, message_type, payload); + std::vector packet = buildPrimaryPackage(primary_interface::RobotPackageType::ROBOT_MESSAGE, body); + return sendRaw(packet.data(), packet.size()); +} + +bool FakePrimaryServer::sendRobotState(const std::vector& sub_packages) +{ + std::vector body = buildRobotStateBody(sub_packages); + std::vector packet = buildPrimaryPackage(primary_interface::RobotPackageType::ROBOT_STATE, body); + return sendRaw(packet.data(), packet.size()); +} + +bool FakePrimaryServer::sendVersionMessage(const std::string& project_name, uint8_t major_version, + uint8_t minor_version, int32_t svn_version, int32_t build_number, + const std::string& build_date) +{ + std::vector payload(sizeof(int8_t) + project_name.size() + sizeof(uint8_t) + sizeof(uint8_t) + + sizeof(int32_t) + sizeof(int32_t) + build_date.size()); + size_t offset = 0; + offset += comm::PackageSerializer::serialize(payload.data() + offset, static_cast(project_name.size())); + offset += comm::PackageSerializer::serialize(payload.data() + offset, project_name); + offset += comm::PackageSerializer::serialize(payload.data() + offset, major_version); + offset += comm::PackageSerializer::serialize(payload.data() + offset, minor_version); + offset += comm::PackageSerializer::serialize(payload.data() + offset, svn_version); + offset += comm::PackageSerializer::serialize(payload.data() + offset, build_number); + offset += comm::PackageSerializer::serialize(payload.data() + offset, build_date); + return sendRobotMessage(primary_interface::RobotMessagePackageType::ROBOT_MESSAGE_VERSION, payload); +} + +bool FakePrimaryServer::sendTextMessage(const std::string& text) +{ + std::vector payload(text.size()); + if (!text.empty()) + { + comm::PackageSerializer::serialize(payload.data(), text); + } + return sendRobotMessage(primary_interface::RobotMessagePackageType::ROBOT_MESSAGE_TEXT, payload); +} + +bool FakePrimaryServer::sendKeyMessage(const std::string& title, const std::string& text, int32_t message_code, + int32_t message_argument) +{ + std::vector payload(sizeof(int32_t) + sizeof(int32_t) + sizeof(uint8_t) + title.size() + text.size()); + size_t offset = 0; + offset += comm::PackageSerializer::serialize(payload.data() + offset, message_code); + offset += comm::PackageSerializer::serialize(payload.data() + offset, message_argument); + offset += comm::PackageSerializer::serialize(payload.data() + offset, static_cast(title.size())); + offset += comm::PackageSerializer::serialize(payload.data() + offset, title); + offset += comm::PackageSerializer::serialize(payload.data() + offset, text); + return sendRobotMessage(primary_interface::RobotMessagePackageType::ROBOT_MESSAGE_KEY, payload); +} + +bool FakePrimaryServer::sendRuntimeExceptionMessage(uint32_t line_number, uint32_t column_number, + const std::string& text) +{ + std::vector payload(sizeof(uint32_t) + sizeof(uint32_t) + text.size()); + size_t offset = 0; + offset += comm::PackageSerializer::serialize(payload.data() + offset, line_number); + offset += comm::PackageSerializer::serialize(payload.data() + offset, column_number); + offset += comm::PackageSerializer::serialize(payload.data() + offset, text); + return sendRobotMessage(primary_interface::RobotMessagePackageType::ROBOT_MESSAGE_RUNTIME_EXCEPTION, payload); +} + +bool FakePrimaryServer::sendSafetyModeMessage(SafetyMode safety_mode, int32_t message_code, int32_t message_argument, + uint32_t report_data_type, uint32_t report_data) +{ + std::vector payload(sizeof(int32_t) + sizeof(int32_t) + sizeof(uint8_t) + sizeof(uint32_t) + + sizeof(uint32_t)); + size_t offset = 0; + offset += comm::PackageSerializer::serialize(payload.data() + offset, message_code); + offset += comm::PackageSerializer::serialize(payload.data() + offset, message_argument); + offset += comm::PackageSerializer::serialize(payload.data() + offset, static_cast(safety_mode)); + offset += comm::PackageSerializer::serialize(payload.data() + offset, report_data_type); + offset += comm::PackageSerializer::serialize(payload.data() + offset, report_data); + return sendRobotMessage(primary_interface::RobotMessagePackageType::ROBOT_MESSAGE_SAFETY_MODE, payload); +} + +bool FakePrimaryServer::sendErrorCodeMessage(int32_t message_code, int32_t message_argument, + primary_interface::ReportLevel report_level, const std::string& text, + uint32_t data_type, uint32_t data) +{ + std::vector payload(sizeof(int32_t) + sizeof(int32_t) + sizeof(int32_t) + sizeof(uint32_t) + + sizeof(uint32_t) + text.size()); + size_t offset = 0; + offset += comm::PackageSerializer::serialize(payload.data() + offset, message_code); + offset += comm::PackageSerializer::serialize(payload.data() + offset, message_argument); + offset += comm::PackageSerializer::serialize(payload.data() + offset, static_cast(report_level)); + offset += comm::PackageSerializer::serialize(payload.data() + offset, data_type); + offset += comm::PackageSerializer::serialize(payload.data() + offset, data); + offset += comm::PackageSerializer::serialize(payload.data() + offset, text); + return sendRobotMessage(primary_interface::RobotMessagePackageType::ROBOT_MESSAGE_ERROR_CODE, payload); +} + +bool FakePrimaryServer::sendRobotModeData(RobotMode robot_mode, bool is_real_robot_connected, + bool is_real_robot_enabled, bool is_robot_power_on, bool is_emergency_stopped, + bool is_protective_stopped, bool is_program_running, bool is_program_paused, + uint8_t control_mode, double target_speed_fraction, double speed_scaling, + double target_speed_fraction_limit) +{ + std::vector payload(sizeof(uint64_t) + 7 * sizeof(uint8_t) + sizeof(int8_t) + sizeof(uint8_t) + + 3 * sizeof(double)); + size_t offset = 0; + uint64_t timestamp = + std::chrono::duration_cast(std::chrono::steady_clock::now().time_since_epoch()).count(); + offset += comm::PackageSerializer::serialize(payload.data() + offset, timestamp); + offset += comm::PackageSerializer::serialize(payload.data() + offset, static_cast(is_real_robot_connected)); + offset += comm::PackageSerializer::serialize(payload.data() + offset, static_cast(is_real_robot_enabled)); + offset += comm::PackageSerializer::serialize(payload.data() + offset, static_cast(is_robot_power_on)); + offset += comm::PackageSerializer::serialize(payload.data() + offset, static_cast(is_emergency_stopped)); + offset += comm::PackageSerializer::serialize(payload.data() + offset, static_cast(is_protective_stopped)); + offset += comm::PackageSerializer::serialize(payload.data() + offset, static_cast(is_program_running)); + offset += comm::PackageSerializer::serialize(payload.data() + offset, static_cast(is_program_paused)); + offset += comm::PackageSerializer::serialize(payload.data() + offset, static_cast(robot_mode)); + offset += comm::PackageSerializer::serialize(payload.data() + offset, control_mode); + offset += comm::PackageSerializer::serialize(payload.data() + offset, target_speed_fraction); + offset += comm::PackageSerializer::serialize(payload.data() + offset, speed_scaling); + offset += comm::PackageSerializer::serialize(payload.data() + offset, target_speed_fraction_limit); + return sendRobotState({ { primary_interface::RobotStateType::ROBOT_MODE_DATA, payload } }); +} + +void FakePrimaryServer::setScriptCallback(std::function callback) +{ + std::lock_guard lock(script_mutex_); + script_callback_ = std::move(callback); +} + +std::string FakePrimaryServer::getLastReceivedScript() +{ + std::lock_guard lock(script_mutex_); + return last_received_script_; +} + +void FakePrimaryServer::clearLastReceivedScript() +{ + std::lock_guard lock(script_mutex_); + last_received_script_.clear(); +} + +} // namespace urcl diff --git a/tests/fake_primary_server.h b/tests/fake_primary_server.h new file mode 100644 index 000000000..e4425bc57 --- /dev/null +++ b/tests/fake_primary_server.h @@ -0,0 +1,257 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2026 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 ------------------------------------------------ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ur_client_library/comm/tcp_server.h" +#include "ur_client_library/primary/package_header.h" +#include "ur_client_library/primary/robot_message.h" +#include "ur_client_library/primary/robot_message/error_code_message.h" +#include "ur_client_library/primary/robot_state.h" +#include "ur_client_library/ur/datatypes.h" + +namespace urcl +{ +/*! + * \brief A fake primary interface server that can be used in tests. + * + * The server is built on top of comm::TCPServer and exposes convenience functions for sending the + * most common primary interface packages (version, key, text, runtime exception, safety mode, + * error code, robot state sub-messages, ...). All messages are serialized using the same wire + * format that the real UR controller uses, so existing primary interface clients can be pointed at + * this server for unit testing. + * + * URScript code (or any other data) that the connected client sends to the server is captured and + * can be inspected through the script callback or via \ref getLastReceivedScript. + * + * Multiple clients can be connected simultaneously. Sending always broadcasts to every connected + * client. + */ +class FakePrimaryServer +{ +public: + /*! + * \brief A single ROBOT_STATE sub-package, consisting of its sub-package type and the already + * serialized payload (without the 4-byte sub-package size and the 1-byte type). + */ + using RobotStateSubPackage = std::pair>; + + FakePrimaryServer() = delete; + + /*! + * \brief Construct a new fake primary server bound to \p port. + * + * If 0 is passed, the OS picks a free port that can be queried via \ref getPort. + * + * \param port Port to bind the server to. Defaults to the standard UR primary interface port. + */ + explicit FakePrimaryServer(const int port = primary_interface::UR_PRIMARY_PORT); + + ~FakePrimaryServer(); + + /*! + * \brief Get the port this server is bound to. + */ + int getPort() const + { + return server_.getPort(); + } + + /*! + * \brief Get the number of currently connected clients. + */ + size_t getClientCount() const; + + /*! + * \brief Block until at least one client is connected, or the timeout expires. + * + * \returns true if a client is connected before the timeout, false otherwise. + */ + bool waitForClient(const std::chrono::milliseconds timeout = std::chrono::seconds(1)); + + // ===================== Generic send ===================== + + /*! + * \brief Send a fully formed primary package to all connected clients. + * + * The bytes are forwarded to the connected client(s) without modification. This is useful when + * replaying real recordings of primary interface traffic. + * + * \param data Buffer holding the complete primary package (including its 4-byte length header). + * \param size Number of bytes in \p data. + * + * \returns true if every connected client accepted the data, false if any write failed. + */ + bool sendRaw(const uint8_t* data, size_t size); + + /*! + * \brief Wrap the given payload as a ROBOT_MESSAGE and send it to all connected clients. + * + * The resulting on-wire package looks like: + * \verbatim + * <4-byte size> <1-byte ROBOT_MESSAGE> <8-byte timestamp> <1-byte source> + * <1-byte message_type> + * \endverbatim + * + * \param message_type The robot message type. + * \param payload Raw payload that follows the robot message header. + * \param timestamp Timestamp to embed. + * \param source Source byte to embed. + */ + bool sendRobotMessage(primary_interface::RobotMessagePackageType message_type, const std::vector& payload, + uint64_t timestamp = 0, uint8_t source = 0); + + /*! + * \brief Wrap the given sub-packages as a ROBOT_STATE package and send it to all clients. + * + * The resulting on-wire package looks like: + * \verbatim + * <4-byte size> <1-byte ROBOT_STATE> + * <4-byte sub-size> <1-byte sub-type> + * ... + * \endverbatim + * + * \param sub_packages List of (type, payload) tuples to include in the ROBOT_STATE. + */ + bool sendRobotState(const std::vector& sub_packages); + + // ===================== Concrete RobotMessage helpers ===================== + + /*! + * \brief Send a VersionMessage. + */ + bool sendVersionMessage(const std::string& project_name = "URControl", uint8_t major_version = 5, + uint8_t minor_version = 24, int32_t svn_version = 0, int32_t build_number = 0, + const std::string& build_date = "01-01-2026, 00:00:00"); + + /*! + * \brief Send a TextMessage with the given text. + */ + bool sendTextMessage(const std::string& text); + + /*! + * \brief Send a KeyMessage (used by the controller to e.g. signal that a program has + * started/stopped). + */ + bool sendKeyMessage(const std::string& title, const std::string& text, int32_t message_code = 0, + int32_t message_argument = 0); + + /*! + * \brief Send a RuntimeExceptionMessage. + */ + bool sendRuntimeExceptionMessage(uint32_t line_number, uint32_t column_number, const std::string& text); + + /*! + * \brief Send a SafetyModeMessage. + */ + bool sendSafetyModeMessage(SafetyMode safety_mode = SafetyMode::NORMAL, int32_t message_code = 0, + int32_t message_argument = 0, uint32_t report_data_type = 0, uint32_t report_data = 0); + + /*! + * \brief Send an ErrorCodeMessage. + */ + bool sendErrorCodeMessage(int32_t message_code, int32_t message_argument, primary_interface::ReportLevel report_level, + const std::string& text, uint32_t data_type = 0, uint32_t data = 0); + + // ===================== Concrete RobotState helpers ===================== + + /*! + * \brief Send a ROBOT_STATE package containing a single RobotModeData sub-message. + * + * Defaults correspond to a robot in RUNNING mode without any running program. + */ + bool sendRobotModeData(RobotMode robot_mode = RobotMode::RUNNING, bool is_real_robot_connected = true, + bool is_real_robot_enabled = true, bool is_robot_power_on = true, + bool is_emergency_stopped = false, bool is_protective_stopped = false, + bool is_program_running = false, bool is_program_paused = false, uint8_t control_mode = 0, + double target_speed_fraction = 1.0, double speed_scaling = 1.0, + double target_speed_fraction_limit = 1.0); + + // ===================== Receive hooks ===================== + + /*! + * \brief Set a callback that will be called for every chunk of data received from a client. + * + * The primary interface accepts URScript code from clients. This callback exposes that data as a + * string. Note that, because TCP is a stream, the chunks are not guaranteed to align with full + * scripts; if you need full scripts use \ref getLastReceivedScript after sending one. + */ + void setScriptCallback(std::function callback); + + /*! + * \brief Return the most recent data received from a client as a string. + */ + std::string getLastReceivedScript(); + + /*! + * \brief Clear the stored "last received script". + */ + void clearLastReceivedScript(); + +private: + void connectionCallback(const socket_t filedescriptor); + void disconnectionCallback(const socket_t filedescriptor); + void messageCallback(const socket_t filedescriptor, char* buffer, int nbytesrecv); + + // Build a complete primary package: 4-byte size + 1-byte type + body. + static std::vector buildPrimaryPackage(primary_interface::RobotPackageType type, + const std::vector& body); + + // Build the body of a ROBOT_MESSAGE package: 8-byte timestamp + 1-byte source + 1-byte + // message_type + payload. + static std::vector buildRobotMessageBody(uint64_t timestamp, uint8_t source, + primary_interface::RobotMessagePackageType message_type, + const std::vector& payload); + + // Build the body of a ROBOT_STATE package by concatenating all sub-packages. + static std::vector buildRobotStateBody(const std::vector& sub_packages); + + comm::TCPServer server_; + + mutable std::mutex clients_mutex_; + std::condition_variable clients_cv_; + std::vector clients_; + + std::mutex script_mutex_; + std::string last_received_script_; + std::function script_callback_; +}; + +} // namespace urcl diff --git a/tests/fake_primary_server_main.cpp b/tests/fake_primary_server_main.cpp new file mode 100644 index 000000000..5e68e2bb6 --- /dev/null +++ b/tests/fake_primary_server_main.cpp @@ -0,0 +1,73 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2026 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#include +#include +#include +#include + +#include + +#include "fake_primary_server.h" + +int main(int argc, char* argv[]) +{ + int port = urcl::primary_interface::UR_PRIMARY_PORT; + if (argc > 1) + { + port = std::atoi(argv[1]); + } + + urcl::FakePrimaryServer server(port); + URCL_LOG_INFO("Fake primary server listening on port %d. Press Ctrl-C to quit.", server.getPort()); + + // Send a version message every time a client connects, mirroring the real robot's behaviour. + std::thread version_thread([&server]() { + size_t last_count = 0; + while (true) + { + const size_t count = server.getClientCount(); + if (count > last_count) + { + URCL_LOG_INFO("Client connected, sending VersionMessage."); + server.sendVersionMessage(); + } + last_count = count; + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + }); + version_thread.detach(); + + while (true) + { + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + return 0; +} diff --git a/tests/test_fake_primary_server.cpp b/tests/test_fake_primary_server.cpp new file mode 100644 index 000000000..658880289 --- /dev/null +++ b/tests/test_fake_primary_server.cpp @@ -0,0 +1,337 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2026 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#include + +#include +#include +#include +#include +#include +#include + +#include "ur_client_library/comm/pipeline.h" +#include "ur_client_library/comm/producer.h" +#include "ur_client_library/comm/stream.h" +#include "ur_client_library/comm/tcp_socket.h" +#include "ur_client_library/exceptions.h" +#include "ur_client_library/primary/abstract_primary_consumer.h" +#include "ur_client_library/primary/primary_package.h" +#include "ur_client_library/primary/primary_parser.h" +#include "ur_client_library/primary/robot_message/error_code_message.h" +#include "ur_client_library/primary/robot_message/key_message.h" +#include "ur_client_library/primary/robot_message/runtime_exception_message.h" +#include "ur_client_library/primary/robot_message/safety_mode_message.h" +#include "ur_client_library/primary/robot_message/text_message.h" +#include "ur_client_library/primary/robot_message/version_message.h" +#include "ur_client_library/primary/robot_state/robot_mode_data.h" + +#include "fake_primary_server.h" + +using namespace urcl; + +namespace +{ +// Picks a free, ephemeral port for each test so that they can run in parallel without colliding. +constexpr int ANY_PORT = 0; + +/*! + * \brief Simple consumer that collects parsed PrimaryPackage objects into a thread-safe queue, + * so individual tests can wait for specific messages to arrive. + */ +class CollectingConsumer : public comm::IConsumer +{ +public: + bool consume(std::shared_ptr product) override + { + if (product == nullptr) + { + return false; + } + std::lock_guard lock(mutex_); + queue_.push(product); + cv_.notify_all(); + return true; + } + + template + std::shared_ptr waitFor(const std::chrono::milliseconds timeout = std::chrono::seconds(1)) + { + const auto deadline = std::chrono::steady_clock::now() + timeout; + std::unique_lock lock(mutex_); + while (true) + { + while (!queue_.empty()) + { + auto front = queue_.front(); + queue_.pop(); + if (auto typed = std::dynamic_pointer_cast(front)) + { + return typed; + } + } + if (cv_.wait_until(lock, deadline) == std::cv_status::timeout) + { + return nullptr; + } + } + } + +private: + std::mutex mutex_; + std::condition_variable cv_; + std::queue> queue_; +}; + +/*! + * \brief Helper that hooks a real PrimaryParser-based pipeline onto a TCP stream so we can verify + * that data sent by FakePrimaryServer is actually a well-formed primary package and decodes back + * to the expected fields. + */ +class TestClient +{ +public: + TestClient(const std::string& host, int port) + : stream_(host, port) + , producer_(stream_, parser_) + , consumer_(std::make_shared()) + , pipeline_(producer_, consumer_.get(), "TestClient Pipeline", notifier_) + { + parser_.setStrictMode(false); + pipeline_.init(); + pipeline_.run(); + } + + ~TestClient() + { + pipeline_.stop(); + stream_.close(); + } + + CollectingConsumer& consumer() + { + return *consumer_; + } + + bool send(const std::string& text) + { + size_t written = 0; + return stream_.write(reinterpret_cast(text.data()), text.size(), written) && written == text.size(); + } + +private: + comm::INotifier notifier_; + primary_interface::PrimaryParser parser_; + comm::URStream stream_; + comm::URProducer producer_; + std::shared_ptr consumer_; + comm::Pipeline pipeline_; +}; + +} // namespace + +class FakePrimaryServerTest : public ::testing::Test +{ +protected: + void SetUp() override + { + server_ = std::make_unique(ANY_PORT); + client_ = std::make_unique("127.0.0.1", server_->getPort()); + ASSERT_TRUE(server_->waitForClient(std::chrono::seconds(2))); + } + + void TearDown() override + { + client_.reset(); + server_.reset(); + } + + std::unique_ptr server_; + std::unique_ptr client_; +}; + +TEST_F(FakePrimaryServerTest, send_version_message_roundtrip) +{ + ASSERT_TRUE(server_->sendVersionMessage("URControl", 5, 24, 0, 4242, "04-10-2025, 00:25:33")); + + auto msg = client_->consumer().waitFor(); + ASSERT_NE(msg, nullptr); + EXPECT_EQ(msg->project_name_, "URControl"); + EXPECT_EQ(msg->major_version_, 5); + EXPECT_EQ(msg->minor_version_, 24); + EXPECT_EQ(msg->svn_version_, 0); + EXPECT_EQ(msg->build_number_, 4242); + EXPECT_EQ(msg->build_date_, "04-10-2025, 00:25:33"); +} + +TEST_F(FakePrimaryServerTest, send_text_message_roundtrip) +{ + ASSERT_TRUE(server_->sendTextMessage("hello world")); + auto msg = client_->consumer().waitFor(); + ASSERT_NE(msg, nullptr); + EXPECT_EQ(msg->text_, "hello world"); +} + +TEST_F(FakePrimaryServerTest, send_key_message_roundtrip) +{ + ASSERT_TRUE(server_->sendKeyMessage("PROGRAM_XXX_STARTED", "test_fun", 1, 2)); + auto msg = client_->consumer().waitFor(); + ASSERT_NE(msg, nullptr); + EXPECT_EQ(msg->title_, "PROGRAM_XXX_STARTED"); + EXPECT_EQ(msg->text_, "test_fun"); + EXPECT_EQ(msg->message_code_, 1); + EXPECT_EQ(msg->message_argument_, 2); +} + +TEST_F(FakePrimaryServerTest, send_runtime_exception_message_roundtrip) +{ + ASSERT_TRUE(server_->sendRuntimeExceptionMessage(12, 5, "compile_error_name_not_found:foo:")); + auto msg = client_->consumer().waitFor(); + ASSERT_NE(msg, nullptr); + EXPECT_EQ(msg->line_number_, 12u); + EXPECT_EQ(msg->column_number_, 5u); + EXPECT_EQ(msg->text_, "compile_error_name_not_found:foo:"); +} + +TEST_F(FakePrimaryServerTest, send_safety_mode_message_roundtrip) +{ + ASSERT_TRUE(server_->sendSafetyModeMessage(SafetyMode::PROTECTIVE_STOP, 7, 8, 0, 1)); + auto msg = client_->consumer().waitFor(); + ASSERT_NE(msg, nullptr); + EXPECT_EQ(msg->safety_mode_type_, SafetyMode::PROTECTIVE_STOP); + EXPECT_EQ(msg->message_code_, 7); + EXPECT_EQ(msg->message_argument_, 8); + EXPECT_EQ(msg->report_data_type_, 0u); +} + +TEST_F(FakePrimaryServerTest, send_error_code_message_roundtrip) +{ + ASSERT_TRUE(server_->sendErrorCodeMessage(210, 0, primary_interface::ReportLevel::VIOLATION, "read-only PI")); + auto msg = client_->consumer().waitFor(); + ASSERT_NE(msg, nullptr); + EXPECT_EQ(msg->message_code_, 210); + EXPECT_EQ(msg->report_level_, primary_interface::ReportLevel::VIOLATION); + EXPECT_EQ(msg->text_, "read-only PI"); +} + +TEST_F(FakePrimaryServerTest, send_robot_mode_data_roundtrip) +{ + ASSERT_TRUE(server_->sendRobotModeData(RobotMode::RUNNING, true, true, true, false, false, true, false)); + auto msg = client_->consumer().waitFor(); + ASSERT_NE(msg, nullptr); + EXPECT_EQ(msg->robot_mode_, static_cast(RobotMode::RUNNING)); + EXPECT_TRUE(msg->is_real_robot_connected_); + EXPECT_TRUE(msg->is_program_running_); + EXPECT_FALSE(msg->is_protective_stopped_); +} + +TEST_F(FakePrimaryServerTest, script_callback_is_invoked) +{ + std::mutex mutex; + std::condition_variable cv; + std::string captured; + server_->setScriptCallback([&](const std::string& payload) { + std::lock_guard lock(mutex); + captured = payload; + cv.notify_all(); + }); + + ASSERT_TRUE(client_->send("textmsg(\"hi\")\n")); + + std::unique_lock lock(mutex); + ASSERT_TRUE(cv.wait_for(lock, std::chrono::seconds(1), [&]() { return !captured.empty(); })); + EXPECT_EQ(captured, "textmsg(\"hi\")\n"); + EXPECT_EQ(server_->getLastReceivedScript(), "textmsg(\"hi\")\n"); +} + +TEST_F(FakePrimaryServerTest, raw_send_forwards_unmodified_bytes) +{ + // Build a minimal VersionMessage by hand and use sendRaw. Total size: 4 (size) + 1 (type) + 8 + // (timestamp) + 1 (source) + 1 (message_type) + 1 (name_length) + 2 (name) + 1 (major) + 1 + // (minor) + 4 (svn) + 4 (build) = 28 bytes; build_date is empty (parsed from remainder). + const std::vector raw = { + 0x00, 0x00, 0x00, 0x1C, // size = 28 (big-endian) + 0x14, // type = ROBOT_MESSAGE + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // timestamp + 0xfe, // source + 0x03, // robot_message_type = VERSION + 0x02, 0x55, 0x52, // project_name_length = 2, "UR" + 0x05, 0x18, // major = 5, minor = 24 + 0x00, 0x00, 0x00, 0x00, // svn_version = 0 + 0x00, 0x00, 0x00, 0x2A // build_number = 42 + }; + + ASSERT_TRUE(server_->sendRaw(raw.data(), raw.size())); + + auto msg = client_->consumer().waitFor(); + ASSERT_NE(msg, nullptr); + EXPECT_EQ(msg->project_name_, "UR"); + EXPECT_EQ(msg->major_version_, 5); + EXPECT_EQ(msg->minor_version_, 24); + EXPECT_EQ(msg->svn_version_, 0); + EXPECT_EQ(msg->build_number_, 42); + EXPECT_EQ(msg->build_date_, ""); +} + +TEST(FakePrimaryServerSingleTest, multiple_clients_receive_broadcasts) +{ + FakePrimaryServer server(ANY_PORT); + TestClient a("127.0.0.1", server.getPort()); + TestClient b("127.0.0.1", server.getPort()); + + // Wait until both have connected. + auto start = std::chrono::steady_clock::now(); + while (server.getClientCount() < 2 && std::chrono::steady_clock::now() - start < std::chrono::seconds(2)) + { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + ASSERT_EQ(server.getClientCount(), 2u); + + ASSERT_TRUE(server.sendTextMessage("broadcast")); + + auto msg_a = a.consumer().waitFor(); + auto msg_b = b.consumer().waitFor(); + ASSERT_NE(msg_a, nullptr); + ASSERT_NE(msg_b, nullptr); + EXPECT_EQ(msg_a->text_, "broadcast"); + EXPECT_EQ(msg_b->text_, "broadcast"); +} + +TEST(FakePrimaryServerSingleTest, send_without_client_returns_false) +{ + FakePrimaryServer server(ANY_PORT); + EXPECT_FALSE(server.sendTextMessage("nobody listening")); +} + +int main(int argc, char* argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/tests/test_primary_client.cpp b/tests/test_primary_client.cpp index 1b99eafa0..430d011d7 100644 --- a/tests/test_primary_client.cpp +++ b/tests/test_primary_client.cpp @@ -37,6 +37,7 @@ #include #include #include +#include "fake_primary_server.h" #include "ur_client_library/exceptions.h" #include "ur_client_library/helpers.h" @@ -96,6 +97,28 @@ class PrimaryClientTest : public ::testing::Test comm::INotifier notifier_; }; +class PrimaryClientFakeTest : public ::testing::Test +{ +protected: + void SetUp() override + { + server_ = std::make_unique(30001); + client_ = std::make_unique("127.0.0.1", notifier_); + EXPECT_NO_THROW(client_->start()); + EXPECT_TRUE(server_->waitForClient()); + } + + void TearDown() override + { + client_.reset(); + server_.reset(); + } + + std::unique_ptr server_; + std::unique_ptr client_; + comm::INotifier notifier_; +}; + TEST_F(PrimaryClientTest, start_communication_succeeds) { EXPECT_NO_THROW(client_->start()); @@ -426,6 +449,213 @@ 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)); +} + +TEST_F(PrimaryClientFakeTest, test_send_script_blocking_fail_on_missing_robot_mode) +{ + // We did NOT send a robot mode, yet. + + EXPECT_FALSE( + client_->sendScriptBlocking("textmsg(\"Still running\")", "test_fun", std::chrono::milliseconds(1000), false)); + + server_->setScriptCallback([this]([[maybe_unused]] const std::string& payload) { + server_->sendKeyMessage("PROGRAM_XXX_STARTED", "test_fun"); + std::this_thread::sleep_for(std::chrono::milliseconds(20)); + server_->sendKeyMessage("PROGRAM_XXX_STOPPED", "test_fun"); + }); + std::thread delayed_robot_mode_thread([this]() { + // We will send the robot mode data and program started message after a short delay, which should allow the + // sendScriptBlocking call to succeed even though it initially times out waiting for robot mode data. + // std::this_thread::sleep_for(std::chrono::milliseconds(200)); + server_->sendRobotModeData(RobotMode::RUNNING, true, true, true, false, false, false, false); + }); + EXPECT_TRUE( + client_->sendScriptBlocking("textmsg(\"Still running\")", "test_fun", std::chrono::milliseconds(1000), false)); + + if (delayed_robot_mode_thread.joinable()) + { + delayed_robot_mode_thread.join(); + } +} + +TEST_F(PrimaryClientFakeTest, test_send_script_blocking_fail_on_fault) +{ + server_->sendRobotModeData(RobotMode::RUNNING, true, true, true, false, false, false, false); + + const std::string script_code = "textmsg(\"Still running\")"; + + // Make the fake server send an error code message with severity fault when it receives a script + server_->setScriptCallback([this, script_code](const std::string& payload) { + if (payload.find(script_code) != std::string::npos) + { + server_->sendKeyMessage("PROGRAM_XXX_STARTED", "test_fun"); + ASSERT_EQ(payload, "def test_fun():\n " + script_code + "\nend\n\n"); + ASSERT_TRUE(server_->sendErrorCodeMessage(999, 0, primary_interface::ReportLevel::FAULT, "Simulated fault")); + } + else + { + // We expect that the primary client calls "stop program" when it receives a fault + ASSERT_EQ(payload, "stop program\n"); + } + }); + + URCL_LOG_INFO("Sending script that will trigger a fault on the fake server"); + EXPECT_FALSE(client_->sendScriptBlocking(script_code, "test_fun", std::chrono::milliseconds(1000), false)); +} + +TEST_F(PrimaryClientFakeTest, test_send_script_to_read_only_server) +{ + server_->sendRobotModeData(RobotMode::RUNNING, true, true, true, false, false, false, false); + + const std::string script_code = "textmsg(\"Still running\")"; + + // Make the fake server send an error code message with code 210 (read-only primary interface) when it receives a + // script + server_->setScriptCallback([this, script_code]([[maybe_unused]] const std::string& payload) { + ASSERT_TRUE(server_->sendErrorCodeMessage(210, 0, primary_interface::ReportLevel::VIOLATION, + "Simulated read-only primary interface error")); + }); + + EXPECT_FALSE(client_->sendScriptBlocking(script_code, "test_fun", std::chrono::milliseconds(1000), false)); +} + +TEST_F(PrimaryClientFakeTest, test_send_script_blocking_timeout_on_no_response) +{ + server_->sendRobotModeData(RobotMode::RUNNING, true, true, true, false, false, false, false); + + const std::string script_code = "textmsg(\"Still running\")"; + + // We do not set a script callback on the fake server, so it will not respond to the script being sent. This should + // cause sendScriptBlocking to time out and return false. + EXPECT_FALSE(client_->sendScriptBlocking(script_code, "test_fun", std::chrono::milliseconds(100), false)); +} + int main(int argc, char* argv[]) { ::testing::InitGoogleTest(&argc, argv);