diff --git a/README.md b/README.md index 24e6f1695..2994c31c0 100644 --- a/README.md +++ b/README.md @@ -87,8 +87,8 @@ See [Architecture documentation](doc/architecture.rst) Currently, this library doesn't support the primary interface very well, as the [Universal Robots ROS driver](https://github.com/UniversalRobots/Universal_Robots_ROS_Driver) was built mainly upon -the RTDE interface. Therefore, there is also no `PrimaryClient` for directly accessing the primary -interface. This may change in future, though. +the RTDE interface. The `PrimaryClient` for directly accessing the primary +interface doesn't support all features of the primary interface. The `comm::URStream` class can be used to open a connection to the primary / secondary interface and send data to it. The [producer/consumer](#producer--consumer-architecture) pipeline structure diff --git a/doc/architecture/ur_driver.rst b/doc/architecture/ur_driver.rst index 1286eedb4..76230b51c 100644 --- a/doc/architecture/ur_driver.rst +++ b/doc/architecture/ur_driver.rst @@ -14,7 +14,7 @@ When creating an instance of the ``UrDriver`` class, the created object will con - a :ref:`ScriptCommandInterface ` - a :ref:`ScriptSender ` - a :ref:`TrajectoryPointInterface `. -- a rudimentary client to the robot's secondary interface +- a rudimentary client to the robot's primary interface - a couple of helper functions As this page is not meant to be a full-blown API documentation, not every public method will be @@ -42,7 +42,7 @@ given to this function. Connection to the primary interface is dropped afterward ``sendScript(const std::string& program)`` ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -This function sends given URScript code directly to the secondary interface. The +This function sends given URScript code directly to the primary interface. The ``sendRobotProgram()`` function is a special case that will send the script code given in the ``RTDEClient`` constructor. diff --git a/doc/setup/robot_setup.rst b/doc/setup/robot_setup.rst index 95462e295..76709b30c 100644 --- a/doc/setup/robot_setup.rst +++ b/doc/setup/robot_setup.rst @@ -37,7 +37,7 @@ The robot needs to be prepared before it can be used with the client library. #. Go to the hamburger menu -> settings. #. Go to Security -> Services. #. Unlock the menu using the admin password. - #. Enable the Dashboard Server, Primary Client Interface, Secondary Client Interface and Real-Time Data Exchange (RTDE) interfaces. + #. Enable the Dashboard Server, Primary Client Interface and Real-Time Data Exchange (RTDE) interfaces. #. Lock the menu and press exit. .. image:: ../images/initial_setup_images/services_polyscope5.png @@ -52,7 +52,7 @@ The robot needs to be prepared before it can be used with the client library. #. Go to the hamburger menu -> settings. #. Go to Security -> Services. #. Unlock the menu using the admin password. - #. Enable the Primary Client Interface, Secondary Client Interface and Real-Time Data Exchange (RTDE) interfaces. + #. Enable the Primary Client Interface and Real-Time Data Exchange (RTDE) interfaces. #. Lock the menu and press exit. .. image:: ../images/initial_setup_images/services_polyscopex.png diff --git a/include/ur_client_library/primary/primary_client.h b/include/ur_client_library/primary/primary_client.h index d8a1bf153..5b7c8f5d8 100644 --- a/include/ur_client_library/primary/primary_client.h +++ b/include/ur_client_library/primary/primary_client.h @@ -68,6 +68,7 @@ class PrimaryClient void removePrimaryConsumer(std::shared_ptr> primary_consumer); void start(const size_t max_connection_attempts = 0, const std::chrono::milliseconds reconnection_timeout = urcl::comm::TCPSocket::DEFAULT_RECONNECTION_TIME); + void stop(); /*! * \brief Retrieves previously raised error codes from PrimaryClient. After calling this, recorded errors will be @@ -75,7 +76,29 @@ class PrimaryClient */ std::deque getErrorCodes(); + /*! + * \brief Sends a custom script program to the robot. + * + * The given code must be valid according the UR Scripting Manual. + * + * \param program URScript code that shall be executed by the robot. + * + * \returns true on successful upload, false otherwise. + */ + bool sendScript(const std::string& program); + + bool checkCalibration(const std::string& checksum); + private: + /*! + * \brief Reconnects the primary stream used to send program to the robot. + * + * Only for use in headless mode, as it replaces the use of the URCaps program. + * + * \returns true of on successful reconnection, false otherwise + */ + bool reconnectStream(); + // The function is called whenever an error code message is received void errorMessageCallback(ErrorCode& code); diff --git a/include/ur_client_library/primary/primary_consumer.h b/include/ur_client_library/primary/primary_consumer.h index cdb84057a..3880c2fc0 100644 --- a/include/ur_client_library/primary/primary_consumer.h +++ b/include/ur_client_library/primary/primary_consumer.h @@ -97,6 +97,8 @@ class PrimaryConsumer : public AbstractPrimaryConsumer */ virtual bool consume(KinematicsInfo& pkg) override { + URCL_LOG_DEBUG("%s", pkg.toString().c_str()); + kinematics_info_ = std::make_shared(pkg); return true; } @@ -162,8 +164,19 @@ class PrimaryConsumer : public AbstractPrimaryConsumer error_code_message_callback_ = callback_function; } + /*! + * \brief Get the kinematics info + * + * \returns Shared pointer to the kinematics info + */ + std::shared_ptr getKinematicsInfo() + { + return kinematics_info_; + } + private: std::function error_code_message_callback_; + std::shared_ptr kinematics_info_; }; } // namespace primary_interface diff --git a/include/ur_client_library/primary/robot_state/kinematics_info.h b/include/ur_client_library/primary/robot_state/kinematics_info.h index 4549d4a09..298be0d4b 100644 --- a/include/ur_client_library/primary/robot_state/kinematics_info.h +++ b/include/ur_client_library/primary/robot_state/kinematics_info.h @@ -43,14 +43,26 @@ namespace primary_interface class KinematicsInfo : public RobotState { public: - KinematicsInfo() = delete; + KinematicsInfo() : RobotState(RobotStateType::KINEMATICS_INFO) + { + } /*! * \brief Creates a new KinematicsInfo object. * * \param type The type of RobotState message received */ - KinematicsInfo(const RobotStateType type) : RobotState(type) + explicit KinematicsInfo(const RobotStateType type) : RobotState(type) + { + } + + KinematicsInfo(const KinematicsInfo& other) : RobotState(RobotStateType::KINEMATICS_INFO) { + checksum_ = other.checksum_; + dh_theta_ = other.dh_theta_; + dh_a_ = other.dh_a_; + dh_d_ = other.dh_d_; + dh_alpha_ = other.dh_alpha_; + calibration_status_ = other.calibration_status_; } virtual ~KinematicsInfo() = default; diff --git a/include/ur_client_library/ur/calibration_checker.h b/include/ur_client_library/ur/calibration_checker.h index d68efed5b..aeb7149ee 100644 --- a/include/ur_client_library/ur/calibration_checker.h +++ b/include/ur_client_library/ur/calibration_checker.h @@ -39,7 +39,9 @@ namespace urcl * packages. These are then checked against the used kinematics to see if the correct calibration * is used. */ -class CalibrationChecker : public comm::IConsumer +class [[deprecated("Using the standalone CalibrationChecker consumer to compare the kinematics hash is deprecated. " + "Please use the PrimaryClient instead.")]] CalibrationChecker + : public comm::IConsumer { public: /*! diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index f25f5d614..3d06561fa 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -868,6 +868,11 @@ class UrDriver */ void startPrimaryClientCommunication(); + void stopPrimaryClientCommunication() + { + primary_client_->stop(); + } + void registerTrajectoryInterfaceDisconnectedCallback(std::function fun) { trajectory_interface_->registerDisconnectionCallback(fun); @@ -875,11 +880,6 @@ class UrDriver static std::string readScriptFile(const std::string& filename); - void closeSecondaryStream() - { - secondary_stream_->close(); - } - bool isReverseInterfaceConnected() const { return reverse_interface_->isConnected(); @@ -893,15 +893,6 @@ class UrDriver private: void init(const UrDriverConfiguration& config); - /*! - * \brief Reconnects the secondary stream used to send program to the robot. - * - * Only for use in headless mode, as it replaces the use of the URCaps program. - * - * \returns true of on successful reconnection, false otherwise - */ - bool reconnectSecondaryStream(); - void initRTDE(); void setupReverseInterface(const uint32_t reverse_port); @@ -912,8 +903,6 @@ class UrDriver std::unique_ptr trajectory_interface_; std::unique_ptr script_command_interface_; std::unique_ptr script_sender_; - std::unique_ptr> primary_stream_; - std::unique_ptr> secondary_stream_; size_t socket_connection_attempts_ = 0; std::chrono::milliseconds socket_reconnection_timeout_ = std::chrono::milliseconds(10000); diff --git a/src/primary/primary_client.cpp b/src/primary/primary_client.cpp index 54b4872e2..18dbb9f5d 100644 --- a/src/primary/primary_client.cpp +++ b/src/primary/primary_client.cpp @@ -67,6 +67,12 @@ void PrimaryClient::start(const size_t max_num_tries, const std::chrono::millise pipeline_->run(); } +void PrimaryClient::stop() +{ + pipeline_->stop(); + stream_.close(); +} + void PrimaryClient::addPrimaryConsumer(std::shared_ptr> primary_consumer) { multi_consumer_->addConsumer(primary_consumer); @@ -91,5 +97,68 @@ std::deque PrimaryClient::getErrorCodes() error_code_queue_.clear(); return error_codes; } + +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'; + + size_t len = program_with_newline.size(); + const uint8_t* data = reinterpret_cast(program_with_newline.c_str()); + size_t written; + + const auto send_script_contents = [this, program_with_newline, data, len, + &written](const std::string&& description) -> bool { + if (stream_.write(data, len, written)) + { + URCL_LOG_DEBUG("Sent program to robot:\n%s", program_with_newline.c_str()); + return true; + } + const std::string error_message = "Could not send program to robot: " + description; + URCL_LOG_ERROR(error_message.c_str()); + return false; + }; + + if (send_script_contents("initial attempt")) + { + return true; + } + + if (reconnectStream()) + { + return send_script_contents("after reconnecting primary stream"); + } + + return false; +} + +bool PrimaryClient::reconnectStream() +{ + URCL_LOG_DEBUG("Closing primary stream..."); + stream_.close(); + if (stream_.connect()) + { + URCL_LOG_DEBUG("Primary stream connected"); + return true; + } + URCL_LOG_ERROR("Failed to reconnect primary stream!"); + return false; +} + +bool PrimaryClient::checkCalibration(const std::string& checksum) +{ + std::shared_ptr kin_info = consumer_->getKinematicsInfo(); + while (kin_info == nullptr) + { + std::this_thread::sleep_for(std::chrono::seconds(1)); + kin_info = consumer_->getKinematicsInfo(); + } + URCL_LOG_DEBUG("Got calibration information from robot."); + + return kin_info->toHash() == checksum; +} + } // namespace primary_interface } // namespace urcl diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index 224fca68e..ff9841b3d 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -70,12 +70,6 @@ void UrDriver::init(const UrDriverConfiguration& config) rtde_client_.reset( new rtde_interface::RTDEClient(robot_ip_, notifier_, config.output_recipe_file, config.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(socket_connection_attempts_, socket_reconnection_timeout_); - primary_client_.reset(new urcl::primary_interface::PrimaryClient(robot_ip_, notifier_)); get_packet_timeout_ = non_blocking_read_ ? 0 : 100; @@ -151,6 +145,7 @@ void UrDriver::init(const UrDriverConfiguration& config) trajectory_interface_.reset(new control::TrajectoryPointInterface(config.trajectory_port)); script_command_interface_.reset(new control::ScriptCommandInterface(config.script_command_port)); + startPrimaryClientCommunication(); if (in_headless_mode_) { full_robot_program_ = "stop program\n"; @@ -561,27 +556,7 @@ std::string UrDriver::readScriptFile(const std::string& filename) bool UrDriver::checkCalibration(const std::string& checksum) { - if (primary_stream_ == 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() @@ -591,44 +566,12 @@ rtde_interface::RTDEWriter& UrDriver::getRTDEWriter() bool UrDriver::sendScript(const std::string& program) { - if (secondary_stream_ == nullptr) + if (primary_client_ == nullptr) { - throw std::runtime_error("Sending script to robot requested while there is no secondary interface established. " + throw std::runtime_error("Sending script to robot requested while there is no primary client initialized. " "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; - - const auto send_script_contents = [this, program_with_newline, data, len, - &written](const std::string&& description) -> bool { - if (secondary_stream_->write(data, len, written)) - { - URCL_LOG_DEBUG("Sent program to robot:\n%s", program_with_newline.c_str()); - return true; - } - const std::string error_message = "Could not send program to robot: " + description; - URCL_LOG_ERROR(error_message.c_str()); - return false; - }; - - if (send_script_contents("initial attempt")) - { - return true; - } - - if (reconnectSecondaryStream()) - { - return send_script_contents("after reconnecting secondary stream"); - } - - return false; + return primary_client_->sendScript(program); } bool UrDriver::sendRobotProgram() @@ -644,19 +587,6 @@ bool UrDriver::sendRobotProgram() } } -bool UrDriver::reconnectSecondaryStream() -{ - URCL_LOG_DEBUG("Closing secondary stream..."); - secondary_stream_->close(); - if (secondary_stream_->connect()) - { - URCL_LOG_DEBUG("Secondary stream connected"); - return true; - } - URCL_LOG_ERROR("Failed to reconnect secondary stream!"); - return false; -} - std::vector UrDriver::getRTDEOutputRecipe() { return rtde_client_->getOutputRecipe(); diff --git a/tests/test_primary_client.cpp b/tests/test_primary_client.cpp index 45d6fcce0..628f723fc 100644 --- a/tests/test_primary_client.cpp +++ b/tests/test_primary_client.cpp @@ -57,7 +57,10 @@ TEST_F(PrimaryClientTest, start_communication_succeeds) TEST_F(PrimaryClientTest, add_and_remove_consumer) { +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" auto calibration_consumer = std::make_shared("test"); +#pragma GCC diagnostic pop client_->addPrimaryConsumer(calibration_consumer); diff --git a/tests/test_ur_driver.cpp b/tests/test_ur_driver.cpp index aaf6ec964..2ff7e8b2a 100644 --- a/tests/test_ur_driver.cpp +++ b/tests/test_ur_driver.cpp @@ -235,14 +235,14 @@ TEST_F(UrDriverTest, target_outside_limits_pose) TEST_F(UrDriverTest, send_robot_program_retry_on_failure) { - // Check that sendRobotProgram is robust to the secondary stream being disconnected. This is what happens when + // Check that sendRobotProgram is robust to the primary stream being disconnected. This is what happens when // switching from Remote to Local and back to Remote mode for example. // To be able to re-send the robot program we'll have to make sure it isn't running g_my_robot->ur_driver_->stopControl(); g_my_robot->waitForProgramNotRunning(); - g_my_robot->ur_driver_->closeSecondaryStream(); + g_my_robot->ur_driver_->stopPrimaryClientCommunication(); EXPECT_TRUE(g_my_robot->resendRobotProgram()); @@ -259,7 +259,6 @@ TEST_F(UrDriverTest, reset_rtde_client) TEST_F(UrDriverTest, read_error_code) { - g_my_robot->ur_driver_->startPrimaryClientCommunication(); // Wait until we actually received a package std::this_thread::sleep_for(std::chrono::milliseconds(1000)); std::stringstream cmd;