diff --git a/CMakeLists.txt b/CMakeLists.txt index 878ffede2..4b4fe7593 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -25,10 +25,17 @@ add_library(urcl SHARED src/control/trajectory_point_interface.cpp src/control/script_command_interface.cpp src/primary/primary_package.cpp + src/primary/program_state_message.cpp src/primary/robot_message.cpp src/primary/robot_state.cpp src/primary/robot_message/version_message.cpp + src/primary/robot_message/key_message.cpp + src/primary/robot_message/error_code_message.cpp + src/primary/robot_message/runtime_exception_message.cpp + src/primary/robot_message/text_message.cpp src/primary/robot_state/kinematics_info.cpp + src/primary/robot_state/robot_mode_data.cpp + src/primary/primary_client.cpp src/rtde/control_package_pause.cpp src/rtde/control_package_setup_inputs.cpp src/rtde/control_package_setup_outputs.cpp diff --git a/examples/primary_pipeline.cpp b/examples/primary_pipeline.cpp index e4390d8b5..ca53c31e0 100644 --- a/examples/primary_pipeline.cpp +++ b/examples/primary_pipeline.cpp @@ -25,10 +25,8 @@ */ //---------------------------------------------------------------------- -#include -#include -#include -#include +#include +#include using namespace urcl; @@ -55,30 +53,67 @@ int main(int argc, char* argv[]) second_to_run = std::stoi(argv[2]); } - // First of all, we need a stream that connects to the robot - comm::URStream primary_stream(robot_ip, urcl::primary_interface::UR_PRIMARY_PORT); + // The robot should be running in order to send script code to it + // Connect the the robot Dashboard + std::unique_ptr dashboard_client; + dashboard_client.reset(new DashboardClient(robot_ip)); + if (!dashboard_client->connect()) + { + URCL_LOG_ERROR("Could not connect to dashboard"); + return 1; + } + + // Stop program, if there is one running + if (!dashboard_client->commandStop()) + { + URCL_LOG_ERROR("Could not send stop program command"); + return 1; + } + + // Release the brakes + if (!dashboard_client->commandBrakeRelease()) + { + URCL_LOG_ERROR("Could not send BrakeRelease command"); + return 1; + } - // This will parse the primary packages - primary_interface::PrimaryParser parser; + primary_interface::PrimaryClient primary_client(robot_ip); - // The producer needs both, the stream and the parser to fully work - comm::URProducer prod(primary_stream, parser); - prod.setupProducer(); + // Check that the calibration checksum matches the one provided from the robot + const std::string calibration_check_sum = ""; + bool check_calibration_result = primary_client.checkCalibration(calibration_check_sum); + std::string calibration_check_sum_matches = check_calibration_result ? "true" : "false"; + URCL_LOG_INFO("calibration check sum matches: %s", calibration_check_sum_matches.c_str()); - // The shell consumer will print the package contents to the shell - std::unique_ptr> consumer; - consumer.reset(new comm::ShellConsumer()); + // Send a script program to the robot + std::stringstream cmd; + cmd.imbue(std::locale::classic()); // Make sure, decimal divider is actually '.' + cmd << "def test():" << std::endl << "textmsg(\"Hello from script program\")" << std::endl << "end"; - // The notifer will be called at some points during connection setup / loss. This isn't fully - // implemented atm. - comm::INotifier notifier; + if (primary_client.sendScript(cmd.str())) + { + URCL_LOG_INFO("Script program was successfully sent to the robot"); + } + else + { + URCL_LOG_ERROR("Script program wasn't send successfully to the robot"); + return 1; + } - // Now that we have all components, we can create and start the pipeline to run it all. - comm::Pipeline pipeline(prod, consumer.get(), "Pipeline", notifier); - pipeline.run(); + // Send a secondary script program to the robot + cmd.str(""); + cmd << "sec setup():" << std::endl << "textmsg(\"Hello from secondary program\")" << std::endl << "end"; + if (primary_client.sendSecondaryScript(cmd.str())) + { + URCL_LOG_INFO("Secondary script program was successfully sent to the robot"); + } + else + { + URCL_LOG_ERROR("Secondary script program wasn't send successfully to the robot"); + return 1; + } // Package contents will be printed while not being interrupted - // Note: Packages for which the parsing isn't implemented, will only get their raw bytes printed. do { std::this_thread::sleep_for(std::chrono::seconds(second_to_run)); diff --git a/examples/primary_pipeline_calibration.cpp b/examples/primary_pipeline_calibration.cpp index 76a3094d7..035e5a944 100644 --- a/examples/primary_pipeline_calibration.cpp +++ b/examples/primary_pipeline_calibration.cpp @@ -16,14 +16,12 @@ // limitations under the License. // -- END LICENSE BLOCK ------------------------------------------------ -#include -#include -#include -#include +#include using namespace urcl; -class CalibrationConsumer : public urcl::comm::IConsumer +// Create a primary consumer for logging calibration data +class CalibrationConsumer : public primary_interface::AbstractPrimaryConsumer { public: CalibrationConsumer() : calibrated_(0), have_received_data(false) @@ -31,15 +29,49 @@ class CalibrationConsumer : public urcl::comm::IConsumer product) + // We should consume all primary packages supported, in this example we just ignore the messages + virtual bool consume(primary_interface::RobotMessage& pkg) override + { + return true; + } + virtual bool consume(primary_interface::RobotState& pkg) override + { + return true; + } + virtual bool consume(primary_interface::ProgramStateMessage& pkg) override + { + return true; + } + virtual bool consume(primary_interface::VersionMessage& pkg) override + { + return true; + } + virtual bool consume(primary_interface::ErrorCodeMessage& pkg) override + { + return true; + } + virtual bool consume(primary_interface::RuntimeExceptionMessage& pkg) override + { + return true; + } + virtual bool consume(primary_interface::KeyMessage& pkg) override + { + return true; + } + virtual bool consume(primary_interface::RobotModeData& pkg) override + { + return true; + } + virtual bool consume(primary_interface::TextMessage& pkg) override { - auto kin_info = std::dynamic_pointer_cast(product); - if (kin_info != nullptr) - { - URCL_LOG_INFO("%s", product->toString().c_str()); - calibrated_ = kin_info->calibration_status_; - have_received_data = true; - } + return true; + } + + // The kinematics info stores the calibration data + virtual bool consume(primary_interface::KinematicsInfo& pkg) override + { + calibrated_ = pkg.calibration_status_; + have_received_data = true; return true; } @@ -63,9 +95,11 @@ class CalibrationConsumer : public urcl::comm::IConsumer primary_stream(robot_ip, urcl::primary_interface::UR_PRIMARY_PORT); + // Create a primary client + primary_interface::PrimaryClient primary_client(robot_ip); - // This will parse the primary packages - primary_interface::PrimaryParser parser; + std::shared_ptr calibration_consumer; + calibration_consumer.reset(new CalibrationConsumer()); - // The producer needs both, the stream and the parser to fully work - comm::URProducer prod(primary_stream, parser); - prod.setupProducer(); + // Add the calibration consumer to the primary consumers + primary_client.addPrimaryConsumer(calibration_consumer); - // The calibration consumer will print the package contents to the shell - CalibrationConsumer calib_consumer; + // Kinematics info is only send when you connect to the primary interface, so triggering a reconnect + primary_client.reconnect(); - // The notifer will be called at some points during connection setup / loss. This isn't fully - // implemented atm. - comm::INotifier notifier; - - // Now that we have all components, we can create and start the pipeline to run it all. - comm::Pipeline calib_pipeline(prod, &calib_consumer, "Pipeline", notifier); - calib_pipeline.run(); - - // Package contents will be printed while not being interrupted - // Note: Packages for which the parsing isn't implemented, will only get their raw bytes printed. - while (!calib_consumer.calibrationStatusReceived()) + while (!calibration_consumer->calibrationStatusReceived()) { std::this_thread::sleep_for(std::chrono::seconds(1)); } - if (calib_consumer.isCalibrated()) + if (calibration_consumer->isCalibrated()) { printf("The robot on IP: %s is calibrated\n", robot_ip.c_str()); } @@ -113,5 +136,8 @@ int main(int argc, char* argv[]) printf("Remeber to turn on the robot to get calibration stored on the robot!\n"); } + // We can remove the consumer again once we are done using it + primary_client.removePrimaryConsumer(calibration_consumer); + return 0; } diff --git a/include/ur_client_library/comm/pipeline.h b/include/ur_client_library/comm/pipeline.h index 7756aca47..0059be13d 100644 --- a/include/ur_client_library/comm/pipeline.h +++ b/include/ur_client_library/comm/pipeline.h @@ -29,6 +29,8 @@ #include #include #include +#include +#include namespace urcl { @@ -89,7 +91,7 @@ template class MultiConsumer : public IConsumer { private: - std::vector*> consumers_; + std::vector>> consumers_; public: /*! @@ -97,10 +99,38 @@ class MultiConsumer : public IConsumer * * \param consumers The list of consumers that should all consume given products */ - MultiConsumer(std::vector*> consumers) : consumers_(consumers) + MultiConsumer(std::vector>> consumers) : consumers_(consumers) { } + /*! + * \brief Adds a new consumer to the list of consumers + * + * \param consumer Consumer that should be added to the list + */ + void addConsumer(std::shared_ptr> consumer) + { + std::lock_guard lk(consumer_list); + consumers_.push_back(consumer); + } + + /*! + * \brief Remove a consumer from the list of consumers + * + * \param consumer Consumer that should be removed from the list + */ + void removeConsumer(std::shared_ptr> consumer) + { + std::lock_guard lk(consumer_list); + auto it = std::find(consumers_.begin(), consumers_.end(), consumer); + if (it == consumers_.end()) + { + URCL_LOG_ERROR("Unable to remove consumer as it is not part of the consumer list"); + return; + } + consumers_.erase(it); + } + /*! * \brief Sets up all registered consumers. */ @@ -151,6 +181,7 @@ class MultiConsumer : public IConsumer */ bool consume(std::shared_ptr product) { + std::lock_guard lk(consumer_list); bool res = true; for (auto& con : consumers_) { @@ -159,6 +190,9 @@ class MultiConsumer : public IConsumer } return res; } + +private: + std::mutex consumer_list; }; /*! diff --git a/include/ur_client_library/primary/abstract_primary_consumer.h b/include/ur_client_library/primary/abstract_primary_consumer.h index b2c13c6d6..0508bc6ad 100644 --- a/include/ur_client_library/primary/abstract_primary_consumer.h +++ b/include/ur_client_library/primary/abstract_primary_consumer.h @@ -31,8 +31,14 @@ #include "ur_client_library/log.h" #include "ur_client_library/comm/pipeline.h" +#include "ur_client_library/primary/program_state_message.h" +#include "ur_client_library/primary/robot_message/key_message.h" #include "ur_client_library/primary/robot_message/version_message.h" +#include "ur_client_library/primary/robot_message/error_code_message.h" +#include "ur_client_library/primary/robot_message/runtime_exception_message.h" +#include "ur_client_library/primary/robot_message/text_message.h" #include "ur_client_library/primary/robot_state/kinematics_info.h" +#include "ur_client_library/primary/robot_state/robot_mode_data.h" namespace urcl { @@ -51,7 +57,7 @@ class AbstractPrimaryConsumer : public comm::IConsumer virtual ~AbstractPrimaryConsumer() = default; /*! - * \brief This consume method is usally being called by the Pipeline structure. We don't + * \brief This consume method is usually being called by the Pipeline structure. We don't * necessarily need to know the specific package type here, as the packages themselves will take * care to be consumed with the correct function (visitor pattern). * @@ -71,8 +77,14 @@ class AbstractPrimaryConsumer : public comm::IConsumer // To be implemented in specific consumers virtual bool consume(RobotMessage& pkg) = 0; virtual bool consume(RobotState& pkg) = 0; + virtual bool consume(ProgramStateMessage& pkg) = 0; virtual bool consume(VersionMessage& pkg) = 0; virtual bool consume(KinematicsInfo& pkg) = 0; + virtual bool consume(ErrorCodeMessage& pkg) = 0; + virtual bool consume(RuntimeExceptionMessage& pkg) = 0; + virtual bool consume(KeyMessage& pkg) = 0; + virtual bool consume(RobotModeData& pkg) = 0; + virtual bool consume(TextMessage& pkg) = 0; private: /* data */ diff --git a/include/ur_client_library/primary/primary_client.h b/include/ur_client_library/primary/primary_client.h new file mode 100644 index 000000000..e07acbb06 --- /dev/null +++ b/include/ur_client_library/primary/primary_client.h @@ -0,0 +1,161 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2019 FZI Forschungszentrum Informatik +// +// Licensed under the Apache License, Text 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// -- END LICENSE BLOCK ------------------------------------------------ + +//---------------------------------------------------------------------- +/*!\file + * + * \author Felix Exner exner@fzi.de + * \date 2020-04-30 + * + */ +//---------------------------------------------------------------------- +#ifndef UR_CLIENT_LIBRARY_PRIMARY_CLIENT_H_INCLUDED +#define UR_CLIENT_LIBRARY_PRIMARY_CLIENT_H_INCLUDED + +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace urcl +{ +namespace primary_interface +{ +class PrimaryClient +{ +public: + PrimaryClient() = delete; + PrimaryClient(const std::string& robot_ip); + virtual ~PrimaryClient() = default; + + /*! + * \brief Checks if the kinematics information in the used model fits the actual robot. + * + * \param checksum Hash of the used kinematics information + * + * \returns True if the robot's calibration checksum matches the one given to the checker. False + * if it doesn't match + */ + bool checkCalibration(const std::string& checksum) const; + + /*! + * \brief Sends a custom script program to the robot. + * + * This function will wait until feedback is received from the robot. This could be either error feedback or that the + * script has started running. + * + * The given code must be valid according the UR Scripting Manual. + * + * \param script_code URScript code that shall be executed by the robot. + * \param timeout Time to wait for feedback from the robot + * + * \returns true if the scripts starts running successfully, false otherwise. + */ + bool sendScript(const std::string& script_code, + const std::chrono::milliseconds timeout = std::chrono::milliseconds(500)); + + /*! + * \brief Sends a secondary custom script program to the robot. + * + * The UR robot supports executing secondary script programs alongside a running program. This function is + * for executing secondary script programs alongside a running program and it will wait for error feedback + * if any is received, but it will not wait for the script to start running. + * + * The given code must be valid according the UR Scripting Manual. + * + * \param script_code URScript code that shall be executed by the robot. + * \param timeout Time to wait for feedback from the robot + * + * \returns true if no error feedback is received within the timeout, false otherwise. + */ + bool sendSecondaryScript(const std::string& script_code, + const std::chrono::milliseconds timeout = std::chrono::milliseconds(100)); + + /*! + * \brief Reconnect to the primary interface, this is necessary if you switch from local to remote control, in order + * to be able to send script code to the robot + */ + void reconnect() const; + + /*! + * \brief Adds a primary consumer to the list of consumers + * + * \param primary_consumer Primary consumer that should be added to the list + */ + void addPrimaryConsumer(std::shared_ptr primary_consumer); + + /*! + * \brief Remove a primary consumer from the list of consumers + * + * \param primary_consumer Primary consumer that should be removed from the list + */ + void removePrimaryConsumer(std::shared_ptr primary_consumer); + +private: + // Used internally to determine the type of feedback received from the robot, when executing script code + enum class RobotFeedbackType : uint8_t + { + ErrorMessage = 0, + RuntimeException = 1, + KeyMessage = 2 + }; + + // The function is called whenever a key message is received + void keyMessageCallback(KeyMessage& msg); + + // The function is called whenever an error code message is received + void errorMessageCallback(ErrorCodeMessage& msg); + + // The function is called whenever a runtime exception message is received + void runtimeExceptionMessageCallback(RuntimeExceptionMessage& msg); + + // Wait for feedback from the robot, returns true if the program starts running, false if an error is received or if + // the function times out + bool waitForRobotFeedback(const std::chrono::milliseconds timeout); + + // Wait for error feedback from the robot, returns true if no error is received within the timeout, false if an error + // is received + bool waitForRobotErrorFeedback(const std::chrono::milliseconds timeout); + + std::string robot_ip_; + PrimaryParser parser_; + + std::shared_ptr consumer_; + std::unique_ptr> multi_consumer_; + + comm::INotifier notifier_; + std::unique_ptr> producer_; + std::unique_ptr> stream_; + std::unique_ptr> pipeline_; + + std::mutex robot_feedback_mutex_; + std::condition_variable robot_feedback_cv_; + RobotFeedbackType robot_feedback_type_; + std::unique_ptr key_message_; + std::unique_ptr error_code_message_; +}; + +} // namespace primary_interface +} // namespace urcl + +#endif // ifndef UR_CLIENT_LIBRARY_PRIMARY_CLIENT_H_INCLUDED \ No newline at end of file diff --git a/include/ur_client_library/primary/primary_consumer.h b/include/ur_client_library/primary/primary_consumer.h new file mode 100644 index 000000000..9b3ed82b4 --- /dev/null +++ b/include/ur_client_library/primary/primary_consumer.h @@ -0,0 +1,345 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- +// +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2020 FZI Forschungszentrum Informatik +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// -- END LICENSE BLOCK ------------------------------------------------ + +//---------------------------------------------------------------------- +/*!\file + * + * \author Felix Exner exner@fzi.de + * \date 2020-04-30 + * + */ +//---------------------------------------------------------------------- + +#ifndef UR_CLIENT_LIBRARY_PRIMARY_CONSUMER_H_INCLUDED +#define UR_CLIENT_LIBRARY_PRIMARY_CONSUMER_H_INCLUDED + +#include "ur_client_library/primary/abstract_primary_consumer.h" + +#include +#include +#include + +namespace urcl +{ +namespace primary_interface +{ +/*! + * \brief Primary consumer implementation + * + * This class implements am AbstractPrimaryConsumer such that it can consume all incoming primary + * messages. The actual work for each package will be done in this class. + */ +class PrimaryConsumer : public AbstractPrimaryConsumer +{ +public: + PrimaryConsumer() : calibration_package_received_(false), robot_mode_data_received_(false) + { + } + virtual ~PrimaryConsumer() = default; + + /*! + * \brief Consume a RobotMessage + * + * \param msg Robot message + * + * \returns True + */ + virtual bool consume(RobotMessage& msg) override + { + return true; + } + + /*! + * \brief Consume a RobotState + * + * \param msg Robot state + * + * \returns True + */ + virtual bool consume(RobotState& msg) override + { + return true; + } + + /*! + * \brief Consume a ProgramStateMessage + * + * \param msg Program state message + * + * \returns True + */ + virtual bool consume(ProgramStateMessage& msg) override + { + return true; + } + + /*! + * \brief Handle a VersionMessage + * + * \param pkg VersionMessage + * + * \returns True + */ + virtual bool consume(VersionMessage& pkg) override + { + URCL_LOG_ERROR("---VersionMessage---\n %s", pkg.toString().c_str()); + return true; + } + + /*! + * \brief Handle a KinematicsInfo + * + * \param pkg KinematicsInfo + * + * \returns True + */ + virtual bool consume(KinematicsInfo& pkg) override + { + URCL_LOG_INFO("---KinematicsInfo---\n %s", pkg.toString().c_str()); + + std::unique_lock lk(calibration_data_mutex_); + expected_hash_ = pkg.toHash(); + calibration_package_received_ = true; + return true; + } + + /*! + * \brief Handle an ErrorCodeMessage + * + * \param pkg ErrorCodeMessage + * + * \returns True + */ + virtual bool consume(ErrorCodeMessage& pkg) override + { + std::stringstream out_ss; + out_ss << "---ErrorCodeMessage---\n" << pkg.toString().c_str() << std::endl; + switch (pkg.report_level_) + { + case ReportLevel::DEBUG: + case ReportLevel::DEVL_DEBUG: + { + URCL_LOG_DEBUG("%s", out_ss.str().c_str()); + break; + } + case ReportLevel::INFO: + case ReportLevel::DEVL_INFO: + { + URCL_LOG_INFO("%s", out_ss.str().c_str()); + break; + } + case ReportLevel::WARNING: + { + URCL_LOG_WARN("%s", out_ss.str().c_str()); + break; + } + case ReportLevel::VIOLATION: + case ReportLevel::FAULT: + case ReportLevel::DEVL_VIOLATION: + case ReportLevel::DEVL_FAULT: + { + URCL_LOG_ERROR("%s", out_ss.str().c_str()); + break; + } + default: + { + std::stringstream ss; + ss << "Unknown report level: " << static_cast(pkg.report_level_) << std::endl << out_ss.str(); + URCL_LOG_ERROR("%s", ss.str().c_str()); + } + } + + if (error_code_message_callback_ != nullptr) + { + error_code_message_callback_(pkg); + } + return true; + } + + /*! + * \brief Handle a RuntimeExceptionMessage + * + * \param pkg RuntimeExceptionMessage + * + * \returns True. + */ + virtual bool consume(RuntimeExceptionMessage& pkg) override + { + URCL_LOG_ERROR("---RuntimeExceptionMessage---\n %s", pkg.toString().c_str()); + + if (runtime_exception_message_callback_ != nullptr) + { + runtime_exception_message_callback_(pkg); + } + return true; + } + + /*! + * \brief Handle a KeyMessage + * + * \param pkg keyMessage + * + * \returns True. + */ + virtual bool consume(KeyMessage& pkg) override + { + URCL_LOG_INFO("---KeyMessage---\n %s", pkg.toString().c_str()); + + if (key_message_callback_ != nullptr) + { + key_message_callback_(pkg); + } + return true; + } + + /*! + * \brief Handle a RobotModeData package + * + * \param pkg RobotModeData + * + * \returns True + */ + virtual bool consume(RobotModeData& pkg) override + { + std::lock_guard lk(robot_mode_data_mutex_); + robot_mode_data_.reset(new RobotModeData(pkg)); + robot_mode_data_received_ = true; + robot_mode_data_cv_.notify_one(); + return true; + } + + /*! + * \brief Handle a TextMessage + * + * \param pkg TextMessage + * + * \returns True + */ + virtual bool consume(TextMessage& pkg) override + { + URCL_LOG_INFO("---TextMessage---\n %s", pkg.toString().c_str()); + return true; + } + + /*! + * \brief Checks if the kinematics hash from the robot matches the expected kinematics hash + * + * \param expected_hash The expected kinematic hash + * + * \returns True if the robot's calibration checksum matches the one given to the checker. False + * if it doesn't match + */ + bool checkCalibration(const std::string& expected_hash) + { + while (!calibration_package_received_) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + std::lock_guard lk(calibration_data_mutex_); + return expected_hash_ == expected_hash; + } + + /*! + * \brief Get the robot mode data package + * + * \param wait_for_new_package if true the function will wait until a new robot mode data package is received over the + * primary interface, if false it will return the latest package received + * + * \returns RobotModeData + */ + std::shared_ptr getRobotModeData(bool wait_for_new_package = false) + { + // Make sure we have received at least one package if we are not waiting for a package + while (!robot_mode_data_received_ && wait_for_new_package == false) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + if (wait_for_new_package) + { + // Robot mode data is published with 10 Hz, so 500 milliseconds, should be plenty of time to wait for a new + // package + std::chrono::milliseconds timeout(500); + std::unique_lock lk(robot_mode_data_mutex_); + if (robot_mode_data_cv_.wait_for(lk, timeout) == std::cv_status::no_timeout) + { + return robot_mode_data_; + } + URCL_LOG_WARN("Failed to receive new robot mode data package within the timeout, are you still connected to the " + "robot? Returning the last received package"); + } + + std::lock_guard lk(robot_mode_data_mutex_); + return robot_mode_data_; + } + + /*! + * \brief Set callback function which will be triggered whenever runtime exception messages are received + * + * \param callback_function Function handling the event information. The runtime exception message received is passed + * to the function. + */ + void setRuntimeExceptionMessageCallback(std::function callback_function) + { + runtime_exception_message_callback_ = callback_function; + } + + /*! + * \brief Set callback function which will be triggered whenever key messages are received + * + * \param callback_function Function handling the event information. The key message received is passed to the + * function. + */ + void setKeyMessageCallback(std::function callback_function) + { + key_message_callback_ = callback_function; + } + + /*! + * \brief Set callback function which will be triggered whenever error code messages are received + * + * \param callback_function Function handling the event information. The error code message received is passed to the + * function. + */ + void setErrorCodeMessageCallback(std::function callback_function) + { + error_code_message_callback_ = callback_function; + } + +private: + std::function runtime_exception_message_callback_; + std::function key_message_callback_; + std::function error_code_message_callback_; + + std::string expected_hash_; + std::mutex calibration_data_mutex_; + + std::shared_ptr robot_mode_data_; + std::mutex robot_mode_data_mutex_; + std::condition_variable robot_mode_data_cv_; + + // Variables used to ensure that a package of the type has been received + bool calibration_package_received_; + bool robot_mode_data_received_; +}; + +} // namespace primary_interface +} // namespace urcl + +#endif // ifndef UR_CLIENT_LIBRARY_PRIMARY_CONSUMER_H_INCLUDED diff --git a/include/ur_client_library/primary/primary_parser.h b/include/ur_client_library/primary/primary_parser.h index 3dfba100f..d6dccb227 100644 --- a/include/ur_client_library/primary/primary_parser.h +++ b/include/ur_client_library/primary/primary_parser.h @@ -26,8 +26,14 @@ #include "ur_client_library/primary/package_header.h" #include "ur_client_library/primary/robot_state.h" #include "ur_client_library/primary/robot_message.h" +#include "ur_client_library/primary/program_state_message.h" #include "ur_client_library/primary/robot_state/kinematics_info.h" +#include "ur_client_library/primary/robot_state/robot_mode_data.h" +#include "ur_client_library/primary/robot_message/error_code_message.h" +#include "ur_client_library/primary/robot_message/runtime_exception_message.h" #include "ur_client_library/primary/robot_message/version_message.h" +#include "ur_client_library/primary/robot_message/key_message.h" +#include "ur_client_library/primary/robot_message/text_message.h" namespace urcl { @@ -115,7 +121,7 @@ class PrimaryParser : public comm::Parser case RobotPackageType::ROBOT_MESSAGE: { uint64_t timestamp; - uint8_t source; + int8_t source; RobotMessagePackageType message_type; bp.parse(timestamp); @@ -134,6 +140,28 @@ class PrimaryParser : public comm::Parser break; } + case RobotPackageType::PROGRAM_STATE_MESSAGE: + { + uint64_t timestamp; + ProgramStateMessageType state_type; + URCL_LOG_DEBUG("ProgramStateMessage received"); + + bp.parse(timestamp); + bp.parse(state_type); + + URCL_LOG_DEBUG("ProgramStateMessage of type %d received", static_cast(state_type)); + + std::unique_ptr packet(programStateFromType(state_type, timestamp)); + if (!packet->parseWith(bp)) + { + URCL_LOG_ERROR("Package parsing of type %d failed!", static_cast(state_type)); + return false; + } + + results.push_back(std::move(packet)); + return true; + } + default: { URCL_LOG_DEBUG("Invalid robot package type recieved: %u", static_cast(type)); @@ -149,12 +177,8 @@ class PrimaryParser : public comm::Parser { switch (type) { - /*case robot_state_type::ROBOT_MODE_DATA: - // SharedRobotModeData* rmd = new SharedRobotModeData(); - - //return new rmd; - case robot_state_type::MASTERBOARD_DATA: - return new MBD;*/ + case RobotStateType::ROBOT_MODE_DATA: + return new RobotModeData(type); case RobotStateType::KINEMATICS_INFO: return new KinematicsInfo(type); default: @@ -166,16 +190,31 @@ class PrimaryParser : public comm::Parser { switch (type) { - /*case robot_state_type::ROBOT_MODE_DATA: - // SharedRobotModeData* rmd = new SharedRobotModeData(); - - //return new rmd; - case robot_state_type::MASTERBOARD_DATA: - return new MBD;*/ + case RobotMessagePackageType::ROBOT_MESSAGE_KEY: + return new KeyMessage(timestamp, source); case RobotMessagePackageType::ROBOT_MESSAGE_VERSION: return new VersionMessage(timestamp, source); + case RobotMessagePackageType::ROBOT_MESSAGE_ERROR_CODE: + return new ErrorCodeMessage(timestamp, source); + case RobotMessagePackageType::ROBOT_MESSAGE_RUNTIME_EXCEPTION: + return new RuntimeExceptionMessage(timestamp, source); + case RobotMessagePackageType::ROBOT_MESSAGE_TEXT: + return new TextMessage(timestamp, source); + default: + return new RobotMessage(timestamp, source, type); + } + } + + ProgramStateMessage* programStateFromType(ProgramStateMessageType type, uint64_t timestamp) + { + switch (type) + { + // case ProgramStateMessageType::GLOBAL_VARIABLES_SETUP: + // return new GlobalVariablesSetupMessage(timestamp); + // case ProgramStateMessageType::TYPE_VARIABLES_UPDATE: + // return new TypeVariablesUpdateMessage(timestamp); default: - return new RobotMessage(timestamp, source); + return new ProgramStateMessage(timestamp, type); } } }; diff --git a/include/ur_client_library/primary/program_state_message.h b/include/ur_client_library/primary/program_state_message.h new file mode 100644 index 000000000..8c205d7aa --- /dev/null +++ b/include/ur_client_library/primary/program_state_message.h @@ -0,0 +1,108 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2019 FZI Forschungszentrum Informatik +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// -- END LICENSE BLOCK ------------------------------------------------ + +//---------------------------------------------------------------------- +/*!\file + * + * \author Felix Exner + * \date 2022-02-21 + * + */ +//---------------------------------------------------------------------- + +#ifndef UR_CLIENT_LIBRARY_PROGRAM_STATE_MESSAGE_H_INCLUDED +#define UR_CLIENT_LIBRARY_PROGRAM_STATE_MESSAGE_H_INCLUDED + +#include "ur_client_library/primary/primary_package.h" +#include "ur_client_library/primary/package_header.h" + +namespace urcl +{ +namespace primary_interface +{ +/*! + * \brief Possible ProgramStateMessage types + */ +enum class ProgramStateMessageType : uint8_t +{ + GLOBAL_VARIABLES_SETUP = 0, + GLOBAL_VARIABLES_UPDATE = 1, + TYPE_VARIABLES_UPDATE = 2, +}; + +/*! + * \brief The ProgramStateMessage class is a parent class for the different received robot messages. + */ +class ProgramStateMessage : public PrimaryPackage +{ +public: + /*! + * \brief Creates a new ProgramStateMessage object to be filled from a package. + * + * \param timestamp Timestamp of the package + */ + ProgramStateMessage(const uint64_t timestamp) : timestamp_(timestamp) + { + } + + /*! + * \brief Creates a new ProgramStateMessage object to be filled from a package. + * + * \param timestamp Timestamp of the package + * \param state_type The package's state type + */ + ProgramStateMessage(const uint64_t timestamp, const ProgramStateMessageType state_type) + : timestamp_(timestamp), state_type_(state_type) + { + } + virtual ~ProgramStateMessage() = default; + + /*! + * \brief Sets the attributes of the package by parsing a serialized representation of the + * package. + * + * \param bp A parser containing a serialized version of the package + * + * \returns True, if the package was parsed successfully, false otherwise + */ + virtual bool parseWith(comm::BinParser& bp); + + /*! + * \brief Consume this package with a specific consumer. + * + * \param consumer Placeholder for the consumer calling this + * + * \returns true on success + */ + virtual bool consumeWith(AbstractPrimaryConsumer& consumer); + + /*! + * \brief Produces a human readable representation of the package object. + * + * \returns A string representing the object + */ + virtual std::string toString() const; + + uint64_t timestamp_; + ProgramStateMessageType state_type_; +}; + +} // namespace primary_interface +} // namespace urcl + +#endif /* UR_CLIENT_LIBRARY_ROBOT_STATE_H_INCLUDED */ diff --git a/include/ur_client_library/primary/robot_message.h b/include/ur_client_library/primary/robot_message.h index 5ee0c3a2c..eeaa4f566 100644 --- a/include/ur_client_library/primary/robot_message.h +++ b/include/ur_client_library/primary/robot_message.h @@ -66,6 +66,19 @@ class RobotMessage : public PrimaryPackage RobotMessage(const uint64_t timestamp, const uint8_t source) : timestamp_(timestamp), source_(source) { } + + /*! + * \brief Creates a new RobotMessage object to be filled from a package. + * + * \param timestamp Timestamp of the package + * \param source The package's source + * \param message_type The package's message type + */ + RobotMessage(const uint64_t timestamp, const int8_t source, const RobotMessagePackageType message_type) + : timestamp_(timestamp), source_(source), message_type_(message_type) + { + } + virtual ~RobotMessage() = default; /*! diff --git a/include/ur_client_library/primary/robot_message/error_code_message.h b/include/ur_client_library/primary/robot_message/error_code_message.h new file mode 100644 index 000000000..c7681aad3 --- /dev/null +++ b/include/ur_client_library/primary/robot_message/error_code_message.h @@ -0,0 +1,114 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2019 FZI Forschungszentrum Informatik +// +// Licensed under the Apache License, Text 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// -- END LICENSE BLOCK ------------------------------------------------ + +//---------------------------------------------------------------------- +/*!\file + * + * \author Felix Exner exner@fzi.de + * \date 2020-04-23 + * + */ +//---------------------------------------------------------------------- + +#ifndef UR_CLIENT_LIBRARY_PRIMARY_ERROR_CODE_MESSAGE_H_INCLUDED +#define UR_CLIENT_LIBRARY_PRIMARY_ERROR_CODE_MESSAGE_H_INCLUDED + +#include "ur_client_library/primary/robot_message.h" + +namespace urcl +{ +namespace primary_interface +{ +enum class ReportLevel : int32_t +{ + DEBUG = 0, + INFO = 1, + WARNING = 2, + VIOLATION = 3, + FAULT = 4, + DEVL_DEBUG = 128, + DEVL_INFO = 129, + DEVL_WARNING = 130, + DEVL_VIOLATION = 131, + DEVL_FAULT = 132 +}; + +/*! + * \brief The ErrorCodeMessage class handles the error code messages sent via the primary UR interface. + */ +class ErrorCodeMessage : public RobotMessage +{ +public: + ErrorCodeMessage() = delete; + /*! + * \brief Creates a new ErrorCodeMessage object to be filled from a package. + * + * \param timestamp Timestamp of the package + * \param source The package's source + */ + ErrorCodeMessage(uint64_t timestamp, int8_t source) + : RobotMessage(timestamp, source, RobotMessagePackageType::ROBOT_MESSAGE_ERROR_CODE) + { + } + + /*! + * \brief Creates a copy of a ErrorCodeMessage object. + * + * \param pkg The ErrorCodeMessage object to be copied + */ + ErrorCodeMessage(const ErrorCodeMessage& pkg); + + virtual ~ErrorCodeMessage() = default; + + /*! + * \brief Sets the attributes of the package by parsing a serialized representation of the + * package. + * + * \param bp A parser containing a serialized text of the package + * + * \returns True, if the package was parsed successfully, false otherwise + */ + virtual bool parseWith(comm::BinParser& bp); + + /*! + * \brief Consume this package with a specific consumer. + * + * \param consumer Placeholder for the consumer calling this + * + * \returns true on success + */ + virtual bool consumeWith(AbstractPrimaryConsumer& consumer); + + /*! + * \brief Produces a human readable representation of the package object. + * + * \returns A string representing the object + */ + virtual std::string toString() const; + + int32_t message_code_; + int32_t message_argument_; + ReportLevel report_level_; + uint8_t data_type_; + uint32_t data_; + std::string text_; +}; +} // namespace primary_interface +} // namespace urcl + +#endif // ifndef UR_CLIENT_LIBRARY_PRIMARY_TEXT_MESSAGE_H_INCLUDED diff --git a/include/ur_client_library/primary/robot_message/key_message.h b/include/ur_client_library/primary/robot_message/key_message.h new file mode 100644 index 000000000..3e0891d8c --- /dev/null +++ b/include/ur_client_library/primary/robot_message/key_message.h @@ -0,0 +1,99 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2019 FZI Forschungszentrum Informatik +// +// Licensed under the Apache License, Text 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// -- END LICENSE BLOCK ------------------------------------------------ + +//---------------------------------------------------------------------- +/*!\file + * + * \author Felix Exner exner@fzi.de + * \date 2020-04-23 + * + */ +//---------------------------------------------------------------------- + +#ifndef UR_CLIENT_LIBRARY_PRIMARY_KEY_MESSAGE_H_INCLUDED +#define UR_CLIENT_LIBRARY_PRIMARY_KEY_MESSAGE_H_INCLUDED + +#include "ur_client_library/primary/robot_message.h" + +namespace urcl +{ +namespace primary_interface +{ +/*! + * \brief The KeyMessage class handles the key messages sent via the primary UR interface. + */ +class KeyMessage : public RobotMessage +{ +public: + KeyMessage() = delete; + /*! + * \brief Creates a new KeyMessage object to be filled from a package. + * + * \param timestamp Timestamp of the package + * \param source The package's source + */ + KeyMessage(uint64_t timestamp, int8_t source) + : RobotMessage(timestamp, source, RobotMessagePackageType::ROBOT_MESSAGE_KEY) + { + } + + /*! + * \brief Creates a copy of a KeyMessage object. + * + * \param pkg The KeyMessage object to be copied + */ + KeyMessage(const KeyMessage& pkg); + + virtual ~KeyMessage() = default; + + /*! + * \brief Sets the attributes of the package by parsing a serialized representation of the + * package. + * + * \param bp A parser containing a serialized text of the package + * + * \returns True, if the package was parsed successfully, false otherwise + */ + virtual bool parseWith(comm::BinParser& bp); + + /*! + * \brief Consume this package with a specific consumer. + * + * \param consumer Placeholder for the consumer calling this + * + * \returns true on success + */ + virtual bool consumeWith(AbstractPrimaryConsumer& consumer); + + /*! + * \brief Produces a human readable representation of the package object. + * + * \returns A string representing the object + */ + virtual std::string toString() const; + + int32_t message_code_; + int32_t message_argument_; + uint8_t title_size_; + std::string title_; + std::string text_; +}; +} // namespace primary_interface +} // namespace urcl + +#endif // ifndef UR_CLIENT_LIBRARY_PRIMARY_TEXT_MESSAGE_H_INCLUDED diff --git a/include/ur_client_library/primary/robot_message/runtime_exception_message.h b/include/ur_client_library/primary/robot_message/runtime_exception_message.h new file mode 100644 index 000000000..25b25e54c --- /dev/null +++ b/include/ur_client_library/primary/robot_message/runtime_exception_message.h @@ -0,0 +1,97 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2019 FZI Forschungszentrum Informatik +// +// Licensed under the Apache License, Text 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// -- END LICENSE BLOCK ------------------------------------------------ + +//---------------------------------------------------------------------- +/*!\file + * + * \author Felix Exner exner@fzi.de + * \date 2020-04-23 + * + */ +//---------------------------------------------------------------------- + +#ifndef UR_CLIENT_LIBRARY_PRIMARY_RUNTIME_EXCEPTION_MESSAGE_H_INCLUDED +#define UR_CLIENT_LIBRARY_PRIMARY_RUNTIME_EXCEPTION_MESSAGE_H_INCLUDED + +#include "ur_client_library/primary/robot_message.h" + +namespace urcl +{ +namespace primary_interface +{ +/*! + * \brief The RuntimeExceptionMessage class handles the runtime exception messages sent via the primary UR interface. + */ +class RuntimeExceptionMessage : public RobotMessage +{ +public: + RuntimeExceptionMessage() = delete; + /*! + * \brief Creates a new RuntimeExceptionMessage object to be filled from a package. + * + * \param timestamp Timestamp of the package + * \param source The package's source + */ + RuntimeExceptionMessage(uint64_t timestamp, int8_t source) + : RobotMessage(timestamp, source, RobotMessagePackageType::ROBOT_MESSAGE_RUNTIME_EXCEPTION) + { + } + + /*! + * \brief Creates a copy of a RuntimeExceptionMessage object. + * + * \param pkg The RuntimeExceptionMessage object to be copied + */ + RuntimeExceptionMessage(const RuntimeExceptionMessage& pkg); + + virtual ~RuntimeExceptionMessage() = default; + + /*! + * \brief Sets the attributes of the package by parsing a serialized representation of the + * package. + * + * \param bp A parser containing a serialized text of the package + * + * \returns True, if the package was parsed successfully, false otherwise + */ + virtual bool parseWith(comm::BinParser& bp); + + /*! + * \brief Consume this package with a specific consumer. + * + * \param consumer Placeholder for the consumer calling this + * + * \returns true on success + */ + virtual bool consumeWith(AbstractPrimaryConsumer& consumer); + + /*! + * \brief Produces a human readable representation of the package object. + * + * \returns A string representing the object + */ + virtual std::string toString() const; + + int32_t line_number_; + int32_t column_number_; + std::string text_; +}; +} // namespace primary_interface +} // namespace urcl + +#endif // ifndef UR_CLIENT_LIBRARY_PRIMARY_TEXT_MESSAGE_H_INCLUDED diff --git a/include/ur_client_library/primary/robot_message/text_message.h b/include/ur_client_library/primary/robot_message/text_message.h new file mode 100644 index 000000000..1d8413e8d --- /dev/null +++ b/include/ur_client_library/primary/robot_message/text_message.h @@ -0,0 +1,98 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2019 FZI Forschungszentrum Informatik +// +// Licensed under the Apache License, Text 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// -- END LICENSE BLOCK ------------------------------------------------ + +//---------------------------------------------------------------------- +/*!\file + * + * \author Felix Exner exner@fzi.de + * \date 2020-04-23 + * + */ +//---------------------------------------------------------------------- + +#ifndef UR_CLIENT_LIBRARY_PRIMARY_TEXT_MESSAGE_H_INCLUDED +#define UR_CLIENT_LIBRARY_PRIMARY_TEXT_MESSAGE_H_INCLUDED + +#include "ur_client_library/primary/robot_message.h" + +namespace urcl +{ +namespace primary_interface +{ +/*! + * \brief The TextMessage class handles the text messages sent via the primary UR interface. + */ +class TextMessage : public RobotMessage +{ +public: + TextMessage() = delete; + /*! + * \brief Creates a new TextMessage object to be filled from a package. + * + * \param timestamp Timestamp of the package + * \param source The package's source + */ + TextMessage(uint64_t timestamp, int8_t source) + : RobotMessage(timestamp, source, RobotMessagePackageType::ROBOT_MESSAGE_TEXT) + { + } + + /*! + * \brief Creates a copy of a TextMessage object. + * + * \param pkg The TextMessage object to be copied + */ + TextMessage(const TextMessage& pkg) + : RobotMessage(pkg.timestamp_, pkg.source_, RobotMessagePackageType::ROBOT_MESSAGE_TEXT) + { + text_ = pkg.text_; + } + virtual ~TextMessage() = default; + + /*! + * \brief Sets the attributes of the package by parsing a serialized representation of the + * package. + * + * \param bp A parser containing a serialized text of the package + * + * \returns True, if the package was parsed successfully, false otherwise + */ + virtual bool parseWith(comm::BinParser& bp); + + /*! + * \brief Consume this package with a specific consumer. + * + * \param consumer Placeholder for the consumer calling this + * + * \returns true on success + */ + virtual bool consumeWith(AbstractPrimaryConsumer& consumer); + + /*! + * \brief Produces a human readable representation of the package object. + * + * \returns A string representing the object + */ + virtual std::string toString() const; + + std::string text_; +}; +} // namespace primary_interface +} // namespace urcl + +#endif // ifndef UR_CLIENT_LIBRARY_PRIMARY_TEXT_MESSAGE_H_INCLUDED diff --git a/include/ur_client_library/primary/robot_state/robot_mode_data.h b/include/ur_client_library/primary/robot_state/robot_mode_data.h new file mode 100644 index 000000000..d6174323d --- /dev/null +++ b/include/ur_client_library/primary/robot_state/robot_mode_data.h @@ -0,0 +1,128 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#ifndef UR_CLIENT_LIBRARY_ROBOT_MODE_DATA_H_INCLUDED +#define UR_CLIENT_LIBRARY_ROBOT_MODE_DATA_H_INCLUDED + +#include "ur_client_library/types.h" +#include "ur_client_library/primary/robot_state.h" +namespace urcl +{ +namespace primary_interface +{ + +/*! + * \brief Possible robot modes + */ +enum class RobotMode : int8_t +{ + ROBOT_MODE_NO_CONTROLLER = -1, + ROBOT_MODE_DISCONNECTED = 0, + ROBOT_MODE_CONFIRM_SAFETY = 1, + ROBOT_MODE_BOOTING = 2, + ROBOT_MODE_POWER_OFF = 3, + ROBOT_MODE_POWER_ON = 4, + ROBOT_MODE_IDLE = 5, + ROBOT_MODE_BACKDRIVE = 6, + ROBOT_MODE_RUNNING = 7, + ROBOT_MODE_UPDATING_FIRMWARE = 8 +}; + +/*! + * \brief This messages contains data about the mode of the robot. + */ +class RobotModeData : public RobotState +{ +public: + RobotModeData() = delete; + /*! + * \brief Creates a new RobotModeData object. + * + * \param type The type of RobotState message received + */ + RobotModeData(const RobotStateType type) : RobotState(type) + { + } + + /*! + * \brief Creates a copy of a RobotModeData object. + * + * \param pkg The RobotModeData object to be copied + */ + RobotModeData(const RobotModeData& pkg); + + virtual ~RobotModeData() = default; + + /*! + * \brief Sets the attributes of the package by parsing a serialized representation of the + * package. + * + * \param bp A parser containing a serialized version of the package + * + * \returns True, if the package was parsed successfully, false otherwise + */ + virtual bool parseWith(comm::BinParser& bp); + + /*! + * \brief Consume this specific package with a specific consumer. + * + * \param consumer Placeholder for the consumer calling this + * + * \returns true on success + */ + virtual bool consumeWith(AbstractPrimaryConsumer& consumer); + + /*! + * \brief Produces a human readable representation of the package object. + * + * \returns A string representing the object + */ + virtual std::string toString() const; + + uint64_t timestamp_; + 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_; + int8_t robot_mode_; + uint8_t control_mode_; + double target_speed_fraction_; + double speed_scaling_; + double target_speed_fraction_limit_; + std::string reserved_; +}; + +} // namespace primary_interface +} // namespace urcl + +#endif // ifndef UR_CLIENT_LIBRARY_ROBOT_MODE_DATA_H_INCLUDED diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index c4970e3c7..c8c7a90eb 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -39,6 +39,7 @@ #include "ur_client_library/ur/version_information.h" #include "ur_client_library/ur/robot_receive_timeout.h" #include "ur_client_library/primary/robot_message/version_message.h" +#include "ur_client_library/primary/primary_client.h" #include "ur_client_library/rtde/rtde_writer.h" namespace urcl @@ -419,13 +420,35 @@ class UrDriver /*! * \brief Sends a custom script program to the robot. * + * This function will wait until feedback is received from the robot. This could be either error feedback or that the + * script has started running. + * + * The given code must be valid according the UR Scripting Manual. + * + * \param script_code URScript code that shall be executed by the robot. + * \param timeout Time to wait for feedback from the robot + * + * \returns true if the scripts starts running successfully, false otherwise. + */ + bool sendScript(const std::string& script_code, + const std::chrono::milliseconds timeout = std::chrono::milliseconds(1000)) const; + + /*! + * \brief Sends a secondary custom script program to the robot. + * + * The UR robot supports executing secondary script programs alongside a running program. This function is + * for executing secondary script programs alongside a running program and it will wait for error feedback + * if any is received, but it will not wait for the script to start running. + * * The given code must be valid according the UR Scripting Manual. * - * \param program URScript code that shall be executed by the robot. + * \param script_code URScript code that shall be executed by the robot. + * \param timeout Time to wait for feedback from the robot * - * \returns true on successful upload, false otherwise. + * \returns true if no error feedback is received within the timeout, false otherwise. */ - bool sendScript(const std::string& program); + bool sendSecondaryScript(const std::string& script_code, + const std::chrono::milliseconds timeout = std::chrono::milliseconds(100)) const; /*! * \brief Sends the external control program to the robot. @@ -499,6 +522,7 @@ class UrDriver std::unique_ptr script_sender_; std::unique_ptr> primary_stream_; std::unique_ptr> secondary_stream_; + std::unique_ptr primary_client_; uint32_t servoj_gain_; double servoj_lookahead_time_; diff --git a/src/primary/primary_client.cpp b/src/primary/primary_client.cpp new file mode 100644 index 000000000..a08e9baa4 --- /dev/null +++ b/src/primary/primary_client.cpp @@ -0,0 +1,264 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2019 FZI Forschungszentrum Informatik +// +// Licensed under the Apache License, Text 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// -- END LICENSE BLOCK ------------------------------------------------ + +//---------------------------------------------------------------------- +/*!\file + * + * \author Felix Exner exner@fzi.de + * \date 2020-04-30 + * + */ +//---------------------------------------------------------------------- + +#include + +namespace urcl +{ +namespace primary_interface +{ +PrimaryClient::PrimaryClient(const std::string& robot_ip) : robot_ip_(robot_ip) +{ + stream_.reset(new comm::URStream(robot_ip_, UR_PRIMARY_PORT)); + producer_.reset(new comm::URProducer(*stream_, parser_)); + producer_->setupProducer(); + + // Configure consumer + consumer_.reset(new PrimaryConsumer()); + consumer_->setKeyMessageCallback(std::bind(&PrimaryClient::keyMessageCallback, this, std::placeholders::_1)); + consumer_->setErrorCodeMessageCallback(std::bind(&PrimaryClient::errorMessageCallback, this, std::placeholders::_1)); + consumer_->setRuntimeExceptionMessageCallback( + std::bind(&PrimaryClient::runtimeExceptionMessageCallback, this, std::placeholders::_1)); + + // Configure multi consumer even though we only have one consumer as default, as this enables the user to add more + // consumers after the object has been created + std::vector>> consumers; + consumers.push_back(consumer_); + multi_consumer_.reset(new comm::MultiConsumer(consumers)); + + pipeline_.reset(new comm::Pipeline(*producer_, multi_consumer_.get(), "primary pipeline", notifier_)); + pipeline_->run(); +} + +bool PrimaryClient::checkCalibration(const std::string& checksum) const +{ + return consumer_->checkCalibration(checksum); +} + +bool PrimaryClient::sendScript(const std::string& script_code, const std::chrono::milliseconds timeout) +{ + if (stream_ == nullptr) + { + throw std::runtime_error("Sending script to robot requested while there is no primary interface established. This " + "should not happen."); + } + + std::shared_ptr robot_mode_data = consumer_->getRobotModeData(); + if (robot_mode_data->robot_mode_ != toUnderlying(RobotMode::ROBOT_MODE_RUNNING)) + { + URCL_LOG_ERROR("The robot should be running in order to send script commands to the robot"); + return false; + } + + // 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 = script_code + '\n'; + + size_t len = program_with_newline.size(); + const uint8_t* data = reinterpret_cast(program_with_newline.c_str()); + size_t written; + + if (!stream_->write(data, len, written)) + { + URCL_LOG_ERROR("Could not send program to robot"); + return false; + } + URCL_LOG_INFO("Sent program to robot:\n%s", program_with_newline.c_str()); + return waitForRobotFeedback(std::chrono::milliseconds(timeout)); +} + +bool PrimaryClient::sendSecondaryScript(const std::string& script_code, const std::chrono::milliseconds timeout) +{ + if (stream_ == nullptr) + { + throw std::runtime_error("Sending script to robot requested while there is no primary interface established. This " + "should not happen."); + } + + std::shared_ptr robot_mode_data = consumer_->getRobotModeData(); + if (robot_mode_data->robot_mode_ != toUnderlying(RobotMode::ROBOT_MODE_RUNNING)) + { + URCL_LOG_ERROR("The robot should be running in order to send script commands to the robot"); + return false; + } + + // 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 = script_code + '\n'; + + size_t len = program_with_newline.size(); + const uint8_t* data = reinterpret_cast(program_with_newline.c_str()); + size_t written; + + if (!stream_->write(data, len, written)) + { + URCL_LOG_ERROR("Could not send program to robot"); + return false; + } + URCL_LOG_INFO("Sent secondary program to robot:\n%s", program_with_newline.c_str()); + return waitForRobotErrorFeedback(std::chrono::milliseconds(timeout)); +} + +void PrimaryClient::reconnect() const +{ + pipeline_->stop(); + + if (stream_->getState() == comm::SocketState::Connected) + { + stream_->disconnect(); + } + + producer_->setupProducer(); + pipeline_->run(); +} + +void PrimaryClient::addPrimaryConsumer(std::shared_ptr primary_consumer) +{ + multi_consumer_->addConsumer(primary_consumer); +} + +void PrimaryClient::removePrimaryConsumer(std::shared_ptr primary_consumer) +{ + multi_consumer_->removeConsumer(primary_consumer); +} + +void PrimaryClient::keyMessageCallback(KeyMessage& msg) +{ + std::lock_guard lk(robot_feedback_mutex_); + robot_feedback_type_ = RobotFeedbackType::KeyMessage; + key_message_.reset(new KeyMessage(msg)); + robot_feedback_cv_.notify_one(); +} + +void PrimaryClient::errorMessageCallback(ErrorCodeMessage& msg) +{ + std::lock_guard lk(robot_feedback_mutex_); + error_code_message_.reset(new ErrorCodeMessage(msg)); + robot_feedback_type_ = RobotFeedbackType::ErrorMessage; + robot_feedback_cv_.notify_one(); +} + +void PrimaryClient::runtimeExceptionMessageCallback(RuntimeExceptionMessage& msg) +{ + std::lock_guard lk(robot_feedback_mutex_); + robot_feedback_type_ = RobotFeedbackType::RuntimeException; + robot_feedback_cv_.notify_one(); +} + +bool PrimaryClient::waitForRobotFeedback(const std::chrono::milliseconds timeout) +{ + std::chrono::duration time_left(timeout.count() / 1000.0); + std::chrono::time_point start = std::chrono::steady_clock::now(); + while ((std::chrono::steady_clock::now() - start) < timeout) + { + std::unique_lock lk(robot_feedback_mutex_); + if (robot_feedback_cv_.wait_for(lk, time_left) == std::cv_status::no_timeout) + { + switch (robot_feedback_type_) + { + case RobotFeedbackType::RuntimeException: + URCL_LOG_ERROR("Above runtime exception message was received from the robot, after executing script."); + return false; + + case RobotFeedbackType::ErrorMessage: + // Robot is not in remote control + if (error_code_message_->message_code_ == 210) + { + URCL_LOG_ERROR("The robot is in local control and it is therefore not possible to send script commands to " + "the robot.\nIf the robot is in remote control, you have connected to the robot while " + "it was in local control. In this case it is necessary to reconnect to the primary " + "interface, for that you can use the function reconnect()."); + } + else + { + URCL_LOG_ERROR("Above error code message was received from the robot, after executing script."); + } + return false; + + case RobotFeedbackType::KeyMessage: + if (key_message_->title_ == "PROGRAM_XXX_STARTED") + { + return true; + } + break; + default: + // This shouldn't happen, but in case it does we just continue the loop + break; + } + } + time_left = timeout - (std::chrono::steady_clock::now() - start); + } + URCL_LOG_ERROR("Timed out waiting for feedback from the robot, are you still connected to the robot?"); + return false; +} + +bool PrimaryClient::waitForRobotErrorFeedback(const std::chrono::milliseconds timeout) +{ + std::chrono::duration time_left(timeout.count() / 1000.0); + std::chrono::time_point start = std::chrono::steady_clock::now(); + while ((std::chrono::steady_clock::now() - start) < timeout) + { + std::unique_lock lk(robot_feedback_mutex_); + if (robot_feedback_cv_.wait_for(lk, time_left) == std::cv_status::no_timeout) + { + switch (robot_feedback_type_) + { + case RobotFeedbackType::RuntimeException: + URCL_LOG_ERROR("Above runtime exception message was received from the robot, after executing script."); + return false; + + case RobotFeedbackType::ErrorMessage: + // Robot is not in remote control + if (error_code_message_->message_code_ == 210) + { + URCL_LOG_ERROR("The robot is in local control and it is therefore not possible to send script commands to " + "the robot.\nIf the robot is in remote control, you have connected to the robot while " + "it was in local control. In this case it is necessary to reconnect to the primary " + "interface, for that you can use the function reconnect()."); + } + else + { + URCL_LOG_ERROR("Above error code message was received from the robot, after executing script."); + } + return false; + + case RobotFeedbackType::KeyMessage: + break; + default: + // This shouldn't happen, but in case it does we just continue the loop + break; + } + } + time_left = timeout - (std::chrono::steady_clock::now() - start); + } + return true; +} + +} // namespace primary_interface +} // namespace urcl \ No newline at end of file diff --git a/src/primary/program_state_message.cpp b/src/primary/program_state_message.cpp new file mode 100644 index 000000000..93939d580 --- /dev/null +++ b/src/primary/program_state_message.cpp @@ -0,0 +1,57 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2020 FZI Forschungszentrum Informatik (ur_robot_driver) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// -- END LICENSE BLOCK ------------------------------------------------ + +//---------------------------------------------------------------------- +/*!\file + * + * \author Felix Exner exner@fzi.de + * \date 2022-02-21 + * + */ +//---------------------------------------------------------------------- +#include + +#include "ur_client_library/primary/program_state_message.h" +#include "ur_client_library/primary/abstract_primary_consumer.h" + +namespace urcl +{ +namespace primary_interface +{ +bool ProgramStateMessage::parseWith(comm::BinParser& bp) +{ + return true; +} + +bool ProgramStateMessage::consumeWith(AbstractPrimaryConsumer& consumer) +{ + return consumer.consume(*this); +} + +// TODO have a look at the toString method +std::string ProgramStateMessage::toString() const +{ + std::stringstream ss; + ss << "timestamp: " << timestamp_ << std::endl; + ss << "Type: " << static_cast(state_type_) << std::endl; + ss << PrimaryPackage::toString(); + return ss.str(); +} + +} // namespace primary_interface +} // namespace urcl diff --git a/src/primary/robot_message/error_code_message.cpp b/src/primary/robot_message/error_code_message.cpp new file mode 100644 index 000000000..e34ebd5ee --- /dev/null +++ b/src/primary/robot_message/error_code_message.cpp @@ -0,0 +1,80 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2019 FZI Forschungszentrum Informatik (ur_robot_driver) +// Copyright 2017, 2018 Simon Rasmussen (refactor) +// +// Copyright 2015, 2016 Thomas Timm Andersen (original version) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// -- END LICENSE BLOCK ------------------------------------------------ + +//---------------------------------------------------------------------- +/*!\file + * + * \author Felix Exner exner@fzi.de + * \date 2020-04-30 + * + */ +//---------------------------------------------------------------------- + +#include "ur_client_library/primary/robot_message/error_code_message.h" +#include "ur_client_library/primary/abstract_primary_consumer.h" + +namespace urcl +{ +namespace primary_interface +{ +ErrorCodeMessage::ErrorCodeMessage(const ErrorCodeMessage& pkg) + : RobotMessage(pkg.timestamp_, pkg.source_, RobotMessagePackageType::ROBOT_MESSAGE_ERROR_CODE) +{ + message_code_ = pkg.message_code_; + message_argument_ = pkg.message_argument_; + report_level_ = pkg.report_level_; + data_type_ = pkg.data_type_; + data_ = pkg.data_; + text_ = pkg.text_; +} + +bool ErrorCodeMessage::parseWith(comm::BinParser& bp) +{ + bp.parse(message_code_); + bp.parse(message_argument_); + int32_t report_level; + bp.parse(report_level); + report_level_ = static_cast(report_level); + bp.parse(data_type_); + bp.parse(data_); + bp.parseRemainder(text_); + + return true; // not really possible to check dynamic size packets +} + +bool ErrorCodeMessage::consumeWith(AbstractPrimaryConsumer& consumer) +{ + return consumer.consume(*this); +} + +std::string ErrorCodeMessage::toString() const +{ + std::stringstream ss; + ss << "Message code: " << message_code_ << std::endl; + ss << "Message argument: " << message_argument_ << std::endl; + ss << "Report level: " << static_cast(report_level_) << std::endl; + ss << "Datatype: " << static_cast(data_type_) << std::endl; + ss << "Data: " << data_ << std::endl; + ss << "Text: " << text_; + return ss.str(); +} +} // namespace primary_interface +} // namespace urcl diff --git a/src/primary/robot_message/key_message.cpp b/src/primary/robot_message/key_message.cpp new file mode 100644 index 000000000..f3e1823b7 --- /dev/null +++ b/src/primary/robot_message/key_message.cpp @@ -0,0 +1,73 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2019 FZI Forschungszentrum Informatik (ur_robot_driver) +// Copyright 2017, 2018 Simon Rasmussen (refactor) +// +// Copyright 2015, 2016 Thomas Timm Andersen (original version) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// -- END LICENSE BLOCK ------------------------------------------------ + +//---------------------------------------------------------------------- +/*!\file + * + * \author Felix Exner exner@fzi.de + * \date 2019-04-08 + * + */ +//---------------------------------------------------------------------- + +#include "ur_client_library/primary/robot_message/key_message.h" +#include "ur_client_library/primary/abstract_primary_consumer.h" + +namespace urcl +{ +namespace primary_interface +{ +KeyMessage::KeyMessage(const KeyMessage& pkg) + : RobotMessage(pkg.timestamp_, pkg.source_, RobotMessagePackageType::ROBOT_MESSAGE_KEY) +{ + message_code_ = pkg.message_code_; + message_argument_ = pkg.message_argument_; + title_size_ = pkg.title_size_; + title_ = pkg.title_; + text_ = pkg.text_; +} + +bool KeyMessage::parseWith(comm::BinParser& bp) +{ + bp.parse(message_code_); + bp.parse(message_argument_); + bp.parse(title_size_); + bp.parse(title_, title_size_); + bp.parseRemainder(text_); + + return true; // not really possible to check dynamic size packets +} + +bool KeyMessage::consumeWith(AbstractPrimaryConsumer& consumer) +{ + return consumer.consume(*this); +} + +std::string KeyMessage::toString() const +{ + std::stringstream ss; + ss << "Message code: " << message_code_ << std::endl; + ss << "Title: " << title_ << std::endl; + ss << "Text: " << text_; + return ss.str(); +} +} // namespace primary_interface +} // namespace urcl diff --git a/src/primary/robot_message/runtime_exception_message.cpp b/src/primary/robot_message/runtime_exception_message.cpp new file mode 100644 index 000000000..ac10e4174 --- /dev/null +++ b/src/primary/robot_message/runtime_exception_message.cpp @@ -0,0 +1,69 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2019 FZI Forschungszentrum Informatik (ur_robot_driver) +// Copyright 2017, 2018 Simon Rasmussen (refactor) +// +// Copyright 2015, 2016 Thomas Timm Andersen (original version) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// -- END LICENSE BLOCK ------------------------------------------------ + +//---------------------------------------------------------------------- +/*!\file + * + * \author Felix Exner exner@fzi.de + * \date 2020-04-30 + * + */ +//---------------------------------------------------------------------- + +#include "ur_client_library/primary/robot_message/runtime_exception_message.h" +#include "ur_client_library/primary/abstract_primary_consumer.h" + +namespace urcl +{ +namespace primary_interface +{ +RuntimeExceptionMessage::RuntimeExceptionMessage(const RuntimeExceptionMessage& pkg) + : RobotMessage(pkg.timestamp_, pkg.source_, RobotMessagePackageType::ROBOT_MESSAGE_RUNTIME_EXCEPTION) +{ + line_number_ = pkg.line_number_; + column_number_ = pkg.column_number_; + text_ = pkg.text_; +} + +bool RuntimeExceptionMessage::parseWith(comm::BinParser& bp) +{ + bp.parse(line_number_); + bp.parse(column_number_); + bp.parseRemainder(text_); + + return true; // not really possible to check dynamic size packets +} + +bool RuntimeExceptionMessage::consumeWith(AbstractPrimaryConsumer& consumer) +{ + return consumer.consume(*this); +} + +std::string RuntimeExceptionMessage::toString() const +{ + std::stringstream ss; + ss << "Runtime error in line " << line_number_; + ss << ", column " << column_number_ << std::endl; + ss << "Error: " << text_; + return ss.str(); +} +} // namespace primary_interface +} // namespace urcl diff --git a/src/primary/robot_message/text_message.cpp b/src/primary/robot_message/text_message.cpp new file mode 100644 index 000000000..148fee37c --- /dev/null +++ b/src/primary/robot_message/text_message.cpp @@ -0,0 +1,55 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2019 FZI Forschungszentrum Informatik (ur_robot_driver) +// Copyright 2017, 2018 Simon Rasmussen (refactor) +// +// Copyright 2015, 2016 Thomas Timm Andersen (original version) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// -- END LICENSE BLOCK ------------------------------------------------ + +//---------------------------------------------------------------------- +/*!\file + * + * \author Felix Exner exner@fzi.de + * \date 2019-04-08 + * + */ +//---------------------------------------------------------------------- + +#include "ur_client_library/primary/robot_message/text_message.h" +#include "ur_client_library/primary/abstract_primary_consumer.h" + +namespace urcl +{ +namespace primary_interface +{ +bool TextMessage::parseWith(comm::BinParser& bp) +{ + bp.parseRemainder(text_); + + return true; // not really possible to check dynamic size packets +} + +bool TextMessage::consumeWith(AbstractPrimaryConsumer& consumer) +{ + return consumer.consume(*this); +} + +std::string TextMessage::toString() const +{ + return text_; +} +} // namespace primary_interface +} // namespace urcl diff --git a/src/primary/robot_state/robot_mode_data.cpp b/src/primary/robot_state/robot_mode_data.cpp new file mode 100644 index 000000000..060a71b01 --- /dev/null +++ b/src/primary/robot_state/robot_mode_data.cpp @@ -0,0 +1,110 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#include "ur_client_library/primary/robot_state/robot_mode_data.h" +#include "ur_client_library/primary/abstract_primary_consumer.h" + +#include + +namespace urcl +{ +namespace primary_interface +{ +RobotModeData::RobotModeData(const RobotModeData& pkg) : RobotState(RobotStateType::ROBOT_MODE_DATA) +{ + timestamp_ = pkg.timestamp_; + is_real_robot_connected_ = pkg.is_real_robot_connected_; + is_real_robot_enabled_ = pkg.is_real_robot_enabled_; + is_robot_power_on_ = pkg.is_robot_power_on_; + is_emergency_stopped_ = pkg.is_emergency_stopped_; + is_protective_stopped_ = pkg.is_protective_stopped_; + is_program_running_ = pkg.is_program_running_; + is_program_paused_ = pkg.is_program_paused_; + robot_mode_ = pkg.robot_mode_; + control_mode_ = pkg.control_mode_; + target_speed_fraction_ = pkg.target_speed_fraction_; + speed_scaling_ = pkg.speed_scaling_; + target_speed_fraction_limit_ = pkg.target_speed_fraction_limit_; + reserved_ = pkg.reserved_; +} + +bool RobotModeData::parseWith(comm::BinParser& bp) +{ + bp.parse(timestamp_); + bp.parse(is_real_robot_connected_); + bp.parse(is_real_robot_enabled_); + bp.parse(is_robot_power_on_); + bp.parse(is_emergency_stopped_); + bp.parse(is_protective_stopped_); + bp.parse(is_program_running_); + bp.parse(is_program_paused_); + bp.parse(robot_mode_); + bp.parse(control_mode_); + bp.parse(target_speed_fraction_); + bp.parse(speed_scaling_); + bp.parse(target_speed_fraction_limit_); + bp.parseRemainder(reserved_); + + return true; +} + +bool RobotModeData::consumeWith(AbstractPrimaryConsumer& consumer) +{ + return consumer.consume(*this); +} + +std::string RobotModeData::toString() const +{ + std::stringstream os; + os << "Timestamp: " << timestamp_ << std::endl; + os << "Is real robot connected: " << is_real_robot_connected_ << std::endl; + os << "Is real robot enabled: " << is_real_robot_enabled_ << std::endl; + os << "Is robot power on: " << is_robot_power_on_ << std::endl; + os << "Is emergency stopped: " << is_emergency_stopped_ << std::endl; + os << "Is protective stopped: " << is_protective_stopped_ << std::endl; + os << "Is program running: " << is_program_running_ << std::endl; + os << "Is program paused: " << is_program_paused_ << std::endl; + os << "Robot mode: " << unsigned(robot_mode_) << std::endl; + os << "Control mode: " << unsigned(control_mode_) << std::endl; + os << "Target speed fraction: " << target_speed_fraction_ << std::endl; + os << "Speed scaling: " << speed_scaling_ << std::endl; + os << "Target speed fraction limit: " << target_speed_fraction_limit_ << std::endl; + os << "Reserved: ( " << reserved_.length() << ")"; + for (const char& c : reserved_) + { + os << std::hex << static_cast(c) << ", "; + } + os << std::endl; + + return os.str(); +} + +} // namespace primary_interface +} // namespace urcl \ No newline at end of file diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index 9292681b5..4b1aafc3e 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -37,8 +37,6 @@ #include #include -#include - namespace urcl { static const std::string BEGIN_REPLACE("{{BEGIN_REPLACE}}"); @@ -69,11 +67,7 @@ urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_ URCL_LOG_DEBUG("Initializing RTDE client"); rtde_client_.reset(new rtde_interface::RTDEClient(robot_ip_, notifier_, output_recipe_file, input_recipe_file)); - primary_stream_.reset( - new comm::URStream(robot_ip_, urcl::primary_interface::UR_PRIMARY_PORT)); - secondary_stream_.reset( - new comm::URStream(robot_ip_, urcl::primary_interface::UR_SECONDARY_PORT)); - secondary_stream_->connect(); + primary_client_.reset(new primary_interface::PrimaryClient(robot_ip_)); non_blocking_read_ = non_blocking_read; get_packet_timeout_ = non_blocking_read_ ? 0 : 100; @@ -323,7 +317,7 @@ bool UrDriver::zeroFTSensor() "only work, if the robot is in remote_control mode."); std::stringstream cmd; cmd << "sec tareSetup():" << std::endl << " zero_ftsensor()" << std::endl << "end"; - return sendScript(cmd.str()); + return sendSecondaryScript(cmd.str()); } } } @@ -343,7 +337,7 @@ bool UrDriver::setPayload(const float mass, const vector3d_t& cog) cmd << "sec setup():" << std::endl << " set_payload(" << mass << ", [" << cog[0] << ", " << cog[1] << ", " << cog[2] << "])" << std::endl << "end"; - return sendScript(cmd.str()); + return sendSecondaryScript(cmd.str()); } } @@ -375,7 +369,7 @@ bool UrDriver::setToolVoltage(const ToolVoltage voltage) "robots this will only work, if the robot is in remote_control mode."); std::stringstream cmd; cmd << "sec setup():" << std::endl << " set_tool_voltage(" << toUnderlying(voltage) << ")" << std::endl << "end"; - return sendScript(cmd.str()); + return sendSecondaryScript(cmd.str()); } } @@ -510,27 +504,11 @@ std::string UrDriver::readScriptFile(const std::string& filename) bool UrDriver::checkCalibration(const std::string& checksum) { - if (primary_stream_ == nullptr) + if (primary_client_ == nullptr) { throw std::runtime_error("checkCalibration() called without a primary interface connection being established."); } - primary_interface::PrimaryParser parser; - comm::URProducer prod(*primary_stream_, parser); - prod.setupProducer(); - - CalibrationChecker consumer(checksum); - - comm::INotifier notifier; - - comm::Pipeline pipeline(prod, &consumer, "Pipeline", notifier); - pipeline.run(); - - while (!consumer.isChecked()) - { - std::this_thread::sleep_for(std::chrono::seconds(1)); - } - URCL_LOG_DEBUG("Got calibration information from robot."); - return consumer.checkSuccessful(); + return primary_client_->checkCalibration(checksum); } rtde_interface::RTDEWriter& UrDriver::getRTDEWriter() @@ -538,31 +516,28 @@ rtde_interface::RTDEWriter& UrDriver::getRTDEWriter() return rtde_client_->getWriter(); } -bool UrDriver::sendScript(const std::string& program) +bool UrDriver::sendScript(const std::string& script_code, const std::chrono::milliseconds timeout) const { - if (secondary_stream_ == nullptr) + if (primary_client_ == nullptr) { throw std::runtime_error("Sending script to robot requested while there is no primary interface established. " "This " "should not happen."); } - // 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'; - - size_t len = program_with_newline.size(); - const uint8_t* data = reinterpret_cast(program_with_newline.c_str()); - size_t written; + return primary_client_->sendScript(script_code, timeout); +} - if (secondary_stream_->write(data, len, written)) +bool UrDriver::sendSecondaryScript(const std::string& script_code, const std::chrono::milliseconds timeout) const +{ + if (primary_client_ == nullptr) { - URCL_LOG_DEBUG("Sent program to robot:\n%s", program_with_newline.c_str()); - return true; + throw std::runtime_error("Sending script to robot requested while there is no primary interface established. " + "This " + "should not happen."); } - URCL_LOG_ERROR("Could not send program to robot"); - return false; + + return primary_client_->sendSecondaryScript(script_code, timeout); } bool UrDriver::sendRobotProgram()