diff --git a/doc/examples/force_mode.rst b/doc/examples/force_mode.rst index 56ab6324..6ee364d3 100644 --- a/doc/examples/force_mode.rst +++ b/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 `_. -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 diff --git a/doc/examples/instruction_executor.rst b/doc/examples/instruction_executor.rst index f4e21ec0..58439972 100644 --- a/doc/examples/instruction_executor.rst +++ b/doc/examples/instruction_executor.rst @@ -20,8 +20,8 @@ The `instruction_executor.cpp tool_comm_setup; - :end-at: auto instruction_executor = std::make_shared(g_my_driver); + :start-at: bool headless_mode = true; + :end-at: auto instruction_executor = std::make_shared(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. diff --git a/doc/examples/tool_contact_example.rst b/doc/examples/tool_contact_example.rst index 25cb9a95..d38630b5 100644 --- a/doc/examples/tool_contact_example.rst +++ b/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 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: diff --git a/doc/examples/trajectory_point_interface.rst b/doc/examples/trajectory_point_interface.rst index 5dd58f20..51607f36 100644 --- a/doc/examples/trajectory_point_interface.rst +++ b/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 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 diff --git a/doc/examples/ur_driver.rst b/doc/examples/ur_driver.rst index a146f198..874c4922 100644 --- a/doc/examples/ur_driver.rst +++ b/doc/examples/ur_driver.rst @@ -20,7 +20,7 @@ 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 @@ -28,8 +28,8 @@ recipes. :caption: examples/full_driver.cpp :linenos: :lineno-match: - :start-at: std::unique_ptr tool_comm_setup; - :end-at: std::move(tool_comm_setup) + :start-at: bool headless_mode = true; + :end-at: // --------------- INITIALIZATION END ------------------- Robot control loop ------------------ diff --git a/examples/force_mode_example.cpp b/examples/force_mode_example.cpp index 03faff5d..47d70f0e 100644 --- a/examples/force_mode_example.cpp +++ b/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 #include #include #include @@ -52,29 +53,11 @@ 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 g_my_driver; -std::unique_ptr 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 lk(g_program_running_mutex); - g_program_running = program_running; - g_program_running_cv.notify_one(); - } -} +std::unique_ptr 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?"); @@ -82,17 +65,6 @@ void sendFreedriveMessageOrDie(const control::FreedriveControlMessage freedrive_ } } -bool waitForProgramRunning(int milliseconds = 100) -{ - std::unique_lock 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(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 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,12 +100,6 @@ 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. @@ -175,22 +107,24 @@ int main(int argc, char* argv[]) // 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,7 +138,7 @@ 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; @@ -212,5 +146,5 @@ int main(int argc, char* argv[]) std::this_thread::sleep_for(std::chrono::milliseconds(2)); } URCL_LOG_INFO("Timeout reached."); - g_my_driver->endForceMode(); + g_my_robot->ur_driver_->endForceMode(); } diff --git a/examples/freedrive_example.cpp b/examples/freedrive_example.cpp index 48c0d8d7..4d691d0d 100644 --- a/examples/freedrive_example.cpp +++ b/examples/freedrive_example.cpp @@ -85,9 +85,9 @@ int main(int argc, char* argv[]) g_my_robot = std::make_unique(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; } diff --git a/examples/full_driver.cpp b/examples/full_driver.cpp index 43b07725..900301c8 100644 --- a/examples/full_driver.cpp +++ b/examples/full_driver.cpp @@ -28,6 +28,7 @@ */ //---------------------------------------------------------------------- +#include #include #include #include @@ -44,38 +45,9 @@ const std::string SCRIPT_FILE = "resources/external_control.urscript"; const std::string OUTPUT_RECIPE = "examples/resources/rtde_output_recipe.txt"; const std::string INPUT_RECIPE = "examples/resources/rtde_input_recipe.txt"; -std::unique_ptr g_my_driver; -std::unique_ptr g_my_dashboard; +std::unique_ptr g_my_robot; vector6d_t g_joint_positions; -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 lk(g_program_running_mutex); - g_program_running = program_running; - g_program_running_cv.notify_one(); - } -} - -bool waitForProgramRunning(int milliseconds = 100) -{ - std::unique_lock 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); @@ -87,59 +59,19 @@ int main(int argc, char* argv[]) robot_ip = std::string(argv[1]); } - // Making the robot ready for the program by: - // Connect the 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; - } - - // Stop program, if there is one running - if (!g_my_dashboard->commandStop()) - { - URCL_LOG_ERROR("Could not send stop program command"); - 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 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))); - // Make sure that external control script is running - if (!waitForProgramRunning()) + bool headless_mode = true; + g_my_robot = std::make_unique(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, headless_mode, + "external_control.urp"); + 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; } + // --------------- INITIALIZATION END ------------------- // Increment depends on robot version double increment_constant = 0.0005; - if (g_my_driver->getVersion().major < 5) + if (g_my_robot->ur_driver_->getVersion().major < 5) { increment_constant = 0.002; } @@ -154,14 +86,14 @@ int main(int argc, char* argv[]) // Once RTDE communication is started, we have to make sure to read from the interface buffer, as // otherwise we will get pipeline overflows. Therefor, do this directly before starting your main // loop. - g_my_driver->startRTDECommunication(); + g_my_robot->ur_driver_->startRTDECommunication(); while (!(passed_positive_part && passed_negative_part)) { // Read latest RTDE package. This will block for a hard-coded timeout (see UrDriver), so the // robot will effectively be in charge of setting the frequency of this loop. // In a real-world application this thread should be scheduled with real-time priority in order // to ensure that this is called in time. - std::unique_ptr data_pkg = g_my_driver->getDataPackage(); + std::unique_ptr data_pkg = g_my_robot->ur_driver_->getDataPackage(); if (!data_pkg) { URCL_LOG_WARN("Could not get fresh data package from robot"); @@ -201,8 +133,8 @@ int main(int argc, char* argv[]) joint_target[5] += increment; // Setting the RobotReceiveTimeout time is for example purposes only. This will make the example running more // reliable on non-realtime systems. Use with caution in productive applications. - bool ret = g_my_driver->writeJointCommand(joint_target, comm::ControlMode::MODE_SERVOJ, - RobotReceiveTimeout::millisec(100)); + bool ret = g_my_robot->ur_driver_->writeJointCommand(joint_target, comm::ControlMode::MODE_SERVOJ, + RobotReceiveTimeout::millisec(100)); if (!ret) { URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?"); @@ -210,7 +142,7 @@ int main(int argc, char* argv[]) } URCL_LOG_DEBUG("data_pkg:\n%s", data_pkg->toString().c_str()); } - g_my_driver->stopControl(); + g_my_robot->ur_driver_->stopControl(); URCL_LOG_INFO("Movement done"); return 0; } diff --git a/examples/instruction_executor.cpp b/examples/instruction_executor.cpp index 90766776..9a55affb 100644 --- a/examples/instruction_executor.cpp +++ b/examples/instruction_executor.cpp @@ -35,6 +35,7 @@ #include "ur_client_library/types.h" #include "ur_client_library/ur/ur_driver.h" #include "ur_client_library/log.h" +#include #include "ur_client_library/control/trajectory_point_interface.h" #include "ur_client_library/ur/dashboard_client.h" #include "ur_client_library/ur/instruction_executor.h" @@ -45,15 +46,7 @@ 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 g_my_dashboard; -std::shared_ptr g_my_driver; - -// 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; -} +std::unique_ptr g_my_robot; int main(int argc, char* argv[]) { @@ -65,49 +58,24 @@ int main(int argc, char* argv[]) robot_ip = std::string(argv[1]); } - // --------------- INITIALIZATION BEGIN ------------------- - // Making the robot ready for the program by: - // Connect the robot Dashboard - g_my_dashboard.reset(new urcl::DashboardClient(robot_ip)); - if (!g_my_dashboard->connect()) + bool headless_mode = true; + g_my_robot = std::make_unique(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, headless_mode, + "external_control.urp"); + if (!g_my_robot->ur_driver_->checkCalibration(CALIBRATION_CHECKSUM)) { - URCL_LOG_ERROR("Could not connect to dashboard"); - return 1; + 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 " + "the description. See " + "[https://github.com/UniversalRobots/Universal_Robots_ROS_Driver#extract-calibration-information] " + "for details."); } - - // // Stop program, if there is one running - if (!g_my_dashboard->commandStop()) - { - URCL_LOG_ERROR("Could not send stop program command"); - 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()) + if (!g_my_robot->isHealthy()) { - URCL_LOG_ERROR("Could not send BrakeRelease command"); + URCL_LOG_ERROR("Something in the robot initialization went wrong. Exiting. Please check the output above."); return 1; } - std::unique_ptr tool_comm_setup; - const bool headless = true; - g_my_driver.reset(new urcl::UrDriver(robot_ip, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, &handleRobotProgramState, - headless, std::move(tool_comm_setup), CALIBRATION_CHECKSUM)); - auto instruction_executor = std::make_shared(g_my_driver); + auto instruction_executor = std::make_shared(g_my_robot->ur_driver_); // --------------- INITIALIZATION END ------------------- URCL_LOG_INFO("Running motion"); @@ -137,6 +105,6 @@ int main(int argc, char* argv[]) // goal time parametrization -- acceleration and velocity will be ignored instruction_executor->moveL({ -0.203, 0.463, 0.559, 0.68, -1.083, -2.076 }, 0.1, 0.1, goal_time_sec); - g_my_driver->stopControl(); + g_my_robot->ur_driver_->stopControl(); return 0; } diff --git a/examples/spline_example.cpp b/examples/spline_example.cpp index 9af4240d..3fb5e1d9 100644 --- a/examples/spline_example.cpp +++ b/examples/spline_example.cpp @@ -30,6 +30,7 @@ // POSSIBILITY OF SUCH DAMAGE. // -- END LICENSE BLOCK ------------------------------------------------ +#include #include #include #include @@ -46,10 +47,8 @@ const std::string DEFAULT_ROBOT_IP = "192.168.56.101"; const std::string SCRIPT_FILE = "resources/external_control.urscript"; 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 g_my_driver; -std::unique_ptr g_my_dashboard; +std::unique_ptr g_my_robot; vector6d_t g_joint_positions; void sendTrajectory(const std::vector& p_p, const std::vector& p_v, @@ -58,43 +57,37 @@ void sendTrajectory(const std::vector& p_p, const std::vectorwriteTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, p_p.size()); + g_my_robot->ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, + p_p.size()); for (size_t i = 0; i < p_p.size() && p_p.size() == time.size() && p_p[i].size() == 6; i++) { // MoveJ if (!use_spline_interpolation_) { - g_my_driver->writeTrajectoryPoint(p_p[i], false, time[i]); + g_my_robot->ur_driver_->writeTrajectoryPoint(p_p[i], false, time[i]); } else // Use spline interpolation { // QUINTIC if (p_v.size() == time.size() && p_a.size() == time.size() && p_v[i].size() == 6 && p_a[i].size() == 6) { - g_my_driver->writeTrajectorySplinePoint(p_p[i], p_v[i], p_a[i], time[i]); + g_my_robot->ur_driver_->writeTrajectorySplinePoint(p_p[i], p_v[i], p_a[i], time[i]); } // CUBIC else if (p_v.size() == time.size() && p_v[i].size() == 6) { - g_my_driver->writeTrajectorySplinePoint(p_p[i], p_v[i], time[i]); + g_my_robot->ur_driver_->writeTrajectorySplinePoint(p_p[i], p_v[i], time[i]); } else { - g_my_driver->writeTrajectorySplinePoint(p_p[i], time[i]); + g_my_robot->ur_driver_->writeTrajectorySplinePoint(p_p[i], time[i]); } } } URCL_LOG_INFO("Finished Sending Trajectory"); } -// 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; -} - // Callback function for trajectory execution. bool g_trajectory_running(false); void handleTrajectoryState(control::TrajectoryResult state) @@ -129,64 +122,24 @@ int main(int argc, char* argv[]) robot_ip = std::string(argv[1]); } - // Making the robot ready for the program by: - // Connect the the robot Dashboard - g_my_dashboard.reset(new DashboardClient(robot_ip)); - if (!g_my_dashboard->connect()) + bool headless_mode = true; + g_my_robot = std::make_unique(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, headless_mode, + "external_control.urp"); + if (!g_my_robot->isHealthy()) { - URCL_LOG_ERROR("Could not connect to dashboard"); + URCL_LOG_ERROR("Something in the robot initialization went wrong. Exiting. Please check the output above."); return 1; } - // Stop program, if there is one running - if (!g_my_dashboard->commandStop()) - { - URCL_LOG_ERROR("Could not send stop program command"); - return 1; - } - - // if the robot is not powered on and ready - std::string robot_mode_running("RUNNING"); - while (!g_my_dashboard->commandRobotMode(robot_mode_running)) - { - // 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 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), CALIBRATION_CHECKSUM)); - - g_my_driver->registerTrajectoryDoneCallback(&handleTrajectoryState); + g_my_robot->ur_driver_->registerTrajectoryDoneCallback(&handleTrajectoryState); // Once RTDE communication is started, we have to make sure to read from the interface buffer, as // otherwise we will get pipeline overflows. Therefore, do this directly before starting your main // loop. - g_my_driver->startRTDECommunication(); + g_my_robot->ur_driver_->startRTDECommunication(); - std::unique_ptr data_pkg = g_my_driver->getDataPackage(); + std::unique_ptr data_pkg = g_my_robot->ur_driver_->getDataPackage(); if (data_pkg) { @@ -209,7 +162,7 @@ int main(int argc, char* argv[]) 4.00000000e+00 }; bool ret = false; - ret = g_my_driver->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_NOOP); + ret = g_my_robot->ur_driver_->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_NOOP); if (!ret) { std::stringstream lastq; @@ -246,7 +199,7 @@ int main(int argc, char* argv[]) g_trajectory_running = true; while (g_trajectory_running) { - std::unique_ptr data_pkg = g_my_driver->getDataPackage(); + std::unique_ptr data_pkg = g_my_robot->ur_driver_->getDataPackage(); if (data_pkg) { // Read current joint positions from robot data @@ -256,7 +209,7 @@ int main(int argc, char* argv[]) std::string error_msg = "Did not find 'actual_q' in data sent from robot. This should not happen!"; throw std::runtime_error(error_msg); } - ret = g_my_driver->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_NOOP); + ret = g_my_robot->ur_driver_->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_NOOP); if (!ret) { @@ -277,7 +230,7 @@ int main(int argc, char* argv[]) g_trajectory_running = true; while (g_trajectory_running) { - std::unique_ptr data_pkg = g_my_driver->getDataPackage(); + std::unique_ptr data_pkg = g_my_robot->ur_driver_->getDataPackage(); if (data_pkg) { // Read current joint positions from robot data @@ -287,7 +240,7 @@ int main(int argc, char* argv[]) std::string error_msg = "Did not find 'actual_q' in data sent from robot. This should not happen!"; throw std::runtime_error(error_msg); } - ret = g_my_driver->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_NOOP); + ret = g_my_robot->ur_driver_->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_NOOP); if (!ret) { @@ -302,7 +255,7 @@ int main(int argc, char* argv[]) URCL_LOG_INFO("QUINTIC Movement done"); - ret = g_my_driver->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_NOOP); + ret = g_my_robot->ur_driver_->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_NOOP); if (!ret) { std::stringstream lastq; @@ -311,6 +264,6 @@ int main(int argc, char* argv[]) lastq.str().c_str()); return 1; } - g_my_driver->stopControl(); + g_my_robot->ur_driver_->stopControl(); return 0; } diff --git a/examples/tool_contact_example.cpp b/examples/tool_contact_example.cpp index b28e878d..7d817ba8 100644 --- a/examples/tool_contact_example.cpp +++ b/examples/tool_contact_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 #include #include #include @@ -47,26 +48,9 @@ const std::string SCRIPT_FILE = "resources/external_control.urscript"; const std::string OUTPUT_RECIPE = "examples/resources/rtde_output_recipe.txt"; const std::string INPUT_RECIPE = "examples/resources/rtde_input_recipe.txt"; -std::unique_ptr g_my_driver; -std::unique_ptr g_my_dashboard; +std::unique_ptr g_my_robot; std::atomic g_tool_contact_result_triggered; control::ToolContactResult g_tool_contact_result; -std::atomic g_program_running; -std::condition_variable g_program_running_cv; -std::mutex g_program_running_mutex; - -// 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; - g_program_running = program_running; - if (program_running) - { - std::lock_guard lk(g_program_running_mutex); - g_program_running_cv.notify_one(); - } -} void handleToolContactResult(control::ToolContactResult result) { @@ -76,17 +60,6 @@ void handleToolContactResult(control::ToolContactResult result) g_tool_contact_result_triggered = true; } -bool waitForProgramRunning(int milliseconds = 100) -{ - std::unique_lock 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); @@ -104,55 +77,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 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; - } - - // // Stop program, if there is one running - if (!g_my_dashboard->commandStop()) - { - URCL_LOG_ERROR("Could not send stop program command"); - 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 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 (!waitForProgramRunning(1000)) + bool headless_mode = true; + g_my_robot = std::make_unique(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, headless_mode, + "external_control.urp"); + if (!g_my_robot->isHealthy()) { - std::cout << "Program did not start running. Is the robot in remote control?" << std::endl; + URCL_LOG_ERROR("Something in the robot initialization went wrong. Exiting. Please check the output above."); return 1; } - g_my_driver->registerToolContactResultCallback(&handleToolContactResult); - g_my_driver->startToolContact(); + g_my_robot->ur_driver_->registerToolContactResultCallback(&handleToolContactResult); + g_my_robot->ur_driver_->startToolContact(); // This will move the robot downward in the z direction of the base until a tool contact is detected or seconds_to_run // is reached @@ -162,8 +96,8 @@ int main(int argc, char* argv[]) { // Setting the RobotReceiveTimeout time is for example purposes only. This will make the example running more // reliable on non-realtime systems. Use with caution in productive applications. - bool ret = - g_my_driver->writeJointCommand(tcp_speed, comm::ControlMode::MODE_SPEEDL, RobotReceiveTimeout::millisec(100)); + bool ret = g_my_robot->ur_driver_->writeJointCommand(tcp_speed, comm::ControlMode::MODE_SPEEDL, + RobotReceiveTimeout::millisec(100)); if (!ret) { URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?"); @@ -177,6 +111,6 @@ int main(int argc, char* argv[]) } } URCL_LOG_INFO("Timed out before reaching tool contact."); - g_my_driver->stopControl(); + g_my_robot->ur_driver_->stopControl(); return 0; } diff --git a/examples/trajectory_point_interface.cpp b/examples/trajectory_point_interface.cpp index 19ba0fbe..fd169e9c 100644 --- a/examples/trajectory_point_interface.cpp +++ b/examples/trajectory_point_interface.cpp @@ -32,6 +32,7 @@ #include #include +#include #include "ur_client_library/types.h" #include "ur_client_library/ur/ur_driver.h" #include "ur_client_library/log.h" @@ -43,39 +44,9 @@ const std::string SCRIPT_FILE = "resources/external_control.urscript"; const std::string OUTPUT_RECIPE = "examples/resources/rtde_output_recipe.txt"; const std::string INPUT_RECIPE = "examples/resources/rtde_input_recipe.txt"; -std::unique_ptr g_my_dashboard; -std::unique_ptr g_my_driver; - +std::unique_ptr g_my_robot; std::atomic g_trajectory_done = false; -std::atomic g_program_running; -std::condition_variable g_program_running_cv; -std::mutex g_program_running_mutex; - -bool waitForProgramRunning(int milliseconds = 100) -{ - std::unique_lock 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; -} - -// 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; - g_program_running = program_running; - if (program_running) - { - std::lock_guard lk(g_program_running_mutex); - g_program_running_cv.notify_one(); - } -} - void trajDoneCallback(const urcl::control::TrajectoryResult& result) { URCL_LOG_INFO("Trajectory done with result %s", urcl::control::trajectoryResultToString(result).c_str()); @@ -92,56 +63,17 @@ int main(int argc, char* argv[]) robot_ip = std::string(argv[1]); } - // --------------- INITIALIZATION BEGIN ------------------- - // Making the robot ready for the program by: - // Connect the robot Dashboard - g_my_dashboard.reset(new urcl::DashboardClient(robot_ip)); - if (!g_my_dashboard->connect()) - { - URCL_LOG_ERROR("Could not connect to dashboard"); - return 1; - } - - // // Stop program, if there is one running - if (!g_my_dashboard->commandStop()) - { - URCL_LOG_ERROR("Could not send stop program command"); - 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; - } - - std::unique_ptr tool_comm_setup; - const bool headless = true; - g_my_driver.reset(new urcl::UrDriver(robot_ip, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, &handleRobotProgramState, - headless, std::move(tool_comm_setup))); - if (!waitForProgramRunning(1000)) + bool headless_mode = true; + g_my_robot = std::make_unique(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, headless_mode, + "external_control.urp"); + if (!g_my_robot->isHealthy()) { - URCL_LOG_ERROR("Program did not start running. Is the robot in remote control?"); + URCL_LOG_ERROR("Something in the robot initialization went wrong. Exiting. Please check the output above."); return 1; } // --------------- INITIALIZATION END ------------------- - g_my_driver->registerTrajectoryDoneCallback(&trajDoneCallback); + g_my_robot->ur_driver_->registerTrajectoryDoneCallback(&trajDoneCallback); URCL_LOG_INFO("Running MoveJ motion"); // --------------- MOVEJ TRAJECTORY ------------------- @@ -155,25 +87,25 @@ int main(int argc, char* argv[]) std::vector blend_radii{ 0.1, 0.1 }; // Trajectory execution - g_my_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, - points.size() * 2); + g_my_robot->ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, + points.size() * 2); for (size_t i = 0; i < points.size(); i++) { - g_my_driver->writeTrajectoryPoint(points[i], false, motion_durations[i], blend_radii[i]); + g_my_robot->ur_driver_->writeTrajectoryPoint(points[i], false, motion_durations[i], blend_radii[i]); } // Same motion, but parametrized with acceleration and velocity motion_durations = { 0.0, 0.0 }; for (size_t i = 0; i < points.size(); i++) { - g_my_driver->writeTrajectoryPoint(points[i], accelerations[i], velocities[i], false, motion_durations[i], - blend_radii[i]); + g_my_robot->ur_driver_->writeTrajectoryPoint(points[i], accelerations[i], velocities[i], false, + motion_durations[i], blend_radii[i]); } while (!g_trajectory_done) { std::this_thread::sleep_for(std::chrono::milliseconds(100)); - g_my_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP); + g_my_robot->ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP); } } // --------------- END MOVEJ TRAJECTORY ------------------- @@ -191,26 +123,26 @@ int main(int argc, char* argv[]) std::vector blend_radii{ 0.0, 0.0 }; // Trajectory execution of the path that goes through the points twice. - g_my_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, - points.size() * 2); + g_my_robot->ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, + points.size() * 2); for (size_t i = 0; i < points.size(); i++) { // setting the cartesian parameter makes it interpret the 6d vector as a pose and use movel - g_my_driver->writeTrajectoryPoint(points[i], true, motion_durations[i], blend_radii[i]); + g_my_robot->ur_driver_->writeTrajectoryPoint(points[i], true, motion_durations[i], blend_radii[i]); } // Same motion, but parametrized with acceleration and velocity motion_durations = { 0.0, 0.0 }; for (size_t i = 0; i < points.size(); i++) { - g_my_driver->writeTrajectoryPoint(points[i], accelerations[i], velocities[i], true, motion_durations[i], - blend_radii[i]); + g_my_robot->ur_driver_->writeTrajectoryPoint(points[i], accelerations[i], velocities[i], true, + motion_durations[i], blend_radii[i]); } while (!g_trajectory_done) { std::this_thread::sleep_for(std::chrono::milliseconds(100)); - g_my_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP); + g_my_robot->ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP); } } // --------------- END MOVEL TRAJECTORY ------------------- @@ -230,21 +162,21 @@ int main(int argc, char* argv[]) std::vector motion_durations{ 3.0, 3.0, 3.0, 3.0 }; // Trajectory execution - g_my_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, - positions.size()); + g_my_robot->ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, + positions.size()); for (size_t i = 0; i < positions.size(); i++) { - g_my_driver->writeTrajectorySplinePoint(positions[i], velocities[i], motion_durations[i]); + g_my_robot->ur_driver_->writeTrajectorySplinePoint(positions[i], velocities[i], motion_durations[i]); } while (!g_trajectory_done) { std::this_thread::sleep_for(std::chrono::milliseconds(100)); - g_my_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP); + g_my_robot->ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP); } } // --------------- END SPLINE TRAJECTORY ------------------- - g_my_driver->stopControl(); + g_my_robot->ur_driver_->stopControl(); return 0; } diff --git a/include/ur_client_library/example_robot_wrapper.h b/include/ur_client_library/example_robot_wrapper.h index 297b32fb..1a24fe46 100644 --- a/include/ur_client_library/example_robot_wrapper.h +++ b/include/ur_client_library/example_robot_wrapper.h @@ -83,7 +83,7 @@ class ExampleRobotWrapper * * The robot will be power-cycled once and end up switched on, breaks released. */ - void initializeRobotWithDashboard(); + bool initializeRobotWithDashboard(); /** * @brief Starts RTDE communication with the robot. @@ -157,6 +157,16 @@ class ExampleRobotWrapper */ bool startRobotProgram(const std::string& program_file_name); + /** + * @brief Clear protective stop on the robot. + * + * This will try to clear a protective stop on the robot. If the robot is not in protective stop + * this call will do nothing. + */ + bool clearProtectiveStop(); + + bool isHealthy() const; + std::shared_ptr dashboard_client_; /*!< Dashboard client to interact with the robot */ std::shared_ptr ur_driver_; /*!< UR driver to interact with the robot */ @@ -168,6 +178,8 @@ class ExampleRobotWrapper std::mutex read_package_mutex_; std::unique_ptr data_pkg_; + bool robot_initialized_ = false; + bool program_running_; std::condition_variable program_running_cv_; std::condition_variable program_not_running_cv_; diff --git a/src/example_robot_wrapper.cpp b/src/example_robot_wrapper.cpp index ea3582a4..90da08cf 100644 --- a/src/example_robot_wrapper.cpp +++ b/src/example_robot_wrapper.cpp @@ -30,6 +30,8 @@ #include #include +#include "ur_client_library/exceptions.h" +#include "ur_client_library/log.h" namespace urcl { @@ -45,7 +47,11 @@ ExampleRobotWrapper::ExampleRobotWrapper(const std::string& robot_ip, const std: { URCL_LOG_ERROR("Could not connect to dashboard"); } - initializeRobotWithDashboard(); + + if (!initializeRobotWithDashboard()) + { + throw UrException("Could not initialize robot with dashboard"); + } std::unique_ptr tool_comm_setup; ur_driver_ = @@ -57,6 +63,14 @@ ExampleRobotWrapper::ExampleRobotWrapper(const std::string& robot_ip, const std: { startRobotProgram(autostart_program); } + + if (headless_mode | !std::empty(autostart_program)) + { + if (!waitForProgramRunning(500)) + { + throw UrException("Program did not start running. Is the robot in remote control?"); + } + } } ExampleRobotWrapper::~ExampleRobotWrapper() @@ -66,34 +80,70 @@ ExampleRobotWrapper::~ExampleRobotWrapper() stopConsumingRTDEData(); } } -void ExampleRobotWrapper::initializeRobotWithDashboard() + +bool ExampleRobotWrapper::clearProtectiveStop() +{ + std::string safety_status; + dashboard_client_->commandSafetyStatus(safety_status); + bool is_protective_stopped = safety_status.find("PROTECTIVE_STOP") != std::string::npos; + if (is_protective_stopped) + { + URCL_LOG_INFO("Robot is in protective stop, trying to release it"); + dashboard_client_->commandClosePopup(); + dashboard_client_->commandCloseSafetyPopup(); + if (!dashboard_client_->commandUnlockProtectiveStop()) + { + std::this_thread::sleep_for(std::chrono::seconds(5)); + if (!dashboard_client_->commandUnlockProtectiveStop()) + { + URCL_LOG_ERROR("Could not unlock protective stop"); + return false; + } + } + } + return true; +} + +bool ExampleRobotWrapper::initializeRobotWithDashboard() { - // // Stop program, if there is one running + if (!clearProtectiveStop()) + { + URCL_LOG_ERROR("Could not clear protective stop"); + return false; + } + + // Stop program, if there is one running if (!dashboard_client_->commandStop()) { URCL_LOG_ERROR("Could not send stop program command"); + return false; } // Power it off if (!dashboard_client_->commandPowerOff()) { URCL_LOG_ERROR("Could not send Power off command"); + return false; } // Power it on if (!dashboard_client_->commandPowerOn()) { URCL_LOG_ERROR("Could not send Power on command"); + return false; } // Release the brakes if (!dashboard_client_->commandBrakeRelease()) { URCL_LOG_ERROR("Could not send BrakeRelease command"); + return false; } // Now the robot is ready to receive a program URCL_LOG_INFO("Robot ready to start a program"); + robot_initialized_ = true; + return true; } void ExampleRobotWrapper::handleRobotProgramState(bool program_running) @@ -210,4 +260,19 @@ bool ExampleRobotWrapper::resendRobotProgram() return startRobotProgram(autostart_program_); } +bool ExampleRobotWrapper::isHealthy() const +{ + if (!robot_initialized_) + { + URCL_LOG_ERROR("Robot is not initialized"); + return false; + } + if (!program_running_) + { + URCL_LOG_ERROR("Robot program is not running"); + return false; + } + return true; +} + } // namespace urcl diff --git a/tests/test_instruction_executor.cpp b/tests/test_instruction_executor.cpp index 5d8aeb64..aadbc2b2 100644 --- a/tests/test_instruction_executor.cpp +++ b/tests/test_instruction_executor.cpp @@ -62,17 +62,7 @@ class InstructionExecutorTest : public ::testing::Test void SetUp() override { executor_ = std::make_unique(g_my_robot->ur_driver_); - std::string safety_status; - g_my_robot->dashboard_client_->commandSafetyStatus(safety_status); - bool is_protective_stopped = safety_status.find("PROTECTIVE_STOP") != std::string::npos; - if (is_protective_stopped) - { - // We forced a protective stop above. Some versions require waiting 5 seconds before releasing - // the protective stop. - std::this_thread::sleep_for(std::chrono::seconds(5)); - g_my_robot->dashboard_client_->commandCloseSafetyPopup(); - ASSERT_TRUE(g_my_robot->dashboard_client_->commandUnlockProtectiveStop()); - } + g_my_robot->clearProtectiveStop(); // Make sure script is running on the robot if (!g_my_robot->waitForProgramRunning()) { diff --git a/tests/test_spline_interpolation.cpp b/tests/test_spline_interpolation.cpp index e75d4c51..1d789355 100644 --- a/tests/test_spline_interpolation.cpp +++ b/tests/test_spline_interpolation.cpp @@ -122,6 +122,11 @@ class SplineInterpolationTest : public ::testing::Test void SetUp() { + step_time_ = 0.002; + if (g_my_robot->ur_driver_->getVersion().major < 5) + { + step_time_ = 0.008; + } std::string safety_status; g_my_robot->dashboard_client_->commandSafetyStatus(safety_status); bool is_protective_stopped = safety_status.find("PROTECTIVE_STOP") != std::string::npos; @@ -133,12 +138,7 @@ class SplineInterpolationTest : public ::testing::Test g_my_robot->dashboard_client_->commandCloseSafetyPopup(); ASSERT_TRUE(g_my_robot->dashboard_client_->commandUnlockProtectiveStop()); } - // Make sure script is running on the robot - if (!g_my_robot->waitForProgramRunning()) - { - g_my_robot->resendRobotProgram(); - ASSERT_TRUE(g_my_robot->waitForProgramRunning()); - } + ASSERT_TRUE(g_my_robot->isHealthy()); } void sendIdle() diff --git a/tests/test_ur_driver.cpp b/tests/test_ur_driver.cpp index e1322c6d..3e8c24fe 100644 --- a/tests/test_ur_driver.cpp +++ b/tests/test_ur_driver.cpp @@ -61,12 +61,7 @@ class UrDriverTest : public ::testing::Test void SetUp() { - // Make sure script is running on the robot - if (!g_my_robot->waitForProgramRunning()) - { - g_my_robot->resendRobotProgram(); - ASSERT_TRUE(g_my_robot->waitForProgramRunning()); - } + ASSERT_TRUE(g_my_robot->isHealthy()); } }; @@ -295,4 +290,4 @@ int main(int argc, char* argv[]) } return RUN_ALL_TESTS(); -} \ No newline at end of file +}