PrimaryClient: Add methods to unlock protective stop and stop the program

include/ur_client_library/exceptions.h

@@ -32,6 +32,7 @@
 #include <chrono>
 #include <stdexcept>
 #include <sstream>
+#include <string>
 #include "ur/version_information.h"
 
 #ifdef _WIN32
@@ -203,5 +204,22 @@ class MissingArgument : public UrException
     return text_.c_str();
   }
 };
+
+class InvalidStateForCommand : public UrException
+{
+private:
+  std::string text_;
+
+public:
+  explicit InvalidStateForCommand() = delete;
+  explicit InvalidStateForCommand(std::string text) : std::runtime_error(text)
+  {
+    text_ = text;
+  }
+  virtual const char* what() const noexcept override
+  {
+    return text_.c_str();
+  }
+};
 }  // namespace urcl
 #endif  // ifndef UR_CLIENT_LIBRARY_EXCEPTIONS_H_INCLUDED

include/ur_client_library/primary/primary_client.h

@@ -128,6 +128,39 @@ 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
+   *
+   * Blocks until the robot has come to a standstill (while following
+   * the program path) and stopped program execution.
+   *
+   * \param already_stopped_ok If true, the function will return true if the robot is already
+   * stopped.
+   * \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 already_stopped_ok = false, const bool validate = true,
+                   const std::chrono::milliseconds timeout = std::chrono::seconds(2));
+
   /*!
    * \brief Get the latest robot mode.
    *
@@ -144,6 +177,34 @@ class PrimaryClient
     return static_cast<RobotMode>(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<RobotModeData> 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.
+   */
+  bool isRobotProtectiveStopped()
+  {
+    std::shared_ptr<RobotModeData> robot_mode_data = consumer_->getRobotModeData();
+    if (robot_mode_data == nullptr)
+    {
+      return false;
+    }
+    return robot_mode_data->is_protective_stopped_;
+  }
+
 private:
   /*!
    * \brief Reconnects the primary stream used to send program to the robot.

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,75 @@ 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 already_stopped_ok, const bool validate,
+                                const std::chrono::milliseconds timeout)
+{
+  std::shared_ptr<RobotModeData> 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 (!(robot_mode_data->is_program_running_ || robot_mode_data->is_program_paused_))
+  {
+    if (already_stopped_ok)
+    {
+      URCL_LOG_DEBUG("Program halt requested, but program is already stopped, skipping.");
+      return;
+    }
+    else
+    {
+      throw InvalidStateForCommand("Cannot halt program execution, as no program is running or paused on the robot.");
+    }
+  }
+
+  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

tests/test_primary_client.cpp

@@ -110,12 +110,65 @@ 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);
 }
 
+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 throw an exception
+  EXPECT_THROW(client_->commandStop(), InvalidStateForCommand);
+
+  // We can specifically allow to be stopped already
+  EXPECT_NO_THROW(client_->commandStop(true));
+
+  EXPECT_TRUE(client_->sendScript(script_code));
+  waitFor([this]() { return client_->getRobotModeData()->is_program_running_; }, std::chrono::seconds(5));
+  EXPECT_THROW(client_->commandStop(true, 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)));
+}
+
 int main(int argc, char* argv[])
 {
   ::testing::InitGoogleTest(&argc, argv);
@@ -129,4 +182,4 @@ int main(int argc, char* argv[])
   }
 
   return RUN_ALL_TESTS();
-}
+}