From 69674c3e4b3b12bfedeecf2c0e8c8e2ebff00198 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 7 Feb 2025 17:17:08 +0100 Subject: [PATCH 1/6] Use ExampleRobotWrapper for initialization in all examples --- examples/force_mode_example.cpp | 117 ++++++------------------ examples/full_driver.cpp | 92 +++---------------- examples/instruction_executor.cpp | 62 +++---------- examples/spline_example.cpp | 94 +++++-------------- examples/tool_contact_example.cpp | 88 +++--------------- examples/trajectory_point_interface.cpp | 116 +++++------------------ 6 files changed, 112 insertions(+), 457 deletions(-) diff --git a/examples/force_mode_example.cpp b/examples/force_mode_example.cpp index 03faff5da..5c9e091e5 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,11 @@ 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; - } - - // // Stop program, if there is one running - if (!g_my_dashboard->commandStop()) - { - URCL_LOG_ERROR("Could not send stop program command"); - return 1; - } + bool headless_mode = true; + g_my_robot = std::make_unique(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, headless_mode, + "external_control.urp"); - // 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 " @@ -163,7 +96,7 @@ int main(int argc, char* argv[]) } // Make sure that external control script is running - if (!waitForProgramRunning()) + if (!g_my_robot->waitForProgramRunning()) { URCL_LOG_ERROR("External Control script not running."); return 1; @@ -175,22 +108,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 +139,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 +147,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/full_driver.cpp b/examples/full_driver.cpp index 43b077252..4519cbdd0 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,51 +59,11 @@ 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))); + bool headless_mode = true; + g_my_robot = std::make_unique(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, headless_mode, + "external_control.urp"); // Make sure that external control script is running - if (!waitForProgramRunning()) + if (!g_my_robot->waitForProgramRunning()) { URCL_LOG_ERROR("External Control script not running."); return 1; @@ -139,7 +71,7 @@ int main(int argc, char* argv[]) // 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 907667766..7f79d0f44 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->waitForProgramRunning(1000)) { - URCL_LOG_ERROR("Could not send BrakeRelease command"); + std::cout << "Program did not start running. Is the robot in remote control?" << std::endl; 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 9af4240d7..9243aa824 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,25 @@ 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"); + // Make sure that external control script is running + if (!g_my_robot->waitForProgramRunning()) { - URCL_LOG_ERROR("Could not connect to dashboard"); + URCL_LOG_ERROR("External Control script not running."); 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 +163,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 +200,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 +210,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 +231,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 +241,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 +256,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 +265,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 b28e878d8..e058a8866 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->waitForProgramRunning(1000)) { std::cout << "Program did not start running. Is the robot in remote control?" << std::endl; 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 19ba0fbe6..8613f0691 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->waitForProgramRunning(1000)) { URCL_LOG_ERROR("Program did not start running. Is the robot in remote control?"); 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; } From e8e76429c4701bc5e37dc242645f292ded3f0dd4 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Mon, 10 Feb 2025 08:50:38 +0100 Subject: [PATCH 2/6] Update documentation --- doc/examples/force_mode.rst | 7 ++++--- doc/examples/instruction_executor.rst | 6 +++--- doc/examples/tool_contact_example.rst | 10 +++++----- doc/examples/trajectory_point_interface.rst | 10 +++++----- doc/examples/ur_driver.rst | 6 +++--- examples/full_driver.cpp | 1 + 6 files changed, 21 insertions(+), 19 deletions(-) diff --git a/doc/examples/force_mode.rst b/doc/examples/force_mode.rst index 56ab63240..6ee364d3e 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 f4e21ec04..58439972a 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 25cb9a957..d38630b5a 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 5dd58f204..51607f36b 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 a146f198a..874c49220 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/full_driver.cpp b/examples/full_driver.cpp index 4519cbdd0..345ff757d 100644 --- a/examples/full_driver.cpp +++ b/examples/full_driver.cpp @@ -68,6 +68,7 @@ int main(int argc, char* argv[]) URCL_LOG_ERROR("External Control script not running."); return 1; } + // --------------- INITIALIZATION END ------------------- // Increment depends on robot version double increment_constant = 0.0005; From 97213da217a8395563721d9a292498b4a2aeefcc Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Mon, 10 Feb 2025 09:30:27 +0100 Subject: [PATCH 3/6] Move waitForProgramRunning to robot initialization --- examples/force_mode_example.cpp | 11 +++---- examples/freedrive_example.cpp | 4 +-- examples/full_driver.cpp | 5 ++- examples/instruction_executor.cpp | 4 +-- examples/spline_example.cpp | 5 ++- examples/tool_contact_example.cpp | 4 +-- examples/trajectory_point_interface.cpp | 4 +-- .../ur_client_library/example_robot_wrapper.h | 6 +++- src/example_robot_wrapper.cpp | 32 ++++++++++++++++++- tests/test_spline_interpolation.cpp | 7 +--- tests/test_ur_driver.cpp | 9 ++---- 11 files changed, 56 insertions(+), 35 deletions(-) diff --git a/examples/force_mode_example.cpp b/examples/force_mode_example.cpp index 5c9e091e5..47d70f0ea 100644 --- a/examples/force_mode_example.cpp +++ b/examples/force_mode_example.cpp @@ -86,6 +86,11 @@ 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->isHealthy()) + { + URCL_LOG_ERROR("Something in the robot initialization went wrong. Exiting. Please check the output above."); + return 1; + } if (!g_my_robot->ur_driver_->checkCalibration(CALIBRATION_CHECKSUM)) { URCL_LOG_ERROR("Calibration checksum does not match actual robot."); @@ -95,12 +100,6 @@ int main(int argc, char* argv[]) "for details."); } - // Make sure that external control script is running - if (!g_my_robot->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. diff --git a/examples/freedrive_example.cpp b/examples/freedrive_example.cpp index 48c0d8d7f..4d691d0d9 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 345ff757d..900301c88 100644 --- a/examples/full_driver.cpp +++ b/examples/full_driver.cpp @@ -62,10 +62,9 @@ int main(int argc, char* argv[]) bool headless_mode = true; g_my_robot = std::make_unique(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, headless_mode, "external_control.urp"); - // Make sure that external control script is running - 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; } // --------------- INITIALIZATION END ------------------- diff --git a/examples/instruction_executor.cpp b/examples/instruction_executor.cpp index 7f79d0f44..9a55affb9 100644 --- a/examples/instruction_executor.cpp +++ b/examples/instruction_executor.cpp @@ -69,9 +69,9 @@ int main(int argc, char* argv[]) "[https://github.com/UniversalRobots/Universal_Robots_ROS_Driver#extract-calibration-information] " "for details."); } - if (!g_my_robot->waitForProgramRunning(1000)) + 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; } diff --git a/examples/spline_example.cpp b/examples/spline_example.cpp index 9243aa824..3fb5e1d95 100644 --- a/examples/spline_example.cpp +++ b/examples/spline_example.cpp @@ -125,10 +125,9 @@ int main(int argc, char* argv[]) bool headless_mode = true; g_my_robot = std::make_unique(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, headless_mode, "external_control.urp"); - // Make sure that external control script is running - 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/tool_contact_example.cpp b/examples/tool_contact_example.cpp index e058a8866..7d817ba8d 100644 --- a/examples/tool_contact_example.cpp +++ b/examples/tool_contact_example.cpp @@ -80,9 +80,9 @@ int main(int argc, char* argv[]) 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->waitForProgramRunning(1000)) + 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_robot->ur_driver_->registerToolContactResultCallback(&handleToolContactResult); diff --git a/examples/trajectory_point_interface.cpp b/examples/trajectory_point_interface.cpp index 8613f0691..fd169e9cc 100644 --- a/examples/trajectory_point_interface.cpp +++ b/examples/trajectory_point_interface.cpp @@ -66,9 +66,9 @@ int main(int argc, char* argv[]) 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->waitForProgramRunning(1000)) + 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 ------------------- diff --git a/include/ur_client_library/example_robot_wrapper.h b/include/ur_client_library/example_robot_wrapper.h index 297b32fb7..4581e56d8 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,8 @@ class ExampleRobotWrapper */ bool startRobotProgram(const std::string& program_file_name); + 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 +170,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 ea3582a4a..698eed5f4 100644 --- a/src/example_robot_wrapper.cpp +++ b/src/example_robot_wrapper.cpp @@ -57,6 +57,14 @@ ExampleRobotWrapper::ExampleRobotWrapper(const std::string& robot_ip, const std: { startRobotProgram(autostart_program); } + + if (headless_mode | !std::empty(autostart_program)) + { + if (!waitForProgramRunning(500)) + { + URCL_LOG_ERROR("Program did not start running. Is the robot in remote control?"); + } + } } ExampleRobotWrapper::~ExampleRobotWrapper() @@ -66,34 +74,41 @@ ExampleRobotWrapper::~ExampleRobotWrapper() stopConsumingRTDEData(); } } -void ExampleRobotWrapper::initializeRobotWithDashboard() + +bool ExampleRobotWrapper::initializeRobotWithDashboard() { // // 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 +225,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_spline_interpolation.cpp b/tests/test_spline_interpolation.cpp index e75d4c510..8e8e4f14c 100644 --- a/tests/test_spline_interpolation.cpp +++ b/tests/test_spline_interpolation.cpp @@ -133,12 +133,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 e1322c6d1..3e8c24fe1 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 +} From b93c730fd0219e1d05786c1796911ffa0b3b6368 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Mon, 10 Feb 2025 09:58:46 +0100 Subject: [PATCH 4/6] Fix: set step time That was a regression introduced in cb79da817f3441170662d127204b22cf4871df24 --- tests/test_spline_interpolation.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/tests/test_spline_interpolation.cpp b/tests/test_spline_interpolation.cpp index 8e8e4f14c..1d7893553 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; From b43ef46bd606bfcb808dc8faa1a66dbe8ba346ee Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Mon, 10 Feb 2025 12:59:50 +0100 Subject: [PATCH 5/6] Throw an exception on failed driver initialization --- src/example_robot_wrapper.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/example_robot_wrapper.cpp b/src/example_robot_wrapper.cpp index 698eed5f4..c76091422 100644 --- a/src/example_robot_wrapper.cpp +++ b/src/example_robot_wrapper.cpp @@ -30,6 +30,7 @@ #include #include +#include "ur_client_library/exceptions.h" namespace urcl { @@ -45,7 +46,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_ = @@ -62,7 +67,7 @@ ExampleRobotWrapper::ExampleRobotWrapper(const std::string& robot_ip, const std: { if (!waitForProgramRunning(500)) { - URCL_LOG_ERROR("Program did not start running. Is the robot in remote control?"); + throw UrException("Program did not start running. Is the robot in remote control?"); } } } From 236a6699947311183dfd9bd0b45b5ba29f1d083a Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Mon, 10 Feb 2025 13:43:05 +0100 Subject: [PATCH 6/6] Check for protective stop in ExampleRobotWrapper initialization --- .../ur_client_library/example_robot_wrapper.h | 8 +++++ src/example_robot_wrapper.cpp | 32 ++++++++++++++++++- tests/test_instruction_executor.cpp | 12 +------ 3 files changed, 40 insertions(+), 12 deletions(-) diff --git a/include/ur_client_library/example_robot_wrapper.h b/include/ur_client_library/example_robot_wrapper.h index 4581e56d8..1a24fe466 100644 --- a/include/ur_client_library/example_robot_wrapper.h +++ b/include/ur_client_library/example_robot_wrapper.h @@ -157,6 +157,14 @@ 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 */ diff --git a/src/example_robot_wrapper.cpp b/src/example_robot_wrapper.cpp index c76091422..90da08cf2 100644 --- a/src/example_robot_wrapper.cpp +++ b/src/example_robot_wrapper.cpp @@ -31,6 +31,7 @@ #include #include #include "ur_client_library/exceptions.h" +#include "ur_client_library/log.h" namespace urcl { @@ -80,9 +81,38 @@ ExampleRobotWrapper::~ExampleRobotWrapper() } } +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"); diff --git a/tests/test_instruction_executor.cpp b/tests/test_instruction_executor.cpp index 5d8aeb641..aadbc2b27 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()) {