diff --git a/include/ur_client_library/primary/primary_client.h b/include/ur_client_library/primary/primary_client.h index 37de1d34a..92322f598 100644 --- a/include/ur_client_library/primary/primary_client.h +++ b/include/ur_client_library/primary/primary_client.h @@ -128,6 +128,33 @@ class PrimaryClient void commandBrakeRelease(const bool validate = true, const std::chrono::milliseconds timeout = std::chrono::seconds(30)); + /*! + * \brief Commands the robot to unlock the protective stop + * + * \param validate If true, the function will block until the protective stop is released or the + * timeout passed by. + * \param timeout The maximum time to wait for the robot to confirm it is no longer protective + * stopped. + * + * \throws urcl::UrException if the command cannot be sent to the robot. + * \throws urcl::TimeoutException if the robot doesn't unlock the protective stop within the + * given timeout. + */ + void commandUnlockProtectiveStop(const bool validate = true, + const std::chrono::milliseconds timeout = std::chrono::milliseconds(5000)); + + /*! + * /brief Stop execution of a running or paused program + * + * \param validate If true, the function will block until the robot has stopped or the timeout + * passed by. + * \param timeout The maximum time to wait for the robot to stop the program. + * + * \throws urcl::UrException if the command cannot be sent to the robot. + * \throws urcl::TimeoutException if the robot doesn't stop the program within the given timeout. + */ + void commandStop(const bool validate = true, const std::chrono::milliseconds timeout = std::chrono::seconds(2)); + /*! * \brief Get the latest robot mode. * @@ -144,6 +171,36 @@ class PrimaryClient return static_cast(consumer_->getRobotModeData()->robot_mode_); } + /*! + * \brief Get the latest robot mode data. + * + * The robot's mode data will be updated in the background. This will always show the latest received + * state independent of the time that has passed since receiving it. The return value of this + * will be a nullptr if no data has been received yet. + */ + std::shared_ptr getRobotModeData() + { + return consumer_->getRobotModeData(); + } + + /*! + * \brief Query if the robot is protective stopped. + * + * The robot's protective_stop state will be updated in the background. This will always show the latest received + * state independent of the time that has passed since receiving it. + * + * \throws UrException when no robot mode data has been received, yet. + */ + bool isRobotProtectiveStopped() + { + std::shared_ptr robot_mode_data = consumer_->getRobotModeData(); + if (robot_mode_data == nullptr) + { + throw UrException("Robot mode data is a nullptr. Probably it hasn't been received, yet."); + } + return robot_mode_data->is_protective_stopped_; + } + private: /*! * \brief Reconnects the primary stream used to send program to the robot. diff --git a/src/primary/primary_client.cpp b/src/primary/primary_client.cpp index 7f44598e9..ae034872e 100644 --- a/src/primary/primary_client.cpp +++ b/src/primary/primary_client.cpp @@ -193,7 +193,7 @@ void PrimaryClient::commandPowerOff(const bool validate, const std::chrono::mill { waitFor([this]() { return getRobotMode() == RobotMode::POWER_OFF; }, timeout); } - catch (const std::exception&) + catch (const TimeoutException&) { throw TimeoutException("Robot did not power off within the given timeout", timeout); } @@ -212,12 +212,61 @@ void PrimaryClient::commandBrakeRelease(const bool validate, const std::chrono:: { waitFor([this]() { return getRobotMode() == RobotMode::RUNNING; }, timeout); } - catch (const std::exception&) + catch (const TimeoutException&) { throw TimeoutException("Robot did not release the brakes within the given timeout", timeout); } } } +void PrimaryClient::commandUnlockProtectiveStop(const bool validate, const std::chrono::milliseconds timeout) +{ + if (!sendScript("set unlock protective stop")) + { + throw UrException("Failed to send unlock protective stop command to robot"); + } + if (validate) + { + try + { + waitFor([this]() { return consumer_->getRobotModeData()->is_protective_stopped_ == false; }, timeout); + } + catch (const TimeoutException&) + { + throw TimeoutException("Robot did not unlock the protective stop within the given timeout", timeout); + } + } +} + +void PrimaryClient::commandStop(const bool validate, const std::chrono::milliseconds timeout) +{ + std::shared_ptr robot_mode_data = consumer_->getRobotModeData(); + if (robot_mode_data == nullptr) + { + throw UrException("Stopping a program while robot state is unknown. This should not happen"); + } + + if (!sendScript("stop program")) + { + throw UrException("Failed to send the command `stop program` to robot"); + } + if (validate) + { + try + { + waitFor( + [this]() { + return !consumer_->getRobotModeData()->is_program_running_ && + !consumer_->getRobotModeData()->is_program_paused_; + }, + timeout); + } + catch (const TimeoutException&) + { + throw TimeoutException("Robot did not stop the program within the given timeout", timeout); + } + } +} + } // namespace primary_interface } // namespace urcl diff --git a/tests/test_primary_client.cpp b/tests/test_primary_client.cpp index 9f28f2553..dda0cd110 100644 --- a/tests/test_primary_client.cpp +++ b/tests/test_primary_client.cpp @@ -110,10 +110,71 @@ TEST_F(PrimaryClientTest, test_power_cycle_commands) EXPECT_NO_THROW(waitFor([this]() { return client_->getRobotMode() == RobotMode::RUNNING; }, timeout)); } +TEST_F(PrimaryClientTest, test_unlock_protective_stop) +{ + EXPECT_NO_THROW(client_->start()); + EXPECT_NO_THROW(client_->commandBrakeRelease(true, std::chrono::milliseconds(20000))); + client_->sendScript("protective_stop()"); + EXPECT_NO_THROW(waitFor([this]() { return client_->isRobotProtectiveStopped(); }, std::chrono::milliseconds(1000))); + // This will not happen immediately + EXPECT_THROW(client_->commandUnlockProtectiveStop(true, std::chrono::milliseconds(1)), TimeoutException); + + // It is not allowed to unlock the protective stop immediately + std::this_thread::sleep_for(std::chrono::seconds(5)); + EXPECT_NO_THROW(client_->commandUnlockProtectiveStop()); +} + 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); + ASSERT_THROW(client_->isRobotProtectiveStopped(), UrException); +} + +TEST_F(PrimaryClientTest, test_stop_command) +{ + // Without started communication the latest robot mode data is a nullptr + EXPECT_THROW(client_->commandStop(), UrException); + + EXPECT_NO_THROW(client_->start()); + EXPECT_NO_THROW(client_->commandPowerOff()); + EXPECT_NO_THROW(client_->commandBrakeRelease()); + + const std::string script_code = "def test_fun():\n" + " while True:\n" + " textmsg(\"still running\")\n" + " sleep(1.0)\n" + " sync()\n" + " end\n" + "end"; + + EXPECT_TRUE(client_->sendScript(script_code)); + waitFor([this]() { return client_->getRobotModeData()->is_program_running_; }, std::chrono::seconds(5)); + + EXPECT_NO_THROW(client_->commandStop()); + EXPECT_FALSE(client_->getRobotModeData()->is_program_running_); + + // Without a program running it should not throw an exception + EXPECT_NO_THROW(client_->commandStop()); + + EXPECT_TRUE(client_->sendScript(script_code)); + waitFor([this]() { return client_->getRobotModeData()->is_program_running_; }, std::chrono::seconds(5)); + EXPECT_THROW(client_->commandStop(true, std::chrono::milliseconds(1)), TimeoutException); + EXPECT_NO_THROW(waitFor( + [this]() { + return !client_->getRobotModeData()->is_program_running_ && !client_->getRobotModeData()->is_program_paused_; + }, + std::chrono::seconds(5))); + + // without validation + EXPECT_TRUE(client_->sendScript(script_code)); + waitFor([this]() { return client_->getRobotModeData()->is_program_running_; }, std::chrono::seconds(5)); + EXPECT_NO_THROW(client_->commandStop(false)); + EXPECT_NO_THROW(waitFor( + [this]() { + return !client_->getRobotModeData()->is_program_running_ && !client_->getRobotModeData()->is_program_paused_; + }, + std::chrono::seconds(5))); } int main(int argc, char* argv[])