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

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<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.
+   *
+   * \throws UrException when no robot mode data has been received, yet.
+   */
+  bool isRobotProtectiveStopped()
+  {
+    std::shared_ptr<RobotModeData> 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.

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<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 (!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,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[])