diff --git a/.clang-tidy b/.clang-tidy index d935e37ea..12b8494a4 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -3,6 +3,7 @@ CheckOptions: - { key: readability-identifier-naming.NamespaceCase, value: lower_case } - { key: readability-identifier-naming.ClassCase, value: CamelCase } - { key: readability-identifier-naming.PrivateMemberSuffix, value: _ } + - { key: readability-identifier-naming.PublicMemberSuffix, value: "" } - { key: readability-identifier-naming.StructCase, value: CamelCase } - { key: readability-identifier-naming.FunctionCase, value: camelBack } - { key: readability-identifier-naming.VariableCase, value: lower_case } diff --git a/.github/workflows/industrial-ci.yml b/.github/workflows/industrial-ci.yml index 09bee9f44..c87fa5b8f 100644 --- a/.github/workflows/industrial-ci.yml +++ b/.github/workflows/industrial-ci.yml @@ -35,5 +35,5 @@ jobs: DOWNSTREAM_WORKSPACE: ${{matrix.ROS_DISTRO.DOWNSTREAM_WORKSPACE}} ROS_DISTRO: ${{matrix.ROS_DISTRO.NAME}} ROS_REPO: ${{matrix.ROS_REPO}} - CLANG_TIDY: true + CLANG_TIDY: pedantic CLANG_TIDY_ARGS: '--extra-arg=-std=c++17' # needed for Humble diff --git a/doc/examples/instruction_executor.rst b/doc/examples/instruction_executor.rst index 58439972a..46dd19f13 100644 --- a/doc/examples/instruction_executor.rst +++ b/doc/examples/instruction_executor.rst @@ -21,7 +21,7 @@ The `instruction_executor.cpp (g_my_robot->ur_driver_); + :end-at: auto instruction_executor = std::make_shared(g_my_robot->getUrDriver()); 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_robot->ur_driver_->stopControl(); + :end-before: g_my_robot->getUrDriver()->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 d38630b5a..abbf2a4c3 100644 --- a/doc/examples/tool_contact_example.rst +++ b/doc/examples/tool_contact_example.rst @@ -20,7 +20,7 @@ At first, we create a initialize a driver as usual: :linenos: :lineno-match: :start-at: bool headless_mode = true; - :end-before: g_my_robot->ur_driver_->registerToolContactResultCallback + :end-before: g_my_robot->getUrDriver()->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_robot->ur_driver_->registerToolContactResultCallback(&handleToolContactResult); - :end-at: g_my_robot->ur_driver_->startToolContact() + :start-at: g_my_robot->getUrDriver()->registerToolContactResultCallback(&handleToolContactResult); + :end-at: g_my_robot->getUrDriver()->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 51607f36b..266e69f08 100644 --- a/doc/examples/trajectory_point_interface.rst +++ b/doc/examples/trajectory_point_interface.rst @@ -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_robot->ur_driver_->registerTrajectoryDoneCallback(&trajDoneCallback); - :end-at: g_my_robot->ur_driver_->registerTrajectoryDoneCallback(&trajDoneCallback); + :start-at: g_my_robot->getUrDriver()->registerTrajectoryDoneCallback(&trajDoneCallback); + :end-at: g_my_robot->getUrDriver()->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_robot->ur_driver_->writeTrajectoryPoint()`` calls will be translated into a +above, each of the ``g_my_robot->getUrDriver()->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_robot->ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP);`` +``g_my_robot->getUrDriver()->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP);`` call. MoveL Trajectory diff --git a/doc/index.rst b/doc/index.rst index 0aa88d58d..f6eaca267 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -15,3 +15,4 @@ can be found on `GitHub ``isControlModeRealtime`` + - ``is_control_mode_non_realtime`` -> ``isControlModeNonRealtime`` + +- In ``urcl::RobotReceiveTimeout`` the ``timeout_`` member is now private. Use + ``getAsMilliseconds()`` to access it. + +- In ``urcl::UrDriverConfiguration`` two members have been renamed: + + - ``rtde_initialization_attempts_`` -> ``rtde_initialization_attempts`` + - ``rtde_initialization_timeout_`` -> ``rtde_initialization_timeout`` diff --git a/examples/force_mode_example.cpp b/examples/force_mode_example.cpp index 47d70f0ea..eb830d596 100644 --- a/examples/force_mode_example.cpp +++ b/examples/force_mode_example.cpp @@ -57,7 +57,7 @@ std::unique_ptr g_my_robot; void sendFreedriveMessageOrDie(const control::FreedriveControlMessage freedrive_action) { - bool ret = g_my_robot->ur_driver_->writeFreedriveControlMessage(freedrive_action); + bool ret = g_my_robot->getUrDriver()->writeFreedriveControlMessage(freedrive_action); if (!ret) { URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?"); @@ -91,7 +91,7 @@ int main(int argc, char* argv[]) 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)) + if (!g_my_robot->getUrDriver()->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 " @@ -107,24 +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_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. + if (g_my_robot->getUrDriver()->getVersion().major < 5) + success = g_my_robot->getUrDriver()->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_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. + success = g_my_robot->getUrDriver()->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) { @@ -138,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_robot->ur_driver_->writeKeepalive(); + g_my_robot->getUrDriver()->writeKeepalive(); stopwatch_now = std::chrono::steady_clock::now(); time_done += stopwatch_now - stopwatch_last; @@ -146,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_robot->ur_driver_->endForceMode(); + g_my_robot->getUrDriver()->endForceMode(); } diff --git a/examples/freedrive_example.cpp b/examples/freedrive_example.cpp index 4d691d0d9..e03e46aff 100644 --- a/examples/freedrive_example.cpp +++ b/examples/freedrive_example.cpp @@ -55,7 +55,7 @@ std::unique_ptr g_my_robot; void sendFreedriveMessageOrDie(const control::FreedriveControlMessage freedrive_action) { - bool ret = g_my_robot->ur_driver_->writeFreedriveControlMessage(freedrive_action); + bool ret = g_my_robot->getUrDriver()->writeFreedriveControlMessage(freedrive_action); if (!ret) { URCL_LOG_ERROR("Could not send joint command. Is there an external_control program running on the robot?"); diff --git a/examples/full_driver.cpp b/examples/full_driver.cpp index 900301c88..d123b5237 100644 --- a/examples/full_driver.cpp +++ b/examples/full_driver.cpp @@ -71,7 +71,7 @@ int main(int argc, char* argv[]) // Increment depends on robot version double increment_constant = 0.0005; - if (g_my_robot->ur_driver_->getVersion().major < 5) + if (g_my_robot->getUrDriver()->getVersion().major < 5) { increment_constant = 0.002; } @@ -86,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_robot->ur_driver_->startRTDECommunication(); + g_my_robot->getUrDriver()->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_robot->ur_driver_->getDataPackage(); + std::unique_ptr data_pkg = g_my_robot->getUrDriver()->getDataPackage(); if (!data_pkg) { URCL_LOG_WARN("Could not get fresh data package from robot"); @@ -133,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_robot->ur_driver_->writeJointCommand(joint_target, comm::ControlMode::MODE_SERVOJ, - RobotReceiveTimeout::millisec(100)); + bool ret = g_my_robot->getUrDriver()->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?"); @@ -142,7 +142,7 @@ int main(int argc, char* argv[]) } URCL_LOG_DEBUG("data_pkg:\n%s", data_pkg->toString().c_str()); } - g_my_robot->ur_driver_->stopControl(); + g_my_robot->getUrDriver()->stopControl(); URCL_LOG_INFO("Movement done"); return 0; } diff --git a/examples/instruction_executor.cpp b/examples/instruction_executor.cpp index 9a55affb9..0bed7d708 100644 --- a/examples/instruction_executor.cpp +++ b/examples/instruction_executor.cpp @@ -61,7 +61,7 @@ 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->ur_driver_->checkCalibration(CALIBRATION_CHECKSUM)) + if (!g_my_robot->getUrDriver()->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 " @@ -75,7 +75,7 @@ int main(int argc, char* argv[]) return 1; } - auto instruction_executor = std::make_shared(g_my_robot->ur_driver_); + auto instruction_executor = std::make_shared(g_my_robot->getUrDriver()); // --------------- INITIALIZATION END ------------------- URCL_LOG_INFO("Running motion"); @@ -105,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_robot->ur_driver_->stopControl(); + g_my_robot->getUrDriver()->stopControl(); return 0; } diff --git a/examples/spline_example.cpp b/examples/spline_example.cpp index 3fb5e1d95..a29b26772 100644 --- a/examples/spline_example.cpp +++ b/examples/spline_example.cpp @@ -57,31 +57,31 @@ void sendTrajectory(const std::vector& p_p, const std::vectorur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, - p_p.size()); + g_my_robot->getUrDriver()->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_robot->ur_driver_->writeTrajectoryPoint(p_p[i], false, time[i]); + g_my_robot->getUrDriver()->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_robot->ur_driver_->writeTrajectorySplinePoint(p_p[i], p_v[i], p_a[i], time[i]); + g_my_robot->getUrDriver()->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_robot->ur_driver_->writeTrajectorySplinePoint(p_p[i], p_v[i], time[i]); + g_my_robot->getUrDriver()->writeTrajectorySplinePoint(p_p[i], p_v[i], time[i]); } else { - g_my_robot->ur_driver_->writeTrajectorySplinePoint(p_p[i], time[i]); + g_my_robot->getUrDriver()->writeTrajectorySplinePoint(p_p[i], time[i]); } } } @@ -131,15 +131,15 @@ int main(int argc, char* argv[]) return 1; } - g_my_robot->ur_driver_->registerTrajectoryDoneCallback(&handleTrajectoryState); + g_my_robot->getUrDriver()->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_robot->ur_driver_->startRTDECommunication(); + g_my_robot->getUrDriver()->startRTDECommunication(); - std::unique_ptr data_pkg = g_my_robot->ur_driver_->getDataPackage(); + std::unique_ptr data_pkg = g_my_robot->getUrDriver()->getDataPackage(); if (data_pkg) { @@ -162,7 +162,7 @@ int main(int argc, char* argv[]) 4.00000000e+00 }; bool ret = false; - ret = g_my_robot->ur_driver_->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_NOOP); + ret = g_my_robot->getUrDriver()->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_NOOP); if (!ret) { std::stringstream lastq; @@ -199,7 +199,7 @@ int main(int argc, char* argv[]) g_trajectory_running = true; while (g_trajectory_running) { - std::unique_ptr data_pkg = g_my_robot->ur_driver_->getDataPackage(); + std::unique_ptr data_pkg = g_my_robot->getUrDriver()->getDataPackage(); if (data_pkg) { // Read current joint positions from robot data @@ -209,7 +209,8 @@ 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_robot->ur_driver_->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_NOOP); + ret = + g_my_robot->getUrDriver()->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_NOOP); if (!ret) { @@ -230,7 +231,7 @@ int main(int argc, char* argv[]) g_trajectory_running = true; while (g_trajectory_running) { - std::unique_ptr data_pkg = g_my_robot->ur_driver_->getDataPackage(); + std::unique_ptr data_pkg = g_my_robot->getUrDriver()->getDataPackage(); if (data_pkg) { // Read current joint positions from robot data @@ -240,7 +241,8 @@ 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_robot->ur_driver_->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_NOOP); + ret = + g_my_robot->getUrDriver()->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_NOOP); if (!ret) { @@ -255,7 +257,7 @@ int main(int argc, char* argv[]) URCL_LOG_INFO("QUINTIC Movement done"); - ret = g_my_robot->ur_driver_->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_NOOP); + ret = g_my_robot->getUrDriver()->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_NOOP); if (!ret) { std::stringstream lastq; @@ -264,6 +266,6 @@ int main(int argc, char* argv[]) lastq.str().c_str()); return 1; } - g_my_robot->ur_driver_->stopControl(); + g_my_robot->getUrDriver()->stopControl(); return 0; } diff --git a/examples/tool_contact_example.cpp b/examples/tool_contact_example.cpp index 7d817ba8d..d16d399e0 100644 --- a/examples/tool_contact_example.cpp +++ b/examples/tool_contact_example.cpp @@ -85,8 +85,8 @@ int main(int argc, char* argv[]) 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); - g_my_robot->ur_driver_->startToolContact(); + g_my_robot->getUrDriver()->registerToolContactResultCallback(&handleToolContactResult); + g_my_robot->getUrDriver()->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 @@ -96,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_robot->ur_driver_->writeJointCommand(tcp_speed, comm::ControlMode::MODE_SPEEDL, - RobotReceiveTimeout::millisec(100)); + bool ret = g_my_robot->getUrDriver()->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?"); @@ -111,6 +111,6 @@ int main(int argc, char* argv[]) } } URCL_LOG_INFO("Timed out before reaching tool contact."); - g_my_robot->ur_driver_->stopControl(); + g_my_robot->getUrDriver()->stopControl(); return 0; } diff --git a/examples/trajectory_point_interface.cpp b/examples/trajectory_point_interface.cpp index fd169e9cc..7e2efe18b 100644 --- a/examples/trajectory_point_interface.cpp +++ b/examples/trajectory_point_interface.cpp @@ -73,7 +73,7 @@ int main(int argc, char* argv[]) } // --------------- INITIALIZATION END ------------------- - g_my_robot->ur_driver_->registerTrajectoryDoneCallback(&trajDoneCallback); + g_my_robot->getUrDriver()->registerTrajectoryDoneCallback(&trajDoneCallback); URCL_LOG_INFO("Running MoveJ motion"); // --------------- MOVEJ TRAJECTORY ------------------- @@ -87,25 +87,26 @@ int main(int argc, char* argv[]) std::vector blend_radii{ 0.1, 0.1 }; // Trajectory execution - g_my_robot->ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, - points.size() * 2); + g_my_robot->getUrDriver()->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, + points.size() * 2); for (size_t i = 0; i < points.size(); i++) { - g_my_robot->ur_driver_->writeTrajectoryPoint(points[i], false, motion_durations[i], blend_radii[i]); + g_my_robot->getUrDriver()->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_robot->ur_driver_->writeTrajectoryPoint(points[i], accelerations[i], velocities[i], false, - motion_durations[i], blend_radii[i]); + g_my_robot->getUrDriver()->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_robot->ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP); + g_my_robot->getUrDriver()->writeTrajectoryControlMessage( + urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP); } } // --------------- END MOVEJ TRAJECTORY ------------------- @@ -123,26 +124,27 @@ 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_robot->ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, - points.size() * 2); + g_my_robot->getUrDriver()->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_robot->ur_driver_->writeTrajectoryPoint(points[i], true, motion_durations[i], blend_radii[i]); + g_my_robot->getUrDriver()->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_robot->ur_driver_->writeTrajectoryPoint(points[i], accelerations[i], velocities[i], true, - motion_durations[i], blend_radii[i]); + g_my_robot->getUrDriver()->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_robot->ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP); + g_my_robot->getUrDriver()->writeTrajectoryControlMessage( + urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP); } } // --------------- END MOVEL TRAJECTORY ------------------- @@ -162,21 +164,22 @@ int main(int argc, char* argv[]) std::vector motion_durations{ 3.0, 3.0, 3.0, 3.0 }; // Trajectory execution - g_my_robot->ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, - positions.size()); + g_my_robot->getUrDriver()->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, + positions.size()); for (size_t i = 0; i < positions.size(); i++) { - g_my_robot->ur_driver_->writeTrajectorySplinePoint(positions[i], velocities[i], motion_durations[i]); + g_my_robot->getUrDriver()->writeTrajectorySplinePoint(positions[i], velocities[i], motion_durations[i]); } while (!g_trajectory_done) { std::this_thread::sleep_for(std::chrono::milliseconds(100)); - g_my_robot->ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP); + g_my_robot->getUrDriver()->writeTrajectoryControlMessage( + urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP); } } // --------------- END SPLINE TRAJECTORY ------------------- - g_my_robot->ur_driver_->stopControl(); + g_my_robot->getUrDriver()->stopControl(); return 0; } diff --git a/include/ur_client_library/comm/control_mode.h b/include/ur_client_library/comm/control_mode.h index 70b4b8c3d..62dd706fb 100644 --- a/include/ur_client_library/comm/control_mode.h +++ b/include/ur_client_library/comm/control_mode.h @@ -83,7 +83,7 @@ class ControlModeTypes * * \returns true if the control mode is realtime, false otherwise */ - static bool is_control_mode_realtime(ControlMode control_mode) + static bool isControlModeRealtime(ControlMode control_mode) { return (std::find(ControlModeTypes::REALTIME_CONTROL_MODES.begin(), ControlModeTypes::REALTIME_CONTROL_MODES.end(), control_mode) != ControlModeTypes::REALTIME_CONTROL_MODES.end()); @@ -96,7 +96,7 @@ class ControlModeTypes * * \returns true if the control mode is non realtime, false otherwise */ - static bool is_control_mode_non_realtime(ControlMode control_mode) + static bool isControlModeNonRealtime(ControlMode control_mode) { return (std::find(ControlModeTypes::NON_REALTIME_CONTROL_MODES.begin(), ControlModeTypes::NON_REALTIME_CONTROL_MODES.end(), diff --git a/include/ur_client_library/example_robot_wrapper.h b/include/ur_client_library/example_robot_wrapper.h index 2053130d7..36831a121 100644 --- a/include/ur_client_library/example_robot_wrapper.h +++ b/include/ur_client_library/example_robot_wrapper.h @@ -169,6 +169,22 @@ class ExampleRobotWrapper bool isHealthy() const; + std::shared_ptr getDashboardClient() const + { + return dashboard_client_; + }; + std::shared_ptr getPrimaryClient() const + { + return primary_client_; + }; + std::shared_ptr getUrDriver() const + { + return ur_driver_; + }; + +private: + void handleRobotProgramState(bool program_running); + //! Dashboard client to interact with the robot std::shared_ptr dashboard_client_; @@ -178,9 +194,6 @@ class ExampleRobotWrapper //! UR driver to interact with the robot std::shared_ptr ur_driver_; -private: - void handleRobotProgramState(bool program_running); - comm::INotifier notifier_; std::atomic rtde_communication_started_ = false; diff --git a/include/ur_client_library/primary/.clang-tidy b/include/ur_client_library/primary/.clang-tidy new file mode 100644 index 000000000..d935e37ea --- /dev/null +++ b/include/ur_client_library/primary/.clang-tidy @@ -0,0 +1,10 @@ +Checks: '-*,readability-identifier-naming' +CheckOptions: + - { key: readability-identifier-naming.NamespaceCase, value: lower_case } + - { key: readability-identifier-naming.ClassCase, value: CamelCase } + - { key: readability-identifier-naming.PrivateMemberSuffix, value: _ } + - { key: readability-identifier-naming.StructCase, value: CamelCase } + - { key: readability-identifier-naming.FunctionCase, value: camelBack } + - { key: readability-identifier-naming.VariableCase, value: lower_case } + - { key: readability-identifier-naming.GlobalVariablePrefix, value: g_ } + - { key: readability-identifier-naming.GlobalConstantCase, value: UPPER_CASE } diff --git a/include/ur_client_library/queue/.clang-tidy b/include/ur_client_library/queue/.clang-tidy new file mode 100644 index 000000000..2f122e331 --- /dev/null +++ b/include/ur_client_library/queue/.clang-tidy @@ -0,0 +1,2 @@ +# Disable all checks in this folder. +Checks: '-*' diff --git a/include/ur_client_library/rtde/.clang-tidy b/include/ur_client_library/rtde/.clang-tidy new file mode 100644 index 000000000..d935e37ea --- /dev/null +++ b/include/ur_client_library/rtde/.clang-tidy @@ -0,0 +1,10 @@ +Checks: '-*,readability-identifier-naming' +CheckOptions: + - { key: readability-identifier-naming.NamespaceCase, value: lower_case } + - { key: readability-identifier-naming.ClassCase, value: CamelCase } + - { key: readability-identifier-naming.PrivateMemberSuffix, value: _ } + - { key: readability-identifier-naming.StructCase, value: CamelCase } + - { key: readability-identifier-naming.FunctionCase, value: camelBack } + - { key: readability-identifier-naming.VariableCase, value: lower_case } + - { key: readability-identifier-naming.GlobalVariablePrefix, value: g_ } + - { key: readability-identifier-naming.GlobalConstantCase, value: UPPER_CASE } diff --git a/include/ur_client_library/ur/robot_receive_timeout.h b/include/ur_client_library/ur/robot_receive_timeout.h index 1a78bc3a5..742063315 100644 --- a/include/ur_client_library/ur/robot_receive_timeout.h +++ b/include/ur_client_library/ur/robot_receive_timeout.h @@ -87,10 +87,14 @@ class RobotReceiveTimeout */ int verifyRobotReceiveTimeout(const comm::ControlMode control_mode, const std::chrono::milliseconds step_time) const; - std::chrono::milliseconds timeout_; + std::chrono::milliseconds getAsMilliseconds() const + { + return timeout_; + } private: + std::chrono::milliseconds timeout_; RobotReceiveTimeout(std::chrono::milliseconds timeout); }; -} // namespace urcl \ No newline at end of file +} // namespace urcl diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index 04ec58c28..efc040c7d 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -125,12 +125,12 @@ struct UrDriverConfiguration * * If set to 0, the driver will try to initialize the RTDE interface indefinitely. */ - size_t rtde_initialization_attempts_ = 3; + size_t rtde_initialization_attempts = 3; /*! * \brief Time in between initialization attempts of the RTDE interface. */ - std::chrono::milliseconds rtde_initialization_timeout_ = std::chrono::seconds(5); + std::chrono::milliseconds rtde_initialization_timeout = std::chrono::seconds(5); bool non_blocking_read = false; diff --git a/src/ur/robot_receive_timeout.cpp b/src/ur/robot_receive_timeout.cpp index b7188ff5a..1ce10d139 100644 --- a/src/ur/robot_receive_timeout.cpp +++ b/src/ur/robot_receive_timeout.cpp @@ -64,7 +64,7 @@ RobotReceiveTimeout RobotReceiveTimeout::off() int RobotReceiveTimeout::verifyRobotReceiveTimeout(const comm::ControlMode control_mode, const std::chrono::milliseconds step_time) const { - if (comm::ControlModeTypes::is_control_mode_non_realtime(control_mode)) + if (comm::ControlModeTypes::isControlModeNonRealtime(control_mode)) { if (timeout_ < step_time && timeout_ > std::chrono::milliseconds(0)) { @@ -79,7 +79,7 @@ int RobotReceiveTimeout::verifyRobotReceiveTimeout(const comm::ControlMode contr return timeout_.count(); } } - else if (comm::ControlModeTypes::is_control_mode_realtime(control_mode)) + else if (comm::ControlModeTypes::isControlModeRealtime(control_mode)) { if (timeout_ < step_time) { diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index ff9841b3d..56c693b42 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -62,8 +62,8 @@ void UrDriver::init(const UrDriverConfiguration& config) in_headless_mode_ = config.headless_mode; socket_connection_attempts_ = config.socket_reconnect_attempts; socket_reconnection_timeout_ = config.socket_reconnection_timeout; - rtde_initialization_attempts_ = config.rtde_initialization_attempts_; - rtde_initialization_timeout_ = config.rtde_initialization_timeout_; + rtde_initialization_attempts_ = config.rtde_initialization_attempts; + rtde_initialization_timeout_ = config.rtde_initialization_timeout; URCL_LOG_DEBUG("Initializing urdriver"); URCL_LOG_DEBUG("Initializing RTDE client"); diff --git a/tests/test_control_mode.cpp b/tests/test_control_mode.cpp index 32386ecb2..287311bf4 100644 --- a/tests/test_control_mode.cpp +++ b/tests/test_control_mode.cpp @@ -39,7 +39,7 @@ TEST(control_mode, is_control_mode_realtime_with_realtime_control_modes) { for (unsigned int i = 0; i < comm::ControlModeTypes::REALTIME_CONTROL_MODES.size(); ++i) { - EXPECT_TRUE(comm::ControlModeTypes::is_control_mode_realtime(comm::ControlModeTypes::REALTIME_CONTROL_MODES[i])); + EXPECT_TRUE(comm::ControlModeTypes::isControlModeRealtime(comm::ControlModeTypes::REALTIME_CONTROL_MODES[i])); } } @@ -47,13 +47,12 @@ TEST(control_mode, is_control_mode_realtime_with_non_realtime_control_modes) { for (unsigned int i = 0; i < comm::ControlModeTypes::NON_REALTIME_CONTROL_MODES.size(); ++i) { - EXPECT_FALSE( - comm::ControlModeTypes::is_control_mode_realtime(comm::ControlModeTypes::NON_REALTIME_CONTROL_MODES[i])); + EXPECT_FALSE(comm::ControlModeTypes::isControlModeRealtime(comm::ControlModeTypes::NON_REALTIME_CONTROL_MODES[i])); } for (unsigned int i = 0; i < comm::ControlModeTypes::STATIONARY_CONTROL_MODES.size(); ++i) { - EXPECT_FALSE(comm::ControlModeTypes::is_control_mode_realtime(comm::ControlModeTypes::STATIONARY_CONTROL_MODES[i])); + EXPECT_FALSE(comm::ControlModeTypes::isControlModeRealtime(comm::ControlModeTypes::STATIONARY_CONTROL_MODES[i])); } } @@ -62,7 +61,7 @@ TEST(control_mode, is_control_mode_non_realtime_with_non_realtime_control_modes) for (unsigned int i = 0; i < comm::ControlModeTypes::NON_REALTIME_CONTROL_MODES.size(); ++i) { EXPECT_TRUE( - comm::ControlModeTypes::is_control_mode_non_realtime(comm::ControlModeTypes::NON_REALTIME_CONTROL_MODES[i])); + comm::ControlModeTypes::isControlModeNonRealtime(comm::ControlModeTypes::NON_REALTIME_CONTROL_MODES[i])); } } @@ -70,13 +69,11 @@ TEST(control_mode, is_control_mode_non_realtime_with_realtime_control_modes) { for (unsigned int i = 0; i < comm::ControlModeTypes::REALTIME_CONTROL_MODES.size(); ++i) { - EXPECT_FALSE( - comm::ControlModeTypes::is_control_mode_non_realtime(comm::ControlModeTypes::REALTIME_CONTROL_MODES[i])); + EXPECT_FALSE(comm::ControlModeTypes::isControlModeNonRealtime(comm::ControlModeTypes::REALTIME_CONTROL_MODES[i])); } for (unsigned int i = 0; i < comm::ControlModeTypes::STATIONARY_CONTROL_MODES.size(); ++i) { - EXPECT_FALSE( - comm::ControlModeTypes::is_control_mode_non_realtime(comm::ControlModeTypes::STATIONARY_CONTROL_MODES[i])); + EXPECT_FALSE(comm::ControlModeTypes::isControlModeNonRealtime(comm::ControlModeTypes::STATIONARY_CONTROL_MODES[i])); } } diff --git a/tests/test_instruction_executor.cpp b/tests/test_instruction_executor.cpp index 225cffe32..df4577149 100644 --- a/tests/test_instruction_executor.cpp +++ b/tests/test_instruction_executor.cpp @@ -46,7 +46,7 @@ const std::string SCRIPT_FILE = "../resources/external_control.urscript"; const std::string OUTPUT_RECIPE = "resources/rtde_output_recipe.txt"; const std::string INPUT_RECIPE = "resources/rtde_input_recipe.txt"; const std::string CALIBRATION_CHECKSUM = "calib_12788084448423163542"; -std::string ROBOT_IP = "192.168.56.101"; +std::string g_ROBOT_IP = "192.168.56.101"; bool g_HEADLESS = true; std::unique_ptr g_my_robot; @@ -58,17 +58,17 @@ class InstructionExecutorTest : public ::testing::Test static void SetUpTestSuite() { - if (!(robotVersionLessThan(ROBOT_IP, "10.0.0") || g_HEADLESS)) + if (!(robotVersionLessThan(g_ROBOT_IP, "10.0.0") || g_HEADLESS)) { GTEST_SKIP_("Running URCap tests for PolyScope X is currently not supported."); } // Setup driver - g_my_robot = std::make_unique(ROBOT_IP, OUTPUT_RECIPE, INPUT_RECIPE, g_HEADLESS, + g_my_robot = std::make_unique(g_ROBOT_IP, OUTPUT_RECIPE, INPUT_RECIPE, g_HEADLESS, "external_control.urp", SCRIPT_FILE); } void SetUp() override { - executor_ = std::make_unique(g_my_robot->ur_driver_); + executor_ = std::make_unique(g_my_robot->getUrDriver()); g_my_robot->clearProtectiveStop(); // Make sure script is running on the robot if (!g_my_robot->waitForProgramRunning()) @@ -79,9 +79,9 @@ class InstructionExecutorTest : public ::testing::Test } void TearDown() override { - g_my_robot->ur_driver_->stopControl(); + g_my_robot->getUrDriver()->stopControl(); g_my_robot->waitForProgramNotRunning(1000); - while (g_my_robot->ur_driver_->isTrajectoryInterfaceConnected()) + while (g_my_robot->getUrDriver()->isTrajectoryInterfaceConnected()) { std::this_thread::sleep_for(std::chrono::milliseconds(1)); } @@ -191,7 +191,7 @@ TEST_F(InstructionExecutorTest, execute_movel_success) TEST_F(InstructionExecutorTest, sending_commands_without_reverse_interface_connected_fails) { - g_my_robot->primary_client_->commandStop(); + g_my_robot->getPrimaryClient()->commandStop(); ASSERT_TRUE(g_my_robot->waitForProgramNotRunning(1000)); ASSERT_FALSE(executor_->moveJ({ -1.57, -1.6, 1.6, -0.7, 0.7, 0.2 })); @@ -219,7 +219,7 @@ TEST_F(InstructionExecutorTest, sending_commands_without_reverse_interface_conne auto motion_thread = std::thread([&]() { ASSERT_FALSE(executor_->moveJ({ -1.57, -1.6, 1.6, -0.7, 0.7, 0.2 })); }); std::this_thread::sleep_for(std::chrono::milliseconds(300)); - g_my_robot->primary_client_->commandStop(); + g_my_robot->getPrimaryClient()->commandStop(); ASSERT_TRUE(g_my_robot->waitForProgramNotRunning(1000)); motion_thread.join(); } @@ -249,7 +249,7 @@ TEST_F(InstructionExecutorTest, canceling_without_running_trajectory_returns_fal TEST(InstructionExecutorTestStandalone, canceling_without_receiving_answer_returns_false) { - if (!(robotVersionLessThan(ROBOT_IP, "10.0.0") || g_HEADLESS)) + if (!(robotVersionLessThan(g_ROBOT_IP, "10.0.0") || g_HEADLESS)) { GTEST_SKIP_("Running URCap tests for PolyScope X is currently not supported."); } @@ -269,9 +269,9 @@ TEST(InstructionExecutorTestStandalone, canceling_without_receiving_answer_retur } } out_file.close(); - auto my_robot = std::make_unique(ROBOT_IP, OUTPUT_RECIPE, INPUT_RECIPE, g_HEADLESS, + auto my_robot = std::make_unique(g_ROBOT_IP, OUTPUT_RECIPE, INPUT_RECIPE, g_HEADLESS, "external_control.urp", test_script_file); - auto executor = std::make_unique(my_robot->ur_driver_); + auto executor = std::make_unique(my_robot->getUrDriver()); my_robot->clearProtectiveStop(); // Make sure script is running on the robot if (!my_robot->waitForProgramRunning()) @@ -322,12 +322,12 @@ TEST_F(InstructionExecutorTest, unfeasible_movel_target_results_in_failure) std::chrono::steady_clock::time_point start = std::chrono::steady_clock::now(); while (!is_protective_stopped || std::chrono::steady_clock::now() > start + std::chrono::seconds(5)) { - is_protective_stopped = g_my_robot->primary_client_->isRobotProtectiveStopped(); + is_protective_stopped = g_my_robot->getPrimaryClient()->isRobotProtectiveStopped(); std::this_thread::sleep_for(std::chrono::milliseconds(100)); } ASSERT_TRUE(is_protective_stopped); ASSERT_TRUE(g_my_robot->clearProtectiveStop()); - ASSERT_NO_THROW(g_my_robot->primary_client_->commandStop()); + ASSERT_NO_THROW(g_my_robot->getPrimaryClient()->commandStop()); move_thread.join(); } @@ -371,7 +371,7 @@ int main(int argc, char* argv[]) { if (std::string(argv[i]) == "--robot_ip" && i + 1 < argc) { - ROBOT_IP = argv[i + 1]; + g_ROBOT_IP = argv[i + 1]; break; } if (std::string(argv[i]) == "--headless" && i + 1 < argc) diff --git a/tests/test_pipeline.cpp b/tests/test_pipeline.cpp index 4f597fa77..8da69fbed 100644 --- a/tests/test_pipeline.cpp +++ b/tests/test_pipeline.cpp @@ -106,11 +106,11 @@ class PipelineTest : public ::testing::Test bool waitForConsumer(int milliseconds = 100) { - std::unique_lock lk(consumed_mutex_); - if (consumed_cv_.wait_for(lk, std::chrono::milliseconds(milliseconds)) == std::cv_status::no_timeout || - consumed_callback_ == true) + std::unique_lock lk(consumed_mutex); + if (consumed_cv.wait_for(lk, std::chrono::milliseconds(milliseconds)) == std::cv_status::no_timeout || + consumed_callback == true) { - consumed_callback_ = true; + consumed_callback = true; return true; } return false; @@ -119,20 +119,20 @@ class PipelineTest : public ::testing::Test // Consume a package virtual bool consume(std::shared_ptr product) { - std::lock_guard lk(consumed_mutex_); + std::lock_guard lk(consumed_mutex); if (rtde_interface::DataPackage* data = dynamic_cast(product.get())) { - data->getData("timestamp", timestamp_); + data->getData("timestamp", timestamp); } - consumed_cv_.notify_one(); - consumed_callback_ = true; + consumed_cv.notify_one(); + consumed_callback = true; return true; } - double timestamp_ = 0.0; - std::condition_variable consumed_cv_; - std::mutex consumed_mutex_; - bool consumed_callback_ = false; + double timestamp = 0.0; + std::condition_variable consumed_cv; + std::mutex consumed_mutex; + bool consumed_callback = false; }; private: @@ -229,7 +229,7 @@ TEST_F(PipelineTest, consumer_pipeline) // Test that the package was consumed double expected_timestamp = 7103.8579; - EXPECT_FLOAT_EQ(consumer.timestamp_, expected_timestamp); + EXPECT_FLOAT_EQ(consumer.timestamp, expected_timestamp); pipeline_->stop(); } diff --git a/tests/test_robot_receive_timeout.cpp b/tests/test_robot_receive_timeout.cpp index 1aa32a0d2..4bf574e00 100644 --- a/tests/test_robot_receive_timeout.cpp +++ b/tests/test_robot_receive_timeout.cpp @@ -41,14 +41,14 @@ TEST(RobotReceiveTimeout, milliseconds_configuration) { const int expected_timeout = 100; RobotReceiveTimeout robot_receive_timeout = RobotReceiveTimeout::millisec(expected_timeout); - EXPECT_EQ(robot_receive_timeout.timeout_.count(), expected_timeout); + EXPECT_EQ(robot_receive_timeout.getAsMilliseconds().count(), expected_timeout); } TEST(RobotReceiveTimeout, empty_milliseconds_configuration) { const int expected_timeout = 20; RobotReceiveTimeout robot_receive_timeout = RobotReceiveTimeout::millisec(); - EXPECT_EQ(robot_receive_timeout.timeout_.count(), expected_timeout); + EXPECT_EQ(robot_receive_timeout.getAsMilliseconds().count(), expected_timeout); } TEST(RobotReceiveTimeout, seconds_configuration) @@ -56,21 +56,21 @@ TEST(RobotReceiveTimeout, seconds_configuration) float timeout = 0.1; RobotReceiveTimeout robot_receive_timeout = RobotReceiveTimeout::sec(timeout); const int expected_timeout = timeout * 1000; // Convert to milliseconds - EXPECT_EQ(robot_receive_timeout.timeout_.count(), expected_timeout); + EXPECT_EQ(robot_receive_timeout.getAsMilliseconds().count(), expected_timeout); } TEST(RobotReceiveTimeout, empty_seconds_configuration) { const int expected_timeout = 20; RobotReceiveTimeout robot_receive_timeout = RobotReceiveTimeout::sec(); - EXPECT_EQ(robot_receive_timeout.timeout_.count(), expected_timeout); + EXPECT_EQ(robot_receive_timeout.getAsMilliseconds().count(), expected_timeout); } TEST(RobotReceiveTimeout, off_configuration) { const int expected_timeout = 0; RobotReceiveTimeout robot_receive_timeout = RobotReceiveTimeout::off(); - EXPECT_EQ(robot_receive_timeout.timeout_.count(), expected_timeout); + EXPECT_EQ(robot_receive_timeout.getAsMilliseconds().count(), expected_timeout); } TEST(RobotReceiveTimeout, off_realtime_control_modes) diff --git a/tests/test_spline_interpolation.cpp b/tests/test_spline_interpolation.cpp index 284da3f69..6c6630e66 100644 --- a/tests/test_spline_interpolation.cpp +++ b/tests/test_spline_interpolation.cpp @@ -119,7 +119,7 @@ class SplineInterpolationTest : public ::testing::Test g_my_robot->startRTDECommununication(true); - g_my_robot->ur_driver_->registerTrajectoryDoneCallback(&handleTrajectoryState); + g_my_robot->getUrDriver()->registerTrajectoryDoneCallback(&handleTrajectoryState); } static void TearDownTestSuite() @@ -127,7 +127,7 @@ class SplineInterpolationTest : public ::testing::Test if (g_my_robot != nullptr) { // Set target speed scaling to 100% as one test change this value - g_my_robot->ur_driver_->getRTDEWriter().sendSpeedSlider(1); + g_my_robot->getUrDriver()->getRTDEWriter().sendSpeedSlider(1); } // Remove temporary file again @@ -137,27 +137,27 @@ class SplineInterpolationTest : public ::testing::Test void TearDown() { // Set target speed scaling to 100% as one test change this value - g_my_robot->ur_driver_->getRTDEWriter().sendSpeedSlider(1); + g_my_robot->getUrDriver()->getRTDEWriter().sendSpeedSlider(1); } void SetUp() { step_time_ = 0.002; - if (g_my_robot->ur_driver_->getVersion().major < 5) + if (g_my_robot->getUrDriver()->getVersion().major < 5) { step_time_ = 0.008; } - if (g_my_robot->primary_client_->isRobotProtectiveStopped()) + if (g_my_robot->getPrimaryClient()->isRobotProtectiveStopped()) { // We forced a protective stop above. Some versions require waiting 5 seconds before releasing // the protective stop. - if (g_my_robot->dashboard_client_ != nullptr) + if (g_my_robot->getDashboardClient() != nullptr) { - g_my_robot->dashboard_client_->commandClosePopup(); + g_my_robot->getDashboardClient()->commandClosePopup(); } std::this_thread::sleep_for(std::chrono::seconds(5)); - ASSERT_NO_THROW(g_my_robot->primary_client_->commandUnlockProtectiveStop()); + ASSERT_NO_THROW(g_my_robot->getPrimaryClient()->commandUnlockProtectiveStop()); } ASSERT_TRUE(g_my_robot->isHealthy()); } @@ -165,11 +165,11 @@ class SplineInterpolationTest : public ::testing::Test void sendTrajectory(const std::vector& s_pos, const std::vector& s_vel, const std::vector& s_acc, const std::vector& s_time) { - ASSERT_TRUE(g_my_robot->ur_driver_->writeTrajectoryControlMessage( + ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectoryControlMessage( urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP)); // Send trajectory to robot for execution - ASSERT_TRUE(g_my_robot->ur_driver_->writeTrajectoryControlMessage( + ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectoryControlMessage( urcl::control::TrajectoryControlMessage::TRAJECTORY_START, s_pos.size())); for (size_t i = 0; i < s_pos.size(); i++) @@ -177,12 +177,12 @@ class SplineInterpolationTest : public ::testing::Test // QUINTIC if (s_pos.size() == s_acc.size()) { - g_my_robot->ur_driver_->writeTrajectorySplinePoint(s_pos[i], s_vel[i], s_acc[i], s_time[i]); + g_my_robot->getUrDriver()->writeTrajectorySplinePoint(s_pos[i], s_vel[i], s_acc[i], s_time[i]); } // CUBIC else { - g_my_robot->ur_driver_->writeTrajectorySplinePoint(s_pos[i], s_vel[i], s_time[i]); + g_my_robot->getUrDriver()->writeTrajectorySplinePoint(s_pos[i], s_vel[i], s_time[i]); } } } @@ -267,7 +267,7 @@ class SplineInterpolationTest : public ::testing::Test data_pkg->getData("output_double_register_1", spline_travel_time); // Keep connection alive - ASSERT_TRUE(g_my_robot->ur_driver_->writeTrajectoryControlMessage( + ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectoryControlMessage( urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP)); if (std::abs(spline_travel_time - 0.0) < 0.01) { @@ -401,7 +401,7 @@ TEST_F(SplineInterpolationTest, cubic_spline_with_end_point_velocity) ASSERT_TRUE(data_pkg->getData("speed_scaling", speed_scaling)); // Keep connection alive - ASSERT_TRUE(g_my_robot->ur_driver_->writeTrajectoryControlMessage( + ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectoryControlMessage( urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP)); // Read spline travel time from the robot @@ -469,7 +469,7 @@ TEST_F(SplineInterpolationTest, quintic_spline_with_end_point_velocity_with_spee g_my_robot->stopConsumingRTDEData(); // Set speed scaling to 25% to test interpolation with speed scaling active const unsigned int reduse_factor(4); - g_my_robot->ur_driver_->getRTDEWriter().sendSpeedSlider(1.0 / reduse_factor); + g_my_robot->getUrDriver()->getRTDEWriter().sendSpeedSlider(1.0 / reduse_factor); std::unique_ptr data_pkg; g_my_robot->readDataPackage(data_pkg); @@ -535,7 +535,7 @@ TEST_F(SplineInterpolationTest, quintic_spline_with_end_point_velocity_with_spee double time_left = s_time[0] - spline_travel_time; // Keep connection alive - ASSERT_TRUE(g_my_robot->ur_driver_->writeTrajectoryControlMessage( + ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectoryControlMessage( urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP)); // Ensure that we follow the joint trajectory @@ -692,7 +692,7 @@ TEST_F(SplineInterpolationTest, spline_interpolation_cubic) spline_travel_time = (spline_travel_time == 0 ? spline_travel_time : spline_travel_time - step_time_); // Keep connection alive - ASSERT_TRUE(g_my_robot->ur_driver_->writeTrajectoryControlMessage( + ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectoryControlMessage( urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP)); if (old_spline_travel_time > spline_travel_time) @@ -805,7 +805,7 @@ TEST_F(SplineInterpolationTest, spline_interpolation_quintic) spline_travel_time = (spline_travel_time == 0 ? spline_travel_time : spline_travel_time - step_time_); // Keep connection alive - ASSERT_TRUE(g_my_robot->ur_driver_->writeTrajectoryControlMessage( + ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectoryControlMessage( urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP)); if (old_spline_travel_time > spline_travel_time) @@ -876,8 +876,8 @@ TEST_F(SplineInterpolationTest, zero_time_trajectory_cubic_spline) // Send illegal trajectory to the robot sendTrajectory(s_pos, s_vel, std::vector(), s_time); - g_my_robot->ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP, -1, - RobotReceiveTimeout::off()); + g_my_robot->getUrDriver()->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP, -1, + RobotReceiveTimeout::off()); // When an illegal trajectory is send to the robot, the control script should keep running but the trajectory result // should be canceled. @@ -916,7 +916,7 @@ TEST_F(SplineInterpolationTest, zero_time_trajectory_cubic_spline) g_my_robot->readDataPackage(data_pkg); ASSERT_TRUE(data_pkg->getData("target_q", joint_positions)); // Keep connection alive - ASSERT_TRUE(g_my_robot->ur_driver_->writeTrajectoryControlMessage( + ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectoryControlMessage( urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP)); } EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS, g_trajectory_result); @@ -956,8 +956,8 @@ TEST_F(SplineInterpolationTest, zero_time_trajectory_quintic_spline) // Send illegal trajectory to the robot sendTrajectory(s_pos, s_vel, s_acc, s_time); - g_my_robot->ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP, -1, - RobotReceiveTimeout::off()); + g_my_robot->getUrDriver()->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP, -1, + RobotReceiveTimeout::off()); // When an illegal trajectory is send to the robot, the control script should keep running but the trajectory result // should be canceled @@ -997,7 +997,7 @@ TEST_F(SplineInterpolationTest, zero_time_trajectory_quintic_spline) g_my_robot->readDataPackage(data_pkg); ASSERT_TRUE(data_pkg->getData("target_q", joint_positions)); // Keep connection alive - ASSERT_TRUE(g_my_robot->ur_driver_->writeTrajectoryControlMessage( + ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectoryControlMessage( urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP)); } EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS, g_trajectory_result); @@ -1036,8 +1036,8 @@ TEST_F(SplineInterpolationTest, physically_unfeasible_trajectory_cubic_spline) // Send unfeasible trajectory to the robot sendTrajectory(s_pos, s_vel, std::vector(), s_time); - g_my_robot->ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP, -1, - RobotReceiveTimeout::off()); + g_my_robot->getUrDriver()->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP, -1, + RobotReceiveTimeout::off()); // When an unfeasible trajectory is send to the robot, the control script should keep running but the trajectory // result should be canceled @@ -1085,8 +1085,8 @@ TEST_F(SplineInterpolationTest, physically_unfeasible_trajectory_quintic_spline) // Send unfeasible trajectory to the robot sendTrajectory(s_pos, s_vel, s_acc, s_time); - g_my_robot->ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP, -1, - RobotReceiveTimeout::off()); + g_my_robot->getUrDriver()->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP, -1, + RobotReceiveTimeout::off()); // When an unfeasible trajectory is send to the robot, the control script should keep running but the trajectory // result should be canceled @@ -1109,11 +1109,11 @@ TEST_F(SplineInterpolationTest, physically_unfeasible_trajectory_quintic_spline) TEST_F(SplineInterpolationTest, switching_control_mode_without_trajectory_produces_no_result) { // We start by putting the robot into trajectory control mode - ASSERT_TRUE( - g_my_robot->ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP)); + ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectoryControlMessage( + urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP)); // Then we switch to idle - ASSERT_TRUE(g_my_robot->ur_driver_->writeKeepalive(RobotReceiveTimeout::sec(2))); + ASSERT_TRUE(g_my_robot->getUrDriver()->writeKeepalive(RobotReceiveTimeout::sec(2))); EXPECT_FALSE(waitForTrajectoryResult(std::chrono::milliseconds(500))); } @@ -1143,11 +1143,11 @@ TEST_F(SplineInterpolationTest, switching_control_mode_with_trajectory_produces_ std::vector s_time = { 0.02 }; sendTrajectory(s_pos, s_vel, s_acc, s_time); - g_my_robot->ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP, -1, - RobotReceiveTimeout::off()); + g_my_robot->getUrDriver()->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP, -1, + RobotReceiveTimeout::off()); // Then we switch to idle - ASSERT_TRUE(g_my_robot->ur_driver_->writeKeepalive(RobotReceiveTimeout::sec(2))); + ASSERT_TRUE(g_my_robot->getUrDriver()->writeKeepalive(RobotReceiveTimeout::sec(2))); EXPECT_TRUE(waitForTrajectoryResult(std::chrono::milliseconds(500))); } diff --git a/tests/test_ur_driver.cpp b/tests/test_ur_driver.cpp index 03a76015d..acb4c9a9b 100644 --- a/tests/test_ur_driver.cpp +++ b/tests/test_ur_driver.cpp @@ -73,7 +73,7 @@ class UrDriverTest : public ::testing::Test void TearDown() { - g_my_robot->ur_driver_->stopControl(); + g_my_robot->getUrDriver()->stopControl(); g_my_robot->waitForProgramNotRunning(1000); } }; @@ -109,7 +109,7 @@ TEST_F(UrDriverTest, robot_receive_timeout) { // Robot program should time out after the robot receive timeout, whether it takes exactly 200 ms is not so important vector6d_t zeros = { 0, 0, 0, 0, 0, 0 }; - g_my_robot->ur_driver_->writeJointCommand(zeros, comm::ControlMode::MODE_IDLE, RobotReceiveTimeout::millisec(200)); + g_my_robot->getUrDriver()->writeJointCommand(zeros, comm::ControlMode::MODE_IDLE, RobotReceiveTimeout::millisec(200)); EXPECT_TRUE(g_my_robot->waitForProgramNotRunning(400)); // Start robot program @@ -117,8 +117,8 @@ TEST_F(UrDriverTest, robot_receive_timeout) EXPECT_TRUE(g_my_robot->waitForProgramRunning(1000)); // Robot program should time out after the robot receive timeout, whether it takes exactly 200 ms is not so important - g_my_robot->ur_driver_->writeFreedriveControlMessage(control::FreedriveControlMessage::FREEDRIVE_NOOP, - RobotReceiveTimeout::millisec(200)); + g_my_robot->getUrDriver()->writeFreedriveControlMessage(control::FreedriveControlMessage::FREEDRIVE_NOOP, + RobotReceiveTimeout::millisec(200)); EXPECT_TRUE(g_my_robot->waitForProgramNotRunning(400)); // Start robot program @@ -126,8 +126,8 @@ TEST_F(UrDriverTest, robot_receive_timeout) EXPECT_TRUE(g_my_robot->waitForProgramRunning(1000)); // Robot program should time out after the robot receive timeout, whether it takes exactly 200 ms is not so important - g_my_robot->ur_driver_->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_NOOP, -1, - RobotReceiveTimeout::millisec(200)); + g_my_robot->getUrDriver()->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_NOOP, -1, + RobotReceiveTimeout::millisec(200)); EXPECT_TRUE(g_my_robot->waitForProgramNotRunning(400)); // Start robot program @@ -135,39 +135,39 @@ TEST_F(UrDriverTest, robot_receive_timeout) EXPECT_TRUE(g_my_robot->waitForProgramRunning(1000)); // Robot program should time out after the robot receive timeout, whether it takes exactly 200 ms is not so important - g_my_robot->ur_driver_->writeKeepalive(RobotReceiveTimeout::millisec(200)); + g_my_robot->getUrDriver()->writeKeepalive(RobotReceiveTimeout::millisec(200)); EXPECT_TRUE(g_my_robot->waitForProgramNotRunning(400)); } TEST_F(UrDriverTest, robot_receive_timeout_off) { // Program should keep running when setting receive timeout off - g_my_robot->ur_driver_->writeKeepalive(RobotReceiveTimeout::off()); + g_my_robot->getUrDriver()->writeKeepalive(RobotReceiveTimeout::off()); EXPECT_FALSE(g_my_robot->waitForProgramNotRunning(1000)); // Program should keep running when setting receive timeout off - g_my_robot->ur_driver_->writeFreedriveControlMessage(control::FreedriveControlMessage::FREEDRIVE_NOOP, - RobotReceiveTimeout::off()); + g_my_robot->getUrDriver()->writeFreedriveControlMessage(control::FreedriveControlMessage::FREEDRIVE_NOOP, + RobotReceiveTimeout::off()); EXPECT_FALSE(g_my_robot->waitForProgramNotRunning(1000)); // Program should keep running when setting receive timeout off - g_my_robot->ur_driver_->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_NOOP, -1, - RobotReceiveTimeout::off()); + g_my_robot->getUrDriver()->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_NOOP, -1, + RobotReceiveTimeout::off()); EXPECT_FALSE(g_my_robot->waitForProgramNotRunning(1000)); // It shouldn't be possible to set robot receive timeout off, when dealing with realtime commands vector6d_t zeros = { 0, 0, 0, 0, 0, 0 }; - g_my_robot->ur_driver_->writeJointCommand(zeros, comm::ControlMode::MODE_SPEEDJ, RobotReceiveTimeout::off()); + g_my_robot->getUrDriver()->writeJointCommand(zeros, comm::ControlMode::MODE_SPEEDJ, RobotReceiveTimeout::off()); EXPECT_TRUE(g_my_robot->waitForProgramNotRunning(400)); } TEST_F(UrDriverTest, stop_robot_control) { vector6d_t zeros = { 0, 0, 0, 0, 0, 0 }; - g_my_robot->ur_driver_->writeJointCommand(zeros, comm::ControlMode::MODE_IDLE, RobotReceiveTimeout::off()); + g_my_robot->getUrDriver()->writeJointCommand(zeros, comm::ControlMode::MODE_IDLE, RobotReceiveTimeout::off()); // Make sure that we can stop the robot control, when robot receive timeout has been set off - g_my_robot->ur_driver_->stopControl(); + g_my_robot->getUrDriver()->stopControl(); EXPECT_TRUE(g_my_robot->waitForProgramNotRunning(400)); } @@ -186,8 +186,8 @@ TEST_F(UrDriverTest, target_outside_limits_servoj) // Send unfeasible targets to the robot g_my_robot->readDataPackage(data_pkg); - g_my_robot->ur_driver_->writeJointCommand(joint_target, comm::ControlMode::MODE_SERVOJ, - RobotReceiveTimeout::millisec(200)); + g_my_robot->getUrDriver()->writeJointCommand(joint_target, comm::ControlMode::MODE_SERVOJ, + RobotReceiveTimeout::millisec(200)); // Ensure that the robot didn't move g_my_robot->readDataPackage(data_pkg); @@ -200,7 +200,7 @@ TEST_F(UrDriverTest, target_outside_limits_servoj) // Make sure the program is stopped g_my_robot->startConsumingRTDEData(); - g_my_robot->ur_driver_->stopControl(); + g_my_robot->getUrDriver()->stopControl(); g_my_robot->waitForProgramNotRunning(1000); } @@ -219,8 +219,8 @@ TEST_F(UrDriverTest, target_outside_limits_pose) // Send unfeasible targets to the robot g_my_robot->readDataPackage(data_pkg); - g_my_robot->ur_driver_->writeJointCommand(tcp_target, comm::ControlMode::MODE_POSE, - RobotReceiveTimeout::millisec(200)); + g_my_robot->getUrDriver()->writeJointCommand(tcp_target, comm::ControlMode::MODE_POSE, + RobotReceiveTimeout::millisec(200)); // Ensure that the robot didn't move g_my_robot->readDataPackage(data_pkg); @@ -233,7 +233,7 @@ TEST_F(UrDriverTest, target_outside_limits_pose) // Make sure the program is stopped g_my_robot->startConsumingRTDEData(); - g_my_robot->ur_driver_->stopControl(); + g_my_robot->getUrDriver()->stopControl(); g_my_robot->waitForProgramNotRunning(1000); } @@ -243,10 +243,10 @@ TEST_F(UrDriverTest, send_robot_program_retry_on_failure) // switching from Remote to Local and back to Remote mode for example. // To be able to re-send the robot program we'll have to make sure it isn't running - g_my_robot->ur_driver_->stopControl(); + g_my_robot->getUrDriver()->stopControl(); g_my_robot->waitForProgramNotRunning(); - g_my_robot->ur_driver_->stopPrimaryClientCommunication(); + g_my_robot->getUrDriver()->stopPrimaryClientCommunication(); EXPECT_TRUE(g_my_robot->resendRobotProgram()); @@ -257,8 +257,8 @@ TEST_F(UrDriverTest, reset_rtde_client) { g_my_robot->stopConsumingRTDEData(); double target_frequency = 50; - g_my_robot->ur_driver_->resetRTDEClient(OUTPUT_RECIPE, INPUT_RECIPE, target_frequency); - ASSERT_EQ(g_my_robot->ur_driver_->getControlFrequency(), target_frequency); + g_my_robot->getUrDriver()->resetRTDEClient(OUTPUT_RECIPE, INPUT_RECIPE, target_frequency); + ASSERT_EQ(g_my_robot->getUrDriver()->getControlFrequency(), target_frequency); } TEST_F(UrDriverTest, read_error_code) @@ -267,12 +267,12 @@ TEST_F(UrDriverTest, read_error_code) std::this_thread::sleep_for(std::chrono::milliseconds(1000)); std::stringstream cmd; cmd << "sec setup():" << std::endl << " protective_stop()" << std::endl << "end"; - EXPECT_TRUE(g_my_robot->ur_driver_->sendScript(cmd.str())); + EXPECT_TRUE(g_my_robot->getUrDriver()->sendScript(cmd.str())); - auto error_codes = g_my_robot->ur_driver_->getErrorCodes(); + auto error_codes = g_my_robot->getUrDriver()->getErrorCodes(); while (error_codes.size() == 0) { - error_codes = g_my_robot->ur_driver_->getErrorCodes(); + error_codes = g_my_robot->getUrDriver()->getErrorCodes(); } ASSERT_EQ(error_codes.size(), 1); @@ -284,11 +284,11 @@ TEST_F(UrDriverTest, read_error_code) // Wait for after PSTOP before clearing it std::this_thread::sleep_for(std::chrono::seconds(6)); - if (g_my_robot->dashboard_client_ != nullptr) + if (g_my_robot->getDashboardClient() != nullptr) { - EXPECT_TRUE(g_my_robot->dashboard_client_->commandCloseSafetyPopup()); + EXPECT_TRUE(g_my_robot->getDashboardClient()->commandCloseSafetyPopup()); } - EXPECT_NO_THROW(g_my_robot->primary_client_->commandUnlockProtectiveStop()); + EXPECT_NO_THROW(g_my_robot->getPrimaryClient()->commandUnlockProtectiveStop()); } TEST(UrDriverInitTest, setting_connection_limits_works_correctly)