diff --git a/CMakeLists.txt b/CMakeLists.txt index dd720dc58..236817477 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -28,6 +28,7 @@ add_library(urcl src/primary/robot_message/version_message.cpp src/primary/robot_message/error_code_message.cpp src/primary/robot_state/kinematics_info.cpp + src/primary/robot_state/robot_mode_data.cpp src/rtde/control_package_pause.cpp src/rtde/control_package_setup_inputs.cpp src/rtde/control_package_setup_outputs.cpp diff --git a/include/ur_client_library/exceptions.h b/include/ur_client_library/exceptions.h index 125e41178..91a6a38c4 100644 --- a/include/ur_client_library/exceptions.h +++ b/include/ur_client_library/exceptions.h @@ -120,6 +120,13 @@ class TimeoutException : public UrException { public: explicit TimeoutException() = delete; + explicit TimeoutException(const std::string& text, std::chrono::milliseconds timeout) : std::runtime_error(text) + { + std::stringstream ss; + ss << text << "(Configured timeout: " << timeout.count() / 1000.0 << " sec)"; + text_ = ss.str(); + } + explicit TimeoutException(const std::string& text, timeval timeout) : std::runtime_error(text) { std::stringstream ss; diff --git a/include/ur_client_library/helpers.h b/include/ur_client_library/helpers.h index e17440237..08b6d0b47 100644 --- a/include/ur_client_library/helpers.h +++ b/include/ur_client_library/helpers.h @@ -29,6 +29,8 @@ #ifndef UR_CLIENT_LIBRARY_HELPERS_H_INCLUDED #define UR_CLIENT_LIBRARY_HELPERS_H_INCLUDED +#include +#include #ifdef _WIN32 # define NOMINMAX @@ -63,5 +65,18 @@ static inline int sched_get_priority_max(int policy) namespace urcl { bool setFiFoScheduling(pthread_t& thread, const int priority); -} + +/*! + * \brief Wait for a condition to be true. + * + * This function will wait for a condition to be true. The condition is checked in intervals of \p check_interval. + * If the condition is not met after \p timeout, the function will throw a urcl::TimeoutException. + * + * \param condition The condition to be checked. + * \param timeout The maximum time to wait for the condition to be true. + * \param check_interval The interval in which the condition is checked. + */ +void waitFor(std::function condition, const std::chrono::milliseconds timeout, + const std::chrono::milliseconds check_interval = std::chrono::milliseconds(50)); +} // namespace urcl #endif // ifndef UR_CLIENT_LIBRARY_HELPERS_H_INCLUDED diff --git a/include/ur_client_library/primary/abstract_primary_consumer.h b/include/ur_client_library/primary/abstract_primary_consumer.h index 3ed464e4e..32a692fb2 100644 --- a/include/ur_client_library/primary/abstract_primary_consumer.h +++ b/include/ur_client_library/primary/abstract_primary_consumer.h @@ -34,6 +34,7 @@ #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_state/kinematics_info.h" +#include "ur_client_library/primary/robot_state/robot_mode_data.h" namespace urcl { @@ -75,6 +76,7 @@ class AbstractPrimaryConsumer : public comm::IConsumer virtual bool consume(VersionMessage& pkg) = 0; virtual bool consume(KinematicsInfo& pkg) = 0; virtual bool consume(ErrorCodeMessage& pkg) = 0; + virtual bool consume(RobotModeData& pkg) = 0; private: /* data */ diff --git a/include/ur_client_library/primary/primary_client.h b/include/ur_client_library/primary/primary_client.h index 5b7c8f5d8..37de1d34a 100644 --- a/include/ur_client_library/primary/primary_client.h +++ b/include/ur_client_library/primary/primary_client.h @@ -31,6 +31,7 @@ #ifndef UR_CLIENT_LIBRARY_PRIMARY_CLIENT_H_INCLUDED #define UR_CLIENT_LIBRARY_PRIMARY_CLIENT_H_INCLUDED +#include #include #include @@ -89,6 +90,60 @@ class PrimaryClient bool checkCalibration(const std::string& checksum); + /*! + * \brief Commands the robot to power on. + * + * \param validate If true, the function will block until the robot is powered on or the timeout + * passed by. + * \param timeout The maximum time to wait for the robot to confirm the power on command. + * + * \throws urcl::UrException if the command cannot be sent to the robot. + * \throws urcl::TimeoutException if the robot doesn't power on within the given timeout. + */ + void commandPowerOn(const bool validate = true, const std::chrono::milliseconds timeout = std::chrono::seconds(30)); + + /*! + * \brief Commands the robot to power off. + * + * \param validate If true, the function will block until the robot is powered off or the timeout + * passed by. + * \param timeout The maximum time to wait for the robot to confirm the power off command. + * + * \throws urcl::UrException if the command cannot be sent to the robot. + * \throws urcl::TimeoutException if the robot doesn't power off within the given timeout. + */ + void commandPowerOff(const bool validate = true, const std::chrono::milliseconds timeout = std::chrono::seconds(30)); + + /*! + * \brief Commands the robot to release the brakes + * + * \param validate If true, the function will block until the robot is running or the timeout + * passed by. + * \param timeout The maximum time to wait for the robot to confirm it is running. + * + * \throws urcl::UrException if the command cannot be sent to the robot. + * \throws urcl::TimeoutException if the robot doesn't release the brakes within the given + * timeout. + */ + void commandBrakeRelease(const bool validate = true, + const std::chrono::milliseconds timeout = std::chrono::seconds(30)); + + /*! + * \brief Get the latest robot mode. + * + * The robot mode will be updated in the background. This will always show the latest received + * robot mode independent of the time that has passed since receiving it. + */ + RobotMode getRobotMode() + { + std::shared_ptr robot_mode_data = consumer_->getRobotModeData(); + if (robot_mode_data == nullptr) + { + return RobotMode::UNKNOWN; + } + return static_cast(consumer_->getRobotModeData()->robot_mode_); + } + private: /*! * \brief Reconnects the primary stream used to send program to the robot. diff --git a/include/ur_client_library/primary/primary_consumer.h b/include/ur_client_library/primary/primary_consumer.h index 3880c2fc0..44ccd5f3e 100644 --- a/include/ur_client_library/primary/primary_consumer.h +++ b/include/ur_client_library/primary/primary_consumer.h @@ -29,6 +29,8 @@ #define UR_CLIENT_LIBRARY_PRIMARY_CONSUMER_H_INCLUDED #include "ur_client_library/primary/abstract_primary_consumer.h" +#include "ur_client_library/primary/robot_state/robot_mode_data.h" +#include "ur_client_library/ur/datatypes.h" #include #include @@ -153,6 +155,14 @@ class PrimaryConsumer : public AbstractPrimaryConsumer return true; } + virtual bool consume(RobotModeData& pkg) override + { + URCL_LOG_DEBUG("Robot mode is now %s", robotModeString(static_cast(pkg.robot_mode_)).c_str()); + std::scoped_lock lock(robot_mode_mutex_); + robot_mode_ = std::make_shared(pkg); + return true; + } + /*! * \brief Set callback function which will be triggered whenever error code messages are received * @@ -174,9 +184,23 @@ class PrimaryConsumer : public AbstractPrimaryConsumer return kinematics_info_; } + /*! + * \brief Get the latest robot mode. + * + * The robot mode will be updated in the background. This will always show the latest received + * robot mode independent of the time that has passed since receiving it. + */ + std::shared_ptr getRobotModeData() + { + std::scoped_lock lock(robot_mode_mutex_); + return robot_mode_; + } + private: std::function error_code_message_callback_; std::shared_ptr kinematics_info_; + std::mutex robot_mode_mutex_; + std::shared_ptr robot_mode_; }; } // namespace primary_interface diff --git a/include/ur_client_library/primary/primary_parser.h b/include/ur_client_library/primary/primary_parser.h index 74d423a85..cb73e55cb 100644 --- a/include/ur_client_library/primary/primary_parser.h +++ b/include/ur_client_library/primary/primary_parser.h @@ -29,6 +29,7 @@ #include "ur_client_library/primary/robot_state/kinematics_info.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_state/robot_mode_data.h" namespace urcl { @@ -150,12 +151,12 @@ class PrimaryParser : public comm::Parser { switch (type) { - /*case robot_state_type::ROBOT_MODE_DATA: - // SharedRobotModeData* rmd = new SharedRobotModeData(); + case RobotStateType::ROBOT_MODE_DATA: + return new RobotModeData(type); - //return new rmd; - case robot_state_type::MASTERBOARD_DATA: - return new MBD;*/ + // return new rmd; + // case robot_state_type::MASTERBOARD_DATA: + // return new MBD;*/ case RobotStateType::KINEMATICS_INFO: return new KinematicsInfo(type); default: 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..560b04550 --- /dev/null +++ b/include/ur_client_library/primary/robot_state/robot_mode_data.h @@ -0,0 +1,112 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2025 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 +#include "ur_client_library/types.h" +#include "ur_client_library/primary/robot_state.h" +namespace urcl +{ +namespace primary_interface +{ + +/*! + * \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/src/helpers.cpp b/src/helpers.cpp index 223ca431d..b80f772b4 100644 --- a/src/helpers.cpp +++ b/src/helpers.cpp @@ -26,12 +26,14 @@ */ //---------------------------------------------------------------------- +#include #include #include #include #include #include +#include // clang-format off // We want to keep the URL in one line to avoid formatting issues. This will make it easier to @@ -99,4 +101,20 @@ bool setFiFoScheduling(pthread_t& thread, const int priority) return true; #endif } + +void waitFor(std::function condition, const std::chrono::milliseconds timeout, + const std::chrono::milliseconds check_interval) +{ + auto start_time = std::chrono::system_clock::now(); + while (std::chrono::system_clock::now() - start_time < timeout) + { + if (condition()) + { + return; + } + URCL_LOG_DEBUG("Waiting for condition to be met..."); + std::this_thread::sleep_for(check_interval); + } + throw urcl::TimeoutException("Timeout while waiting for condition to be met", timeout); +} } // namespace urcl diff --git a/src/primary/primary_client.cpp b/src/primary/primary_client.cpp index 18dbb9f5d..7f44598e9 100644 --- a/src/primary/primary_client.cpp +++ b/src/primary/primary_client.cpp @@ -31,7 +31,8 @@ #include #include #include - +#include "ur_client_library/exceptions.h" +#include namespace urcl { namespace primary_interface @@ -160,5 +161,63 @@ bool PrimaryClient::checkCalibration(const std::string& checksum) return kin_info->toHash() == checksum; } +void PrimaryClient::commandPowerOn(const bool validate, const std::chrono::milliseconds timeout) +{ + if (!sendScript("power on")) + { + throw UrException("Failed to send power on command to robot"); + } + + if (validate) + { + try + { + waitFor([this]() { return getRobotMode() == RobotMode::IDLE; }, timeout); + } + catch (const TimeoutException& ex) + { + throw TimeoutException("Robot did not power on within the given timeout", timeout); + } + } +} + +void PrimaryClient::commandPowerOff(const bool validate, const std::chrono::milliseconds timeout) +{ + if (!sendScript("power off")) + { + throw UrException("Failed to send power off command to robot"); + } + if (validate) + { + try + { + waitFor([this]() { return getRobotMode() == RobotMode::POWER_OFF; }, timeout); + } + catch (const std::exception&) + { + throw TimeoutException("Robot did not power off within the given timeout", timeout); + } + } +} + +void PrimaryClient::commandBrakeRelease(const bool validate, const std::chrono::milliseconds timeout) +{ + if (!sendScript("set robotmode run")) + { + throw UrException("Failed to send brake release command to robot"); + } + if (validate) + { + try + { + waitFor([this]() { return getRobotMode() == RobotMode::RUNNING; }, timeout); + } + catch (const std::exception&) + { + throw TimeoutException("Robot did not release the brakes within the given timeout", timeout); + } + } +} + } // 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..a188cbc0e --- /dev/null +++ b/src/primary/robot_state/robot_mode_data.cpp @@ -0,0 +1,110 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2025 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 diff --git a/tests/test_primary_client.cpp b/tests/test_primary_client.cpp index 628f723fc..9f28f2553 100644 --- a/tests/test_primary_client.cpp +++ b/tests/test_primary_client.cpp @@ -32,7 +32,11 @@ #include #include +#include #include +#include +#include "ur_client_library/exceptions.h" +#include "ur_client_library/helpers.h" using namespace urcl; @@ -77,9 +81,52 @@ TEST_F(PrimaryClientTest, add_and_remove_consumer) client_->removePrimaryConsumer(calibration_consumer); } +TEST_F(PrimaryClientTest, test_power_cycle_commands) +{ + EXPECT_NO_THROW(client_->start()); + EXPECT_NO_THROW(client_->commandPowerOff()); + EXPECT_NO_THROW(client_->commandPowerOn()); + EXPECT_NO_THROW(client_->commandBrakeRelease()); + EXPECT_NO_THROW(client_->commandPowerOff()); + EXPECT_NO_THROW(client_->commandBrakeRelease()); + EXPECT_NO_THROW(client_->commandPowerOff()); + + auto timeout = std::chrono::seconds(30); + // provoke a timeout + EXPECT_THROW(client_->commandBrakeRelease(true, std::chrono::milliseconds(1)), urcl::TimeoutException); + EXPECT_NO_THROW(waitFor([this]() { return client_->getRobotMode() == RobotMode::RUNNING; }, timeout)); + EXPECT_THROW(client_->commandPowerOff(true, std::chrono::milliseconds(1)), urcl::TimeoutException); + EXPECT_NO_THROW(waitFor([this]() { return client_->getRobotMode() == RobotMode::POWER_OFF; }, timeout)); + EXPECT_THROW(client_->commandPowerOn(true, std::chrono::milliseconds(1)), urcl::TimeoutException); + EXPECT_NO_THROW(waitFor([this]() { return client_->getRobotMode() == RobotMode::IDLE; }, timeout)); + + // Without a verification the calls should succeed, the robot ending up in the desired state + // eventually. + EXPECT_NO_THROW(client_->commandPowerOff(false)); + EXPECT_NO_THROW(waitFor([this]() { return client_->getRobotMode() == RobotMode::POWER_OFF; }, timeout)); + EXPECT_NO_THROW(client_->commandPowerOn(false)); + EXPECT_NO_THROW(waitFor([this]() { return client_->getRobotMode() == RobotMode::IDLE; }, timeout)); + EXPECT_NO_THROW(client_->commandBrakeRelease(false)); + EXPECT_NO_THROW(waitFor([this]() { return client_->getRobotMode() == RobotMode::RUNNING; }, timeout)); +} + +TEST_F(PrimaryClientTest, test_uninitialized_primary_client) +{ + // The client is not started yet, so the robot mode should be UNKNOWN + ASSERT_EQ(client_->getRobotMode(), RobotMode::UNKNOWN); +} + int main(int argc, char* argv[]) { ::testing::InitGoogleTest(&argc, argv); + for (int i = 0; i < argc; i++) + { + if (std::string(argv[i]) == "--robot_ip" && i + 1 < argc) + { + g_ROBOT_IP = argv[i + 1]; + break; + } + } return RUN_ALL_TESTS(); }