Use ExampleRobotWrapper for initialization in all examples (#265)

This way the examples are a lot shorter and initialization is the same
everywhere, making things easier to maintain.

Author: urfeex

doc/examples/force_mode.rst

@@ -6,15 +6,16 @@ Force Mode example
 The ``ur_client_library`` supports leveraging the robot's force mode directly. An example on how to
 use it can be found in `force_mode_example.cpp <https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/examples/force_mode_example.cpp>`_.
 
-In order to utilize force mode, we'll have to create and initialize a full ``UrDriver`` object
-first:
+In order to utilize force mode, we'll have to create and initialize a driver object
+first. We use the ``ExampleRobotWrapper`` class for this example. That starts a :ref:`ur_driver`
+and a :ref:`dashboard_client` to communicate with the robot:
 
 .. literalinclude:: ../../examples/force_mode_example.cpp
    :language: c++
    :caption: examples/force_mode_example.cpp
    :linenos:
    :lineno-match:
-   :start-at: // Now the robot is ready to receive a program
+   :start-at: bool headless_mode = true;
    :end-at: // End of initialization
 
 Start force mode

doc/examples/instruction_executor.rst

@@ -20,8 +20,8 @@ The `instruction_executor.cpp <https://github.com/UniversalRobots/Universal_Robo
    :caption: examples/instruction_executor.cpp
    :linenos:
    :lineno-match:
-   :start-at: std::unique_ptr<urcl::ToolCommSetup> tool_comm_setup;
-   :end-at: auto instruction_executor = std::make_shared<urcl::InstructionExecutor>(g_my_driver);
+   :start-at: bool headless_mode = true;
+   :end-at: auto instruction_executor = std::make_shared<urcl::InstructionExecutor>(g_my_robot->ur_driver_);
 
 At first, a ``InstructionExecutor`` object is created with the URDriver object as it needs that
 for communication with the robot.
@@ -61,6 +61,6 @@ To run a single motion, the ``InstructionExecutor`` provides the methods ``moveJ
    :linenos:
    :lineno-match:
    :start-at: double goal_time_sec = 2.0;
-   :end-before: g_my_driver->stopControl();
+   :end-before: g_my_robot->ur_driver_->stopControl();
 
 Again, time parametrization has priority over acceleration / velocity parameters.

doc/examples/tool_contact_example.rst

@@ -12,15 +12,15 @@ tool contact mode to detect collisions and stop the robot.
 As a basic concept, we will move the robot linearly in the negative z axis until the tool hits
 something or the program's timeout is hit.
 
-At first, we create a ``UrDriver`` object as usual:
+At first, we create a initialize a driver as usual:
 
 .. literalinclude:: ../../examples/tool_contact_example.cpp
    :language: c++
    :caption: examples/tool_contact_example.cpp
    :linenos:
    :lineno-match:
-   :start-at: std::unique_ptr<ToolCommSetup> tool_comm_setup;
-   :end-before: g_my_driver->registerToolContactResultCallback
+   :start-at: bool headless_mode = true;
+   :end-before: g_my_robot->ur_driver_->registerToolContactResultCallback
 
 We use a small helper function to make sure that the reverse interface is active and connected
 before proceeding.
@@ -45,8 +45,8 @@ This function is registered as a callback to the driver and then tool_contact mo
    :caption: examples/tool_contact_example.cpp
    :linenos:
    :lineno-match:
-   :start-at: g_my_driver->registerToolContactResultCallback(&handleToolContactResult);
-   :end-at: g_my_driver->startToolContact()
+   :start-at: g_my_robot->ur_driver_->registerToolContactResultCallback(&handleToolContactResult);
+   :end-at: g_my_robot->ur_driver_->startToolContact()
 
 Once everything is initialized, we can start a control loop commanding the robot to move in the
 negative z direction until the program timeout is hit or a tool contact is detected:

doc/examples/trajectory_point_interface.rst

@@ -14,7 +14,7 @@ At first, we create a ``UrDriver`` object as usual:
    :caption: examples/trajectory_point_interface.cpp
    :linenos:
    :lineno-match:
-   :start-at: std::unique_ptr<urcl::ToolCommSetup> tool_comm_setup;
+   :start-at: bool headless_mode = true;
    :end-before: // --------------- INITIALIZATION END -------------------
 
 We use a small helper function to make sure that the reverse interface is active and connected
@@ -45,8 +45,8 @@ That callback can be registered at the ``UrDriver`` object:
    :caption: examples/trajectory_point_interface.cpp
    :linenos:
    :lineno-match:
-   :start-at: g_my_driver->registerTrajectoryDoneCallback(&trajDoneCallback);
-   :end-at: g_my_driver->registerTrajectoryDoneCallback(&trajDoneCallback);
+   :start-at: g_my_robot->ur_driver_->registerTrajectoryDoneCallback(&trajDoneCallback);
+   :end-at: g_my_robot->ur_driver_->registerTrajectoryDoneCallback(&trajDoneCallback);
 
 
 MoveJ Trajectory
@@ -67,12 +67,12 @@ parameters. The following example shows execution of a 2-point trajectory using
 In fact, the path is followed twice, once parametrized by a segment duration and once using maximum
 velocity / acceleration settings. If a duration > 0 is given for a segment, the velocity and
 acceleration settings will be ignored as in the underlying URScript functions. In the example
-above, each of the ``g_my_driver->writeTrajectoryPoint()`` calls will be translated into a
+above, each of the ``g_my_robot->ur_driver_->writeTrajectoryPoint()`` calls will be translated into a
 ``movej`` command in URScript.
 
 While the trajectory is running, we need to tell the robot program that connection is still active
 and we expect the trajectory to be running. This is being done by the
-``g_my_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP);``
+``g_my_robot->ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP);``
 call.
 
 MoveL Trajectory

doc/examples/ur_driver.rst

@@ -20,16 +20,16 @@ example.
 Initialization
 --------------
 
-At first, we create a ``UrDriver`` object giving it the robot's IP address, script file and RTDE
+At first, we create a ``ExampleRobotWrapper`` object giving it the robot's IP address, script file and RTDE
 recipes.
 
 .. literalinclude:: ../../examples/full_driver.cpp
    :language: c++
    :caption: examples/full_driver.cpp
    :linenos:
    :lineno-match:
-   :start-at: std::unique_ptr<ToolCommSetup> tool_comm_setup;
-   :end-at: std::move(tool_comm_setup)
+   :start-at: bool headless_mode = true;
+   :end-at: // --------------- INITIALIZATION END -------------------
 
 Robot control loop
 ------------------

examples/force_mode_example.cpp

@@ -33,6 +33,7 @@
 // In a real-world example it would be better to get those values from command line parameters / a
 // better configuration system such as Boost.Program_options
 
+#include <ur_client_library/example_robot_wrapper.h>
 #include <ur_client_library/ur/dashboard_client.h>
 #include <ur_client_library/ur/ur_driver.h>
 #include <ur_client_library/types.h>
@@ -52,47 +53,18 @@ const std::string OUTPUT_RECIPE = "examples/resources/rtde_output_recipe.txt";
 const std::string INPUT_RECIPE = "examples/resources/rtde_input_recipe.txt";
 const std::string CALIBRATION_CHECKSUM = "calib_12788084448423163542";
 
-std::unique_ptr<UrDriver> g_my_driver;
-std::unique_ptr<DashboardClient> g_my_dashboard;
-
-std::condition_variable g_program_running_cv;
-std::mutex g_program_running_mutex;
-bool g_program_running;
-
-// We need a callback function to register. See UrDriver's parameters for details.
-void handleRobotProgramState(bool program_running)
-{
-  // Print the text in green so we see it better
-  std::cout << "\033[1;32mProgram running: " << std::boolalpha << program_running << "\033[0m\n" << std::endl;
-  if (program_running)
-  {
-    std::lock_guard<std::mutex> lk(g_program_running_mutex);
-    g_program_running = program_running;
-    g_program_running_cv.notify_one();
-  }
-}
+std::unique_ptr<ExampleRobotWrapper> g_my_robot;
 
 void sendFreedriveMessageOrDie(const control::FreedriveControlMessage freedrive_action)
 {
-  bool ret = g_my_driver->writeFreedriveControlMessage(freedrive_action);
+  bool ret = g_my_robot->ur_driver_->writeFreedriveControlMessage(freedrive_action);
   if (!ret)
   {
     URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?");
     exit(1);
   }
 }
 
-bool waitForProgramRunning(int milliseconds = 100)
-{
-  std::unique_lock<std::mutex> lk(g_program_running_mutex);
-  if (g_program_running_cv.wait_for(lk, std::chrono::milliseconds(milliseconds)) == std::cv_status::no_timeout ||
-      g_program_running == true)
-  {
-    return true;
-  }
-  return false;
-}
-
 int main(int argc, char* argv[])
 {
   urcl::setLogLevel(urcl::LogLevel::INFO);
@@ -110,50 +82,16 @@ int main(int argc, char* argv[])
     second_to_run = std::chrono::seconds(std::stoi(argv[2]));
   }
 
-  // Making the robot ready for the program by:
-  // Connect the robot Dashboard
-  g_my_dashboard.reset(new DashboardClient(robot_ip));
-  if (!g_my_dashboard->connect())
-  {
-    URCL_LOG_ERROR("Could not connect to dashboard");
-    return 1;
-  }
+  bool headless_mode = true;
+  g_my_robot = std::make_unique<ExampleRobotWrapper>(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, headless_mode,
+                                                     "external_control.urp");
 
-  // // Stop program, if there is one running
-  if (!g_my_dashboard->commandStop())
+  if (!g_my_robot->isHealthy())
   {
-    URCL_LOG_ERROR("Could not send stop program command");
+    URCL_LOG_ERROR("Something in the robot initialization went wrong. Exiting. Please check the output above.");
     return 1;
   }
-
-  // Power it off
-  // if (!g_my_dashboard->commandPowerOff())
-  //{
-  // URCL_LOG_ERROR("Could not send Power off command");
-  // return 1;
-  //}
-
-  // Power it on
-  // if (!g_my_dashboard->commandPowerOn())
-  //{
-  // URCL_LOG_ERROR("Could not send Power on command");
-  // return 1;
-  //}
-
-  // Release the brakes
-  // if (!g_my_dashboard->commandBrakeRelease())
-  //{
-  // URCL_LOG_ERROR("Could not send BrakeRelease command");
-  // return 1;
-  //}
-
-  // Now the robot is ready to receive a program
-  std::unique_ptr<ToolCommSetup> tool_comm_setup;
-  const bool headless = true;
-  g_my_driver.reset(new UrDriver(robot_ip, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, &handleRobotProgramState, headless,
-                                 std::move(tool_comm_setup)));
-
-  if (!g_my_driver->checkCalibration(CALIBRATION_CHECKSUM))
+  if (!g_my_robot->ur_driver_->checkCalibration(CALIBRATION_CHECKSUM))
   {
     URCL_LOG_ERROR("Calibration checksum does not match actual robot.");
     URCL_LOG_ERROR("Use the ur_calibration tool to extract the correct calibration from the robot and pass that into "
@@ -162,35 +100,31 @@ int main(int argc, char* argv[])
                    "for details.");
   }
 
-  // Make sure that external control script is running
-  if (!waitForProgramRunning())
-  {
-    URCL_LOG_ERROR("External Control script not running.");
-    return 1;
-  }
   // End of initialization -- We've started the external control program, which means we have to
   // write keepalive signals from now on. Otherwise the connection will be dropped.
 
   // Start force mode
   // Task frame at the robot's base with limits being large enough to cover the whole workspace
   // Compliance in z axis and rotation around z axis
   bool success;
-  if (g_my_driver->getVersion().major < 5)
-    success = g_my_driver->startForceMode({ 0, 0, 0, 0, 0, 0 },   // Task frame at the robot's base
-                                          { 0, 0, 1, 0, 0, 1 },   // Compliance in z axis and rotation around z axis
-                                          { 0, 0, -2, 0, 0, 0 },  // Press in -z direction
-                                          2,                      // do not transform the force frame at all
-                                          { 0.1, 0.1, 1.5, 3.14, 3.14, 0.5 },  // limits
-                                          0.005);  // damping_factor. See ScriptManual for details.
+  if (g_my_robot->ur_driver_->getVersion().major < 5)
+    success =
+        g_my_robot->ur_driver_->startForceMode({ 0, 0, 0, 0, 0, 0 },  // Task frame at the robot's base
+                                               { 0, 0, 1, 0, 0, 1 },  // Compliance in z axis and rotation around z axis
+                                               { 0, 0, -2, 0, 0, 0 },  // Press in -z direction
+                                               2,                      // do not transform the force frame at all
+                                               { 0.1, 0.1, 1.5, 3.14, 3.14, 0.5 },  // limits
+                                               0.005);  // damping_factor. See ScriptManual for details.
   else
   {
-    success = g_my_driver->startForceMode({ 0, 0, 0, 0, 0, 0 },   // Task frame at the robot's base
-                                          { 0, 0, 1, 0, 0, 1 },   // Compliance in z axis and rotation around z axis
-                                          { 0, 0, -2, 0, 0, 0 },  // Press in -z direction
-                                          2,                      // do not transform the force frame at all
-                                          { 0.1, 0.1, 1.5, 3.14, 3.14, 0.5 },  // limits
-                                          0.005,                               // damping_factor
-                                          1.0);  // gain_scaling. See ScriptManual for details.
+    success =
+        g_my_robot->ur_driver_->startForceMode({ 0, 0, 0, 0, 0, 0 },  // Task frame at the robot's base
+                                               { 0, 0, 1, 0, 0, 1 },  // Compliance in z axis and rotation around z axis
+                                               { 0, 0, -2, 0, 0, 0 },  // Press in -z direction
+                                               2,                      // do not transform the force frame at all
+                                               { 0.1, 0.1, 1.5, 3.14, 3.14, 0.5 },  // limits
+                                               0.005,                               // damping_factor
+                                               1.0);  // gain_scaling. See ScriptManual for details.
   }
   if (!success)
   {
@@ -204,13 +138,13 @@ int main(int argc, char* argv[])
   auto stopwatch_now = stopwatch_last;
   while (time_done < timeout || second_to_run.count() == 0)
   {
-    g_my_driver->writeKeepalive();
+    g_my_robot->ur_driver_->writeKeepalive();
 
     stopwatch_now = std::chrono::steady_clock::now();
     time_done += stopwatch_now - stopwatch_last;
     stopwatch_last = stopwatch_now;
     std::this_thread::sleep_for(std::chrono::milliseconds(2));
   }
   URCL_LOG_INFO("Timeout reached.");
-  g_my_driver->endForceMode();
+  g_my_robot->ur_driver_->endForceMode();
 }

examples/freedrive_example.cpp

@@ -85,9 +85,9 @@ int main(int argc, char* argv[])
   g_my_robot = std::make_unique<ExampleRobotWrapper>(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, headless_mode,
                                                      "external_control.urp");
 
-  if (!g_my_robot->waitForProgramRunning())
+  if (!g_my_robot->isHealthy())
   {
-    URCL_LOG_ERROR("External Control script not running.");
+    URCL_LOG_ERROR("Something in the robot initialization went wrong. Exiting. Please check the output above.");
     return 1;
   }