Succeed power on command on PolyScope 5 when robot is running (#397)

We check for the robot state being IDLE and retry that with a very high
timeout by default. This commit also accepts the robot to be in RUNNING
in order to succeed.

Otherwise the robot will reply "Powering on" through the dashboard
interface, but the state will stay unchanged because it is already
idling. Thus, the call will fail (and block very long) while the pure
dashboard call would be succeeding.

Author: urfeex

src/ur/dashboard_client_implementation_g5.cpp

@@ -438,7 +438,7 @@ DashboardResponse DashboardClientImplG5::commandPowerOn(const std::chrono::durat
   DashboardResponse response;
   try
   {
-    response.message = retryCommandString("power on", "Powering on", "robotmode", "Robotmode: IDLE", timeout);
+    response.message = retryCommandString("power on", "Powering on", "robotmode", "Robotmode: (IDLE|RUNNING)", timeout);
     response.ok = true;
   }
   catch (const UnexpectedResponse& e)

tests/test_dashboard_client_g5.cpp

@@ -91,6 +91,43 @@ TEST_F(DashboardClientTestG5, connect)
   ASSERT_TRUE(response.ok);
 }
 
+TEST_F(DashboardClientTestG5, power_cycle)
+{
+  EXPECT_TRUE(dashboard_client_->connect());
+  DashboardResponse response;
+
+  // Cycle from POWER_OFF to IDLE to RUNNING
+  response = dashboard_client_->commandPowerOff();
+  EXPECT_TRUE(response.ok);
+  response = dashboard_client_->commandPowerOn(std::chrono::seconds(5));
+  EXPECT_TRUE(response.ok);
+  response = dashboard_client_->commandBrakeRelease();
+  EXPECT_TRUE(response.ok);
+
+  // Calling power_on on a brake-released robot should succeed
+  response = dashboard_client_->commandPowerOn(std::chrono::seconds(5));
+  EXPECT_TRUE(response.ok);
+
+  // Power off from brake-released state (RUNNING)
+  response = dashboard_client_->commandPowerOff();
+  EXPECT_TRUE(response.ok);
+
+  // Power off from powered_on state (IDLE)
+  dashboard_client_->commandPowerOn();
+  response = dashboard_client_->commandPowerOff();
+  EXPECT_TRUE(response.ok);
+
+  // Power off from powered_on state (IDLE)
+  dashboard_client_->commandPowerOff();
+  response = dashboard_client_->commandPowerOff();
+  EXPECT_TRUE(response.ok);
+
+  // Brake release from POWER_OFF should succeed
+  dashboard_client_->commandPowerOff();
+  response = dashboard_client_->commandBrakeRelease();
+  EXPECT_TRUE(response.ok);
+}
+
 TEST_F(DashboardClientTestG5, run_program)
 {
   EXPECT_TRUE(dashboard_client_->connect());