diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index f3ac1dae..0938a7fd 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -1,5 +1,10 @@ name: Integration tests -on: [push, pull_request] +on: + workflow_dispatch: + pull_request: + push: + branches: + - main jobs: build: diff --git a/.github/workflows/industrial-ci.yml b/.github/workflows/industrial-ci.yml index 28c3c07f..23183aa7 100644 --- a/.github/workflows/industrial-ci.yml +++ b/.github/workflows/industrial-ci.yml @@ -1,5 +1,10 @@ name: ROS industrial ci -on: [push, pull_request] +on: + workflow_dispatch: + pull_request: + push: + branches: + - master jobs: industrial_ci: @@ -11,7 +16,6 @@ jobs: ROS_DISTRO: - {NAME: noetic, DOWNSTREAM_WORKSPACE: "github:UniversalRobots/Universal_Robots_ROS_Driver#master https://raw.githubusercontent.com/UniversalRobots/Universal_Robots_ROS_Driver/master/.noetic.rosinstall"} - {NAME: humble, DOWNSTREAM_WORKSPACE: "github:UniversalRobots/Universal_Robots_ROS2_Driver#humble"} - - {NAME: iron, DOWNSTREAM_WORKSPACE: "github:UniversalRobots/Universal_Robots_ROS2_Driver#iron"} - {NAME: jazzy, DOWNSTREAM_WORKSPACE: "github:UniversalRobots/Universal_Robots_ROS2_Driver#main"} - {NAME: rolling, DOWNSTREAM_WORKSPACE: "github:UniversalRobots/Universal_Robots_ROS2_Driver#main"} ROS_REPO: @@ -22,6 +26,7 @@ jobs: steps: - uses: actions/checkout@v4 - run: docker network create --subnet=192.168.56.0/24 ursim_net + if: ${{ !env.ACT }} - uses: 'ros-industrial/industrial_ci@master' env: IMMEDIATE_TEST_OUTPUT: true @@ -30,3 +35,4 @@ jobs: DOWNSTREAM_WORKSPACE: ${{matrix.ROS_DISTRO.DOWNSTREAM_WORKSPACE}} ROS_DISTRO: ${{matrix.ROS_DISTRO.NAME}} ROS_REPO: ${{matrix.ROS_REPO}} + CLANG_TIDY: true diff --git a/.github/workflows/pre-commit.yml b/.github/workflows/pre-commit.yml index 983137c5..04301871 100644 --- a/.github/workflows/pre-commit.yml +++ b/.github/workflows/pre-commit.yml @@ -8,7 +8,7 @@ on: pull_request: push: branches: - - main + - master jobs: pre-commit: @@ -19,8 +19,6 @@ jobs: - uses: actions/setup-python@v5 with: python-version: 3.10.4 - - name: Install system hooks - run: sudo apt-get install clang-format-14 cppcheck - uses: pre-commit/action@v3.0.1 with: extra_args: --all-files --hook-stage manual diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 1d98db09..407582db 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -1,6 +1,6 @@ repos: - repo: https://github.com/pre-commit/mirrors-clang-format - rev: 'v14.0.6' + rev: 'v19.1.5' hooks: - id: clang-format - repo: https://github.com/DavidAnson/markdownlint-cli2 diff --git a/examples/force_mode_example.cpp b/examples/force_mode_example.cpp index 1b275c24..02bc9070 100644 --- a/examples/force_mode_example.cpp +++ b/examples/force_mode_example.cpp @@ -55,8 +55,8 @@ 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_; +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. @@ -66,9 +66,9 @@ void handleRobotProgramState(bool program_running) 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_); + std::lock_guard lk(g_program_running_mutex); g_program_running = program_running; - g_program_running_cv_.notify_one(); + g_program_running_cv.notify_one(); } } @@ -84,8 +84,8 @@ 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 || + 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; @@ -149,8 +149,8 @@ int main(int argc, char* argv[]) // 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, + 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)) diff --git a/examples/freedrive_example.cpp b/examples/freedrive_example.cpp index c24f4ff2..53466cd4 100644 --- a/examples/freedrive_example.cpp +++ b/examples/freedrive_example.cpp @@ -128,8 +128,8 @@ int main(int argc, char* argv[]) // 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, + 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)); // Once RTDE communication is started, we have to make sure to read from the interface buffer, as diff --git a/examples/full_driver.cpp b/examples/full_driver.cpp index a1e429db..e2cbe6d8 100644 --- a/examples/full_driver.cpp +++ b/examples/full_driver.cpp @@ -107,8 +107,8 @@ int main(int argc, char* argv[]) // 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, + 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)); // Once RTDE communication is started, we have to make sure to read from the interface buffer, as diff --git a/examples/primary_pipeline_calibration.cpp b/examples/primary_pipeline_calibration.cpp index 76a3094d..9f54e2e8 100644 --- a/examples/primary_pipeline_calibration.cpp +++ b/examples/primary_pipeline_calibration.cpp @@ -26,7 +26,7 @@ using namespace urcl; class CalibrationConsumer : public urcl::comm::IConsumer { public: - CalibrationConsumer() : calibrated_(0), have_received_data(false) + CalibrationConsumer() : calibrated_(0), have_received_data_(false) { } virtual ~CalibrationConsumer() = default; @@ -38,25 +38,25 @@ class CalibrationConsumer : public urcl::comm::IConsumertoString().c_str()); calibrated_ = kin_info->calibration_status_; - have_received_data = true; + have_received_data_ = true; } return true; } bool isCalibrated() const { - const uint32_t LINEARIZED = 2; - return calibrated_ == LINEARIZED; + const uint32_t linearized = 2; + return calibrated_ == linearized; } bool calibrationStatusReceived() { - return have_received_data; + return have_received_data_; } private: uint32_t calibrated_; - bool have_received_data; + bool have_received_data_; }; // In a real-world example it would be better to get those values from command line parameters / a better configuration diff --git a/examples/rtde_client.cpp b/examples/rtde_client.cpp index 387f045e..01a1bb40 100644 --- a/examples/rtde_client.cpp +++ b/examples/rtde_client.cpp @@ -70,8 +70,8 @@ int main(int argc, char* argv[]) // loop. my_client.start(); - unsigned long startTime = clock(); - while (second_to_run < 0 || ((clock() - startTime) / CLOCKS_PER_SEC) < static_cast(second_to_run)) + unsigned long start_time = clock(); + while (second_to_run < 0 || ((clock() - start_time) / CLOCKS_PER_SEC) < static_cast(second_to_run)) { // Read latest RTDE package. This will block for READ_TIMEOUT, so the // robot will effectively be in charge of setting the frequency of this loop unless RTDE @@ -93,9 +93,7 @@ int main(int argc, char* argv[]) { // This will happen for example, when the required keys are not configured inside the input // recipe. - std::cout << "\033[1;31mSending RTDE data failed." - << "\033[0m\n" - << std::endl; + std::cout << "\033[1;31mSending RTDE data failed." << "\033[0m\n" << std::endl; return 1; } diff --git a/examples/spline_example.cpp b/examples/spline_example.cpp index e6f1c10e..9af4240d 100644 --- a/examples/spline_example.cpp +++ b/examples/spline_example.cpp @@ -52,7 +52,7 @@ std::unique_ptr g_my_driver; std::unique_ptr g_my_dashboard; vector6d_t g_joint_positions; -void SendTrajectory(const std::vector& p_p, const std::vector& p_v, +void sendTrajectory(const std::vector& p_p, const std::vector& p_v, const std::vector& p_a, const std::vector& time, bool use_spline_interpolation_) { assert(p_p.size() == time.size()); @@ -146,8 +146,8 @@ int main(int argc, char* argv[]) } // if the robot is not powered on and ready - std::string robotModeRunning("RUNNING"); - while (!g_my_dashboard->commandRobotMode(robotModeRunning)) + std::string robot_mode_running("RUNNING"); + while (!g_my_dashboard->commandRobotMode(robot_mode_running)) { // Power it off if (!g_my_dashboard->commandPowerOff()) @@ -174,8 +174,8 @@ int main(int argc, char* argv[]) // 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, + 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); @@ -241,7 +241,7 @@ int main(int argc, char* argv[]) } // CUBIC - SendTrajectory(p, v, std::vector(), time, true); + sendTrajectory(p, v, std::vector(), time, true); g_trajectory_running = true; while (g_trajectory_running) @@ -272,7 +272,7 @@ int main(int argc, char* argv[]) URCL_LOG_INFO("CUBIC Movement done"); // QUINTIC - SendTrajectory(p, v, a, time, true); + sendTrajectory(p, v, a, time, true); g_trajectory_running = true; while (g_trajectory_running) diff --git a/examples/tool_contact_example.cpp b/examples/tool_contact_example.cpp index 74e6ae7f..35bf739a 100644 --- a/examples/tool_contact_example.cpp +++ b/examples/tool_contact_example.cpp @@ -124,8 +124,8 @@ int main(int argc, char* argv[]) // 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, + 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->registerToolContactResultCallback(&handleToolContactResult); diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index 3dc12875..4e05a487 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -136,9 +136,7 @@ urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_ } begin_replace << "set_tool_voltage(" << static_cast::type>(tool_comm_setup->getToolVoltage()) << ")\n"; - begin_replace << "set_tool_communication(" - << "True" - << ", " << tool_comm_setup->getBaudRate() << ", " + begin_replace << "set_tool_communication(" << "True" << ", " << tool_comm_setup->getBaudRate() << ", " << static_cast::type>(tool_comm_setup->getParity()) << ", " << tool_comm_setup->getStopBits() << ", " << tool_comm_setup->getRxIdleChars() << ", " << tool_comm_setup->getTxIdleChars() << ")"; diff --git a/tests/test_dashboard_client.cpp b/tests/test_dashboard_client.cpp index 36fcabb6..1ef235c8 100644 --- a/tests/test_dashboard_client.cpp +++ b/tests/test_dashboard_client.cpp @@ -39,14 +39,14 @@ using namespace urcl; -std::string ROBOT_IP = "192.168.56.101"; +std::string g_ROBOT_IP = "192.168.56.101"; class DashboardClientTest : public ::testing::Test { protected: void SetUp() { - dashboard_client_.reset(new DashboardClient(ROBOT_IP)); + dashboard_client_.reset(new DashboardClient(g_ROBOT_IP)); } void TearDown() @@ -227,7 +227,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; } } diff --git a/tests/test_pipeline.cpp b/tests/test_pipeline.cpp index fccf0dd8..327b3b55 100644 --- a/tests/test_pipeline.cpp +++ b/tests/test_pipeline.cpp @@ -60,7 +60,7 @@ class PipelineTest : public ::testing::Test pipeline_->init(); } - void Teardown() + void teardown() { // Clean up pipeline_->stop(); diff --git a/tests/test_producer.cpp b/tests/test_producer.cpp index 15663cbb..5ceaec4e 100644 --- a/tests/test_producer.cpp +++ b/tests/test_producer.cpp @@ -49,7 +49,7 @@ class ProducerTest : public ::testing::Test server_->start(); } - void Teardown() + void teardown() { // Clean up server_.reset(); diff --git a/tests/test_robot_receive_timeout.cpp b/tests/test_robot_receive_timeout.cpp index 43eba2d2..1aa32a0d 100644 --- a/tests/test_robot_receive_timeout.cpp +++ b/tests/test_robot_receive_timeout.cpp @@ -35,7 +35,7 @@ using namespace urcl; -const std::chrono::milliseconds g_step_time(2); +const std::chrono::milliseconds G_STEP_TIME(2); TEST(RobotReceiveTimeout, milliseconds_configuration) { @@ -80,8 +80,8 @@ TEST(RobotReceiveTimeout, off_realtime_control_modes) for (unsigned int i = 0; i < comm::ControlModeTypes::REALTIME_CONTROL_MODES.size(); ++i) { const int actual_timeout = - robot_receive_timeout.verifyRobotReceiveTimeout(comm::ControlModeTypes::REALTIME_CONTROL_MODES[i], g_step_time); - EXPECT_EQ(actual_timeout, g_step_time.count()); + robot_receive_timeout.verifyRobotReceiveTimeout(comm::ControlModeTypes::REALTIME_CONTROL_MODES[i], G_STEP_TIME); + EXPECT_EQ(actual_timeout, G_STEP_TIME.count()); } } @@ -92,7 +92,7 @@ TEST(RobotReceiveTimeout, off_non_realtime_control_modes) for (unsigned int i = 0; i < comm::ControlModeTypes::NON_REALTIME_CONTROL_MODES.size(); ++i) { const int actual_timeout = robot_receive_timeout.verifyRobotReceiveTimeout( - comm::ControlModeTypes::NON_REALTIME_CONTROL_MODES[i], g_step_time); + comm::ControlModeTypes::NON_REALTIME_CONTROL_MODES[i], G_STEP_TIME); const int expected_timeout = 0; EXPECT_EQ(actual_timeout, expected_timeout); } @@ -105,8 +105,8 @@ TEST(RobotReceiveTimeout, timeout_below_step_time_realtime_control_modes) for (unsigned int i = 0; i < comm::ControlModeTypes::REALTIME_CONTROL_MODES.size(); ++i) { const int actual_timeout = - robot_receive_timeout.verifyRobotReceiveTimeout(comm::ControlModeTypes::REALTIME_CONTROL_MODES[i], g_step_time); - EXPECT_EQ(actual_timeout, g_step_time.count()); + robot_receive_timeout.verifyRobotReceiveTimeout(comm::ControlModeTypes::REALTIME_CONTROL_MODES[i], G_STEP_TIME); + EXPECT_EQ(actual_timeout, G_STEP_TIME.count()); } } @@ -117,8 +117,8 @@ TEST(RobotReceiveTimeout, timeout_below_step_time_non_realtime_control_modes) for (unsigned int i = 0; i < comm::ControlModeTypes::NON_REALTIME_CONTROL_MODES.size(); ++i) { const int actual_timeout = robot_receive_timeout.verifyRobotReceiveTimeout( - comm::ControlModeTypes::NON_REALTIME_CONTROL_MODES[i], g_step_time); - EXPECT_EQ(actual_timeout, g_step_time.count()); + comm::ControlModeTypes::NON_REALTIME_CONTROL_MODES[i], G_STEP_TIME); + EXPECT_EQ(actual_timeout, G_STEP_TIME.count()); } } @@ -129,7 +129,7 @@ TEST(RobotReceiveTimeout, timeout_above_max_allowed_timeout_realtime_control_mod for (unsigned int i = 0; i < comm::ControlModeTypes::REALTIME_CONTROL_MODES.size(); ++i) { const int actual_timeout = - robot_receive_timeout.verifyRobotReceiveTimeout(comm::ControlModeTypes::REALTIME_CONTROL_MODES[i], g_step_time); + robot_receive_timeout.verifyRobotReceiveTimeout(comm::ControlModeTypes::REALTIME_CONTROL_MODES[i], G_STEP_TIME); EXPECT_EQ(actual_timeout, RobotReceiveTimeout::MAX_RT_RECEIVE_TIMEOUT_MS.count()); } } @@ -142,7 +142,7 @@ TEST(RobotReceiveTimeout, timeout_within_limit_realtime_control_modes) for (unsigned int i = 0; i < comm::ControlModeTypes::REALTIME_CONTROL_MODES.size(); ++i) { const int actual_timeout = - robot_receive_timeout.verifyRobotReceiveTimeout(comm::ControlModeTypes::REALTIME_CONTROL_MODES[i], g_step_time); + robot_receive_timeout.verifyRobotReceiveTimeout(comm::ControlModeTypes::REALTIME_CONTROL_MODES[i], G_STEP_TIME); EXPECT_EQ(actual_timeout, expected_timeout); } } @@ -155,7 +155,7 @@ TEST(RobotReceiveTimeout, timeout_within_limit_non_realtime_control_modes) for (unsigned int i = 0; i < comm::ControlModeTypes::NON_REALTIME_CONTROL_MODES.size(); ++i) { const int actual_timeout = robot_receive_timeout.verifyRobotReceiveTimeout( - comm::ControlModeTypes::NON_REALTIME_CONTROL_MODES[i], g_step_time); + comm::ControlModeTypes::NON_REALTIME_CONTROL_MODES[i], G_STEP_TIME); EXPECT_EQ(actual_timeout, expected_timeout); } } @@ -167,7 +167,7 @@ TEST(RobotReceiveTimeout, unknown_control_mode) { RobotReceiveTimeout robot_receive_timeout = RobotReceiveTimeout::millisec(200); EXPECT_THROW(robot_receive_timeout.verifyRobotReceiveTimeout(comm::ControlModeTypes::STATIONARY_CONTROL_MODES[i], - g_step_time), + G_STEP_TIME), UrException); } } diff --git a/tests/test_rtde_client.cpp b/tests/test_rtde_client.cpp index d72011d1..a3683b63 100644 --- a/tests/test_rtde_client.cpp +++ b/tests/test_rtde_client.cpp @@ -40,14 +40,14 @@ using namespace urcl; -std::string ROBOT_IP = "192.168.56.101"; +std::string g_ROBOT_IP = "192.168.56.101"; class RTDEClientTest : public ::testing::Test { protected: void SetUp() { - client_.reset(new rtde_interface::RTDEClient(ROBOT_IP, notifier_, output_recipe_file_, input_recipe_file_)); + client_.reset(new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, output_recipe_file_, input_recipe_file_)); } void TearDown() @@ -116,12 +116,12 @@ TEST_F(RTDEClientTest, no_recipe) std::string output_recipe_file = ""; std::string input_recipe_file = ""; EXPECT_THROW( - client_.reset(new rtde_interface::RTDEClient(ROBOT_IP, notifier_, output_recipe_file, input_recipe_file)), + client_.reset(new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, output_recipe_file, input_recipe_file)), UrException); // Only input recipe is unconfigured EXPECT_THROW( - client_.reset(new rtde_interface::RTDEClient(ROBOT_IP, notifier_, output_recipe_file_, input_recipe_file)), + client_.reset(new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, output_recipe_file_, input_recipe_file)), UrException); } @@ -130,12 +130,12 @@ TEST_F(RTDEClientTest, empty_recipe_file) std::string output_recipe_file = "resources/empty.txt"; std::string input_recipe_file = "resources/empty.txt"; EXPECT_THROW( - client_.reset(new rtde_interface::RTDEClient(ROBOT_IP, notifier_, output_recipe_file, input_recipe_file)), + client_.reset(new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, output_recipe_file, input_recipe_file)), UrException); // Only input recipe is empty EXPECT_THROW( - client_.reset(new rtde_interface::RTDEClient(ROBOT_IP, notifier_, output_recipe_file_, input_recipe_file)), + client_.reset(new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, output_recipe_file_, input_recipe_file)), UrException); } @@ -143,12 +143,12 @@ TEST_F(RTDEClientTest, invalid_target_frequency) { // Setting target frequency below 0 or above 500, should throw an exception client_.reset( - new rtde_interface::RTDEClient(ROBOT_IP, notifier_, output_recipe_file_, input_recipe_file_, -1.0, false)); + new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, output_recipe_file_, input_recipe_file_, -1.0, false)); EXPECT_THROW(client_->init(), UrException); client_.reset( - new rtde_interface::RTDEClient(ROBOT_IP, notifier_, output_recipe_file_, input_recipe_file_, 1000, false)); + new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, output_recipe_file_, input_recipe_file_, 1000, false)); EXPECT_THROW(client_->init(), UrException); } @@ -167,7 +167,8 @@ TEST_F(RTDEClientTest, unconfigured_target_frequency) TEST_F(RTDEClientTest, set_target_frequency) { - client_.reset(new rtde_interface::RTDEClient(ROBOT_IP, notifier_, output_recipe_file_, input_recipe_file_, 1, false)); + client_.reset( + new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, output_recipe_file_, input_recipe_file_, 1, false)); client_->init(); // Maximum frequency should still be equal to the robot's maximum frequency @@ -268,7 +269,7 @@ TEST_F(RTDEClientTest, output_recipe_file) TEST_F(RTDEClientTest, recipe_compairson) { // Check that vectorized constructor provides same recipes as from file - auto client = rtde_interface::RTDEClient(ROBOT_IP, notifier_, resources_output_recipe_, resources_input_recipe_); + auto client = rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, resources_output_recipe_, resources_input_recipe_); std::vector output_recipe_from_file = client_->getOutputRecipe(); std::vector output_recipe_from_vector = client.getOutputRecipe(); @@ -341,7 +342,7 @@ TEST_F(RTDEClientTest, write_rtde_data) TEST_F(RTDEClientTest, output_recipe_without_timestamp) { std::string output_recipe_file = "resources/rtde_output_recipe_without_timestamp.txt"; - client_.reset(new rtde_interface::RTDEClient(ROBOT_IP, notifier_, output_recipe_file, input_recipe_file_)); + client_.reset(new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, output_recipe_file, input_recipe_file_)); std::vector actual_output_recipe_from_file = client_->getOutputRecipe(); const std::string timestamp = "timestamp"; @@ -350,7 +351,7 @@ TEST_F(RTDEClientTest, output_recipe_without_timestamp) // Verify that timestamp is added to the recipe when using the vectorized constructor std::vector output_recipe = { "actual_q", "actual_qd" }; - auto client = rtde_interface::RTDEClient(ROBOT_IP, notifier_, output_recipe, resources_input_recipe_); + auto client = rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, output_recipe, resources_input_recipe_); std::vector actual_output_recipe_from_vector = client.getOutputRecipe(); it = std::find(actual_output_recipe_from_vector.begin(), actual_output_recipe_from_vector.end(), timestamp); EXPECT_FALSE(it == actual_output_recipe_from_vector.end()); @@ -376,8 +377,8 @@ TEST_F(RTDEClientTest, check_all_rtde_output_variables_exist) client_->init(); // Ignore unknown output variables to account for variables not available in old urcontrol versions. - client_.reset(new rtde_interface::RTDEClient(ROBOT_IP, notifier_, exhaustive_output_recipe_file_, input_recipe_file_, - 0.0, true)); + client_.reset(new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, exhaustive_output_recipe_file_, + input_recipe_file_, 0.0, true)); EXPECT_NO_THROW(client_->init()); client_->start(); @@ -405,7 +406,7 @@ TEST_F(RTDEClientTest, check_unknown_rtde_output_variable) std::vector incorrect_output_recipe = client_->getOutputRecipe(); incorrect_output_recipe.push_back("unknown_rtde_variable"); - client_.reset(new rtde_interface::RTDEClient(ROBOT_IP, notifier_, incorrect_output_recipe, resources_input_recipe_, + client_.reset(new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, incorrect_output_recipe, resources_input_recipe_, 0.0, false)); EXPECT_THROW(client_->init(), UrException); @@ -419,7 +420,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; } } diff --git a/tests/test_spline_interpolation.cpp b/tests/test_spline_interpolation.cpp index 28aa5c6b..39eec3d7 100644 --- a/tests/test_spline_interpolation.cpp +++ b/tests/test_spline_interpolation.cpp @@ -49,16 +49,16 @@ const std::string SPLINE_SCRIPT_FILE = "spline_external_control.urscript"; const std::string OUTPUT_RECIPE = "resources/rtde_output_recipe_spline.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"; -std::unique_ptr g_ur_driver_; -std::unique_ptr g_dashboard_client_; +std::unique_ptr g_ur_driver; +std::unique_ptr g_dashboard_client; bool g_program_running; -std::condition_variable g_program_not_running_cv_; -std::mutex g_program_not_running_mutex_; -std::condition_variable g_program_running_cv_; -std::mutex g_program_running_mutex_; +std::condition_variable g_program_not_running_cv; +std::mutex g_program_not_running_mutex; +std::condition_variable g_program_running_cv; +std::mutex g_program_running_mutex; // Helper functions for the driver void handleRobotProgramState(bool program_running) @@ -67,41 +67,41 @@ void handleRobotProgramState(bool program_running) 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_); + std::lock_guard lk(g_program_running_mutex); g_program_running = program_running; - g_program_running_cv_.notify_one(); + g_program_running_cv.notify_one(); } else { - std::lock_guard lk(g_program_not_running_mutex_); + std::lock_guard lk(g_program_not_running_mutex); g_program_running = program_running; - g_program_not_running_cv_.notify_one(); + g_program_not_running_cv.notify_one(); } } -bool g_trajectory_running_; -control::TrajectoryResult g_trajectory_result_; +bool g_trajectory_running; +control::TrajectoryResult g_trajectory_result; void handleTrajectoryState(control::TrajectoryResult state) { - g_trajectory_result_ = state; - g_trajectory_running_ = false; + g_trajectory_result = state; + g_trajectory_running = false; } -bool g_rtde_read_thread_running_ = false; -bool g_consume_rtde_packages_ = false; -std::mutex g_read_package_mutex_; +bool g_rtde_read_thread_running = false; +bool g_consume_rtde_packages = false; +std::mutex g_read_package_mutex; std::thread g_rtde_read_thread; void rtdeConsumeThread() { - while (g_rtde_read_thread_running_) + while (g_rtde_read_thread_running) { // Consume package to prevent pipeline overflow - if (g_consume_rtde_packages_ == true) + if (g_consume_rtde_packages == true) { - std::lock_guard lk(g_read_package_mutex_); + std::lock_guard lk(g_read_package_mutex); std::unique_ptr data_pkg; - data_pkg = g_ur_driver_->getDataPackage(); + data_pkg = g_ur_driver->getDataPackage(); } else { @@ -115,7 +115,7 @@ int sign(double val) return (0.0 < val) - (val < 0.0); } -bool nearly_equal(double a, double b, double eps = 1e-15) +bool nearlyEqual(double a, double b, double eps = 1e-15) { const double c(a - b); return c < eps || -c < eps; @@ -126,20 +126,20 @@ class SplineInterpolationTest : public ::testing::Test protected: static void SetUpTestSuite() { - g_dashboard_client_.reset(new DashboardClient(ROBOT_IP)); - ASSERT_TRUE(g_dashboard_client_->connect()); + g_dashboard_client.reset(new DashboardClient(g_ROBOT_IP)); + ASSERT_TRUE(g_dashboard_client->connect()); // Make robot ready for test timeval tv; tv.tv_sec = 10; tv.tv_usec = 0; - g_dashboard_client_->setReceiveTimeout(tv); + g_dashboard_client->setReceiveTimeout(tv); // Stop running program if there is one - ASSERT_TRUE(g_dashboard_client_->commandStop()); + ASSERT_TRUE(g_dashboard_client->commandStop()); // if the robot is not powered on and ready - ASSERT_TRUE(g_dashboard_client_->commandBrakeRelease()); + ASSERT_TRUE(g_dashboard_client->commandBrakeRelease()); // Add splineTimerTraveled to output registers to check for correct computation on test side std::ifstream in_file(SCRIPT_FILE); @@ -160,39 +160,39 @@ class SplineInterpolationTest : public ::testing::Test // Setup driver std::unique_ptr tool_comm_setup; - const bool HEADLESS = true; + const bool headless = true; try { - g_ur_driver_.reset(new UrDriver(ROBOT_IP, SPLINE_SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, - &handleRobotProgramState, HEADLESS, std::move(tool_comm_setup), - CALIBRATION_CHECKSUM)); + g_ur_driver.reset(new UrDriver(g_ROBOT_IP, SPLINE_SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, + &handleRobotProgramState, headless, std::move(tool_comm_setup), + CALIBRATION_CHECKSUM)); } catch (UrException& exp) { std::cout << "caught exception " << exp.what() << " while launch driver, retrying once in 10 seconds" << std::endl; std::this_thread::sleep_for(std::chrono::seconds(10)); - g_ur_driver_.reset(new UrDriver(ROBOT_IP, SPLINE_SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, - &handleRobotProgramState, HEADLESS, std::move(tool_comm_setup), - CALIBRATION_CHECKSUM)); + g_ur_driver.reset(new UrDriver(g_ROBOT_IP, SPLINE_SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, + &handleRobotProgramState, headless, std::move(tool_comm_setup), + CALIBRATION_CHECKSUM)); } - g_ur_driver_->registerTrajectoryDoneCallback(&handleTrajectoryState); + g_ur_driver->registerTrajectoryDoneCallback(&handleTrajectoryState); - g_ur_driver_->startRTDECommunication(); + g_ur_driver->startRTDECommunication(); // Setup rtde read thread - g_rtde_read_thread_running_ = true; + g_rtde_read_thread_running = true; g_rtde_read_thread = std::thread(rtdeConsumeThread); } static void TearDownTestSuite() { // Set target speed scaling to 100% as one test change this value - g_ur_driver_->getRTDEWriter().sendSpeedSlider(1); + g_ur_driver->getRTDEWriter().sendSpeedSlider(1); - g_rtde_read_thread_running_ = false; + g_rtde_read_thread_running = false; g_rtde_read_thread.join(); - g_dashboard_client_->disconnect(); + g_dashboard_client->disconnect(); // Remove temporary file again std::remove(SPLINE_SCRIPT_FILE.c_str()); } @@ -200,51 +200,51 @@ class SplineInterpolationTest : public ::testing::Test void TearDown() { // Set target speed scaling to 100% as one test change this value - g_ur_driver_->getRTDEWriter().sendSpeedSlider(1); + g_ur_driver->getRTDEWriter().sendSpeedSlider(1); } void SetUp() { step_time_ = 0.002; - if (g_ur_driver_->getVersion().major < 5) + if (g_ur_driver->getVersion().major < 5) { step_time_ = 0.008; } // Make sure script is running on the robot if (g_program_running == false) { - g_consume_rtde_packages_ = true; - g_ur_driver_->sendRobotProgram(); + g_consume_rtde_packages = true; + g_ur_driver->sendRobotProgram(); ASSERT_TRUE(waitForProgramRunning(1000)); } - g_consume_rtde_packages_ = false; + g_consume_rtde_packages = false; } void sendIdle() { - ASSERT_TRUE(g_ur_driver_->writeKeepalive(RobotReceiveTimeout::sec())); + ASSERT_TRUE(g_ur_driver->writeKeepalive(RobotReceiveTimeout::sec())); } 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_ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP)); + ASSERT_TRUE(g_ur_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP)); // Send trajectory to robot for execution - ASSERT_TRUE(g_ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, - s_pos.size())); + ASSERT_TRUE(g_ur_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, + s_pos.size())); for (size_t i = 0; i < s_pos.size(); i++) { // QUINTIC if (s_pos.size() == s_acc.size()) { - g_ur_driver_->writeTrajectorySplinePoint(s_pos[i], s_vel[i], s_acc[i], s_time[i]); + g_ur_driver->writeTrajectorySplinePoint(s_pos[i], s_vel[i], s_acc[i], s_time[i]); } // CUBIC else { - g_ur_driver_->writeTrajectorySplinePoint(s_pos[i], s_vel[i], s_time[i]); + g_ur_driver->writeTrajectorySplinePoint(s_pos[i], s_vel[i], s_time[i]); } } } @@ -329,8 +329,7 @@ class SplineInterpolationTest : public ::testing::Test data_pkg->getData("output_double_register_1", spline_travel_time); // Keep connection alive - ASSERT_TRUE( - g_ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP)); + ASSERT_TRUE(g_ur_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP)); if (std::abs(spline_travel_time - 0.0) < 0.01) { return; @@ -348,8 +347,8 @@ class SplineInterpolationTest : public ::testing::Test 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 || + 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; @@ -359,8 +358,8 @@ class SplineInterpolationTest : public ::testing::Test bool waitForProgramNotRunning(int milliseconds = 100) { - std::unique_lock lk(g_program_not_running_mutex_); - if (g_program_not_running_cv_.wait_for(lk, std::chrono::milliseconds(milliseconds)) == std::cv_status::no_timeout || + std::unique_lock lk(g_program_not_running_mutex); + if (g_program_not_running_cv.wait_for(lk, std::chrono::milliseconds(milliseconds)) == std::cv_status::no_timeout || g_program_running == false) { return true; @@ -370,13 +369,13 @@ class SplineInterpolationTest : public ::testing::Test void readDataPackage(std::unique_ptr& data_pkg) { - if (g_consume_rtde_packages_ == true) + if (g_consume_rtde_packages == true) { URCL_LOG_ERROR("Unable to read packages while consuming, this should not happen!"); GTEST_FAIL(); } - std::lock_guard lk(g_read_package_mutex_); - data_pkg = g_ur_driver_->getDataPackage(); + std::lock_guard lk(g_read_package_mutex); + data_pkg = g_ur_driver->getDataPackage(); if (data_pkg == nullptr) { URCL_LOG_ERROR("Timed out waiting for a new package from the robot"); @@ -483,14 +482,14 @@ TEST_F(SplineInterpolationTest, cubic_spline_with_end_point_velocity) // Send the trajectory to the robot sendTrajectory(s_pos, s_vel, std::vector(), s_time); - g_trajectory_running_ = true; + g_trajectory_running = true; // Make sure that the trajectory has started before we start testing for trajectory execution waitForTrajectoryStarted(); double old_spline_travel_time = 0.0; double plot_time = 0.0; - while (g_trajectory_running_) + while (g_trajectory_running) { readDataPackage(data_pkg); ASSERT_TRUE(data_pkg->getData("target_q", joint_positions)); @@ -499,7 +498,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_ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP)); + ASSERT_TRUE(g_ur_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP)); // Read spline travel time from the robot double spline_travel_time = 0.0; @@ -542,7 +541,7 @@ TEST_F(SplineInterpolationTest, cubic_spline_with_end_point_velocity) spline_time.push_back(spline_travel_time); plot_time += step_time_; } - EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS, g_trajectory_result_); + EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS, g_trajectory_result); // Make sure the velocity is zero when the trajectory has finished readDataPackage(data_pkg); @@ -564,8 +563,8 @@ TEST_F(SplineInterpolationTest, cubic_spline_with_end_point_velocity) TEST_F(SplineInterpolationTest, quintic_spline_with_end_point_velocity_with_speedscaling) { // Set speed scaling to 25% to test interpolation with speed scaling active - const unsigned int REDUSE_FACTOR(4); - g_ur_driver_->getRTDEWriter().sendSpeedSlider(1.0 / REDUSE_FACTOR); + const unsigned int reduse_factor(4); + g_ur_driver->getRTDEWriter().sendSpeedSlider(1.0 / reduse_factor); std::unique_ptr data_pkg; readDataPackage(data_pkg); @@ -604,7 +603,7 @@ TEST_F(SplineInterpolationTest, quintic_spline_with_end_point_velocity_with_spee // Send trajectory to the robot sendTrajectory(s_pos, s_vel, s_acc, s_time); - g_trajectory_running_ = true; + g_trajectory_running = true; // Make sure that trajectory has started before we start testing for trajectory execution waitForTrajectoryStarted(); @@ -614,8 +613,8 @@ TEST_F(SplineInterpolationTest, quintic_spline_with_end_point_velocity_with_spee unsigned int loop_count = 0; bool init_acc_test = true; urcl::vector6d_t last_joint_acc = joint_acc, last_change_acc; - const double EPS_ACC_CHANGE(1e-15); - while (g_trajectory_running_) + const double eps_acc_change(1e-15); + while (g_trajectory_running) { readDataPackage(data_pkg); ASSERT_TRUE(data_pkg->getData("target_q", joint_positions)); @@ -635,7 +634,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_ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP)); + ASSERT_TRUE(g_ur_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP)); // Ensure that we follow the joint trajectory urcl::vector6d_t expected_joint_positions, change_acc; @@ -653,7 +652,7 @@ TEST_F(SplineInterpolationTest, quintic_spline_with_end_point_velocity_with_spee last_change_acc.fill(0.0); } - if (loop_count % REDUSE_FACTOR == 0) + if (loop_count % reduse_factor == 0) { last_joint_acc = joint_acc; last_change_acc.fill(0.0); @@ -664,7 +663,7 @@ TEST_F(SplineInterpolationTest, quintic_spline_with_end_point_velocity_with_spee { change_acc[i] = joint_acc[i] - last_joint_acc[i]; - if (!nearly_equal(change_acc[i], 0.0, EPS_ACC_CHANGE) && !nearly_equal(last_change_acc[i], 0.0, EPS_ACC_CHANGE)) + if (!nearlyEqual(change_acc[i], 0.0, eps_acc_change) && !nearlyEqual(last_change_acc[i], 0.0, eps_acc_change)) { // Acceleration should only increase or be constant within one scaled timescale. // It should not fluctuate to zero or overshoot @@ -706,7 +705,7 @@ TEST_F(SplineInterpolationTest, quintic_spline_with_end_point_velocity_with_spee plot_time += step_time_; loop_count += 1; } - EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS, g_trajectory_result_); + EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS, g_trajectory_result); // Make sure the velocity is zero when the trajectory has finished readDataPackage(data_pkg); @@ -767,7 +766,7 @@ TEST_F(SplineInterpolationTest, spline_interpolation_cubic) // Send the trajectory to the robot sendTrajectory(s_pos, s_vel, std::vector(), s_time); - g_trajectory_running_ = true; + g_trajectory_running = true; // Make sure that trajectory has started before we start testing for trajectory execution waitForTrajectoryStarted(); @@ -775,7 +774,7 @@ TEST_F(SplineInterpolationTest, spline_interpolation_cubic) int segment_idx = 0; double old_spline_travel_time = 0.0; double plot_time = 0.0; - while (g_trajectory_running_) + while (g_trajectory_running) { readDataPackage(data_pkg); ASSERT_TRUE(data_pkg->getData("target_q", joint_positions)); @@ -790,7 +789,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_ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP)); + ASSERT_TRUE(g_ur_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP)); if (old_spline_travel_time > spline_travel_time) { @@ -818,7 +817,7 @@ TEST_F(SplineInterpolationTest, spline_interpolation_cubic) spline_time.push_back(spline_travel_time); plot_time += step_time_; } - EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS, g_trajectory_result_); + EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS, g_trajectory_result); // Verify that the full trajectory have been executed double spline_travel_time; @@ -877,7 +876,7 @@ TEST_F(SplineInterpolationTest, spline_interpolation_quintic) // Send the trajectory to the robot sendTrajectory(s_pos, s_vel, s_acc, s_time); - g_trajectory_running_ = true; + g_trajectory_running = true; // Make sure that trajectory has started before we start testing for trajectory execution waitForTrajectoryStarted(); @@ -885,8 +884,8 @@ TEST_F(SplineInterpolationTest, spline_interpolation_quintic) int segment_idx = 0; double old_spline_travel_time = 0.0; double plot_time = 0.0; - g_trajectory_running_ = true; - while (g_trajectory_running_) + g_trajectory_running = true; + while (g_trajectory_running) { readDataPackage(data_pkg); ASSERT_TRUE(data_pkg->getData("target_q", joint_positions)); @@ -901,7 +900,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_ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP)); + ASSERT_TRUE(g_ur_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP)); if (old_spline_travel_time > spline_travel_time) { @@ -928,7 +927,7 @@ TEST_F(SplineInterpolationTest, spline_interpolation_quintic) spline_time.push_back(spline_travel_time); plot_time += step_time_; } - EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS, g_trajectory_result_); + EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS, g_trajectory_result); // Verify that the full trajectory have been executed double spline_travel_time; @@ -948,7 +947,7 @@ TEST_F(SplineInterpolationTest, zero_time_trajectory_cubic_spline) ASSERT_TRUE(data_pkg->getData("target_q", joint_positions_before)); // Start consuming rtde packages to avoid pipeline overflows while testing that the control script aborts correctly - g_consume_rtde_packages_ = true; + g_consume_rtde_packages = true; // Create illegal trajectory std::vector s_pos, s_vel; @@ -970,16 +969,16 @@ TEST_F(SplineInterpolationTest, zero_time_trajectory_cubic_spline) // Send illegal trajectory to the robot sendTrajectory(s_pos, s_vel, std::vector(), s_time); - g_ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP, -1, - RobotReceiveTimeout::off()); + g_ur_driver->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. ASSERT_FALSE(waitForProgramNotRunning(1000)); - EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_CANCELED, g_trajectory_result_); + EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_CANCELED, g_trajectory_result); // Stop consuming rtde packages - g_consume_rtde_packages_ = false; + g_consume_rtde_packages = false; // Ensure that the robot hasn't moved readDataPackage(data_pkg); @@ -1002,17 +1001,17 @@ TEST_F(SplineInterpolationTest, zero_time_trajectory_cubic_spline) s_vel = { zeros, zeros, zeros }; s_time = { 0.0, 1.0, 2.0 }; sendTrajectory(s_pos, s_vel, std::vector(), s_time); - g_trajectory_running_ = true; + g_trajectory_running = true; waitForTrajectoryStarted(); urcl::vector6d_t joint_positions; - while (g_trajectory_running_) + while (g_trajectory_running) { readDataPackage(data_pkg); ASSERT_TRUE(data_pkg->getData("target_q", joint_positions)); // Keep connection alive - ASSERT_TRUE(g_ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP)); + ASSERT_TRUE(g_ur_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP)); } - EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS, g_trajectory_result_); + EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS, g_trajectory_result); ASSERT_TRUE(data_pkg->getData("target_q", joint_positions_after)); for (unsigned int i = 0; i < 6; ++i) @@ -1030,7 +1029,7 @@ TEST_F(SplineInterpolationTest, zero_time_trajectory_quintic_spline) ASSERT_TRUE(data_pkg->getData("target_q", joint_positions_before)); // Start consuming rtde packages to avoid pipeline overflows while testing - g_consume_rtde_packages_ = true; + g_consume_rtde_packages = true; // Create illegal trajectory std::vector s_pos, s_vel, s_acc; @@ -1048,16 +1047,16 @@ TEST_F(SplineInterpolationTest, zero_time_trajectory_quintic_spline) // Send illegal trajectory to the robot sendTrajectory(s_pos, s_vel, s_acc, s_time); - g_ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP, -1, - RobotReceiveTimeout::off()); + g_ur_driver->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 ASSERT_FALSE(waitForProgramNotRunning(1000)); - EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_CANCELED, g_trajectory_result_); + EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_CANCELED, g_trajectory_result); // Stop consuming rtde packages - g_consume_rtde_packages_ = false; + g_consume_rtde_packages = false; // Ensure that the robot hasn't moved readDataPackage(data_pkg); @@ -1081,17 +1080,17 @@ TEST_F(SplineInterpolationTest, zero_time_trajectory_quintic_spline) s_acc = { zeros, zeros, zeros }; s_time = { 0.0, 1.0, 2.0 }; sendTrajectory(s_pos, s_vel, s_acc, s_time); - g_trajectory_running_ = true; + g_trajectory_running = true; waitForTrajectoryStarted(); urcl::vector6d_t joint_positions; - while (g_trajectory_running_) + while (g_trajectory_running) { readDataPackage(data_pkg); ASSERT_TRUE(data_pkg->getData("target_q", joint_positions)); // Keep connection alive - ASSERT_TRUE(g_ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP)); + ASSERT_TRUE(g_ur_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP)); } - EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS, g_trajectory_result_); + EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS, g_trajectory_result); ASSERT_TRUE(data_pkg->getData("target_q", joint_positions_after)); for (unsigned int i = 0; i < 6; ++i) @@ -1109,7 +1108,7 @@ TEST_F(SplineInterpolationTest, physically_unfeasible_trajectory_cubic_spline) ASSERT_TRUE(data_pkg->getData("target_q", joint_positions_before)); // Start consuming rtde packages to avoid pipeline overflows while testing that the control script aborts correctly - g_consume_rtde_packages_ = true; + g_consume_rtde_packages = true; // Create a trajectory that cannot be executed within the robots limits std::vector s_pos, s_vel; @@ -1126,16 +1125,16 @@ TEST_F(SplineInterpolationTest, physically_unfeasible_trajectory_cubic_spline) // Send unfeasible trajectory to the robot sendTrajectory(s_pos, s_vel, std::vector(), s_time); - g_ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP, -1, - RobotReceiveTimeout::off()); + g_ur_driver->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 ASSERT_FALSE(waitForProgramNotRunning(1000)); - EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_CANCELED, g_trajectory_result_); + EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_CANCELED, g_trajectory_result); // Stop consuming rtde packages - g_consume_rtde_packages_ = false; + g_consume_rtde_packages = false; // Ensure that the robot hasn't moved readDataPackage(data_pkg); @@ -1156,7 +1155,7 @@ TEST_F(SplineInterpolationTest, physically_unfeasible_trajectory_quintic_spline) ASSERT_TRUE(data_pkg->getData("target_q", joint_positions_before)); // Start consuming rtde packages to avoid pipeline overflows while testing that the control script aborts correctly - g_consume_rtde_packages_ = true; + g_consume_rtde_packages = true; // Create a trajectory that cannot be executed within the robots limits std::vector s_pos, s_vel, s_acc; @@ -1174,16 +1173,16 @@ TEST_F(SplineInterpolationTest, physically_unfeasible_trajectory_quintic_spline) // Send unfeasible trajectory to the robot sendTrajectory(s_pos, s_vel, s_acc, s_time); - g_ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP, -1, - RobotReceiveTimeout::off()); + g_ur_driver->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 ASSERT_FALSE(waitForProgramNotRunning(1000)); - EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_CANCELED, g_trajectory_result_); + EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_CANCELED, g_trajectory_result); // Stop consuming rtde packages - g_consume_rtde_packages_ = false; + g_consume_rtde_packages = false; // Ensure that the robot hasn't moved readDataPackage(data_pkg); @@ -1203,7 +1202,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; } } diff --git a/tests/test_stream.cpp b/tests/test_stream.cpp index d095b41d..6368d0d8 100644 --- a/tests/test_stream.cpp +++ b/tests/test_stream.cpp @@ -51,7 +51,7 @@ class StreamTest : public ::testing::Test server_->start(); } - void Teardown() + void teardown() { // Clean up server_.reset(); diff --git a/tests/test_ur_driver.cpp b/tests/test_ur_driver.cpp index 3bfcacc0..8ec2b989 100644 --- a/tests/test_ur_driver.cpp +++ b/tests/test_ur_driver.cpp @@ -40,16 +40,16 @@ 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"; -std::unique_ptr g_ur_driver_; -std::unique_ptr g_dashboard_client_; +std::unique_ptr g_ur_driver; +std::unique_ptr g_dashboard_client; bool g_program_running; -std::condition_variable g_program_not_running_cv_; -std::mutex g_program_not_running_mutex_; -std::condition_variable g_program_running_cv_; -std::mutex g_program_running_mutex_; +std::condition_variable g_program_not_running_cv; +std::mutex g_program_not_running_mutex; +std::condition_variable g_program_running_cv; +std::mutex g_program_running_mutex; // Helper functions for the driver void handleRobotProgramState(bool program_running) @@ -58,33 +58,33 @@ void handleRobotProgramState(bool program_running) 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_); + std::lock_guard lk(g_program_running_mutex); g_program_running = program_running; - g_program_running_cv_.notify_one(); + g_program_running_cv.notify_one(); } else { - std::lock_guard lk(g_program_not_running_mutex_); + std::lock_guard lk(g_program_not_running_mutex); g_program_running = program_running; - g_program_not_running_cv_.notify_one(); + g_program_not_running_cv.notify_one(); } } -bool g_rtde_read_thread_running_ = false; -bool g_consume_rtde_packages_ = false; -std::mutex g_read_package_mutex_; +bool g_rtde_read_thread_running = false; +bool g_consume_rtde_packages = false; +std::mutex g_read_package_mutex; std::thread g_rtde_read_thread; void rtdeConsumeThread() { - while (g_rtde_read_thread_running_) + while (g_rtde_read_thread_running) { // Consume package to prevent pipeline overflow - if (g_consume_rtde_packages_ == true) + if (g_consume_rtde_packages == true) { - std::lock_guard lk(g_read_package_mutex_); + std::lock_guard lk(g_read_package_mutex); std::unique_ptr data_pkg; - data_pkg = g_ur_driver_->getDataPackage(); + data_pkg = g_ur_driver->getDataPackage(); } else { @@ -98,76 +98,76 @@ class UrDriverTest : public ::testing::Test protected: static void SetUpTestSuite() { - g_dashboard_client_.reset(new DashboardClient(ROBOT_IP)); - ASSERT_TRUE(g_dashboard_client_->connect()); + g_dashboard_client.reset(new DashboardClient(g_ROBOT_IP)); + ASSERT_TRUE(g_dashboard_client->connect()); // Make robot ready for test timeval tv; tv.tv_sec = 10; tv.tv_usec = 0; - g_dashboard_client_->setReceiveTimeout(tv); + g_dashboard_client->setReceiveTimeout(tv); // Stop running program if there is one - ASSERT_TRUE(g_dashboard_client_->commandStop()); + ASSERT_TRUE(g_dashboard_client->commandStop()); // if the robot is not powered on and ready - ASSERT_TRUE(g_dashboard_client_->commandBrakeRelease()); + ASSERT_TRUE(g_dashboard_client->commandBrakeRelease()); // Setup driver std::unique_ptr tool_comm_setup; - const bool HEADLESS = true; + const bool headless = true; try { - g_ur_driver_.reset(new UrDriver(ROBOT_IP, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, &handleRobotProgramState, - HEADLESS, std::move(tool_comm_setup), CALIBRATION_CHECKSUM)); + g_ur_driver.reset(new UrDriver(g_ROBOT_IP, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, &handleRobotProgramState, + headless, std::move(tool_comm_setup), CALIBRATION_CHECKSUM)); } catch (UrException& exp) { std::cout << "caught exception " << exp.what() << " while launch driver, retrying once in 10 seconds" << std::endl; std::this_thread::sleep_for(std::chrono::seconds(10)); - g_ur_driver_.reset(new UrDriver(ROBOT_IP, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, &handleRobotProgramState, - HEADLESS, std::move(tool_comm_setup), CALIBRATION_CHECKSUM)); + g_ur_driver.reset(new UrDriver(g_ROBOT_IP, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, &handleRobotProgramState, + headless, std::move(tool_comm_setup), CALIBRATION_CHECKSUM)); } - g_ur_driver_->startRTDECommunication(); + g_ur_driver->startRTDECommunication(); // Setup rtde read thread - g_rtde_read_thread_running_ = true; + g_rtde_read_thread_running = true; g_rtde_read_thread = std::thread(rtdeConsumeThread); } void SetUp() { step_time_ = 0.002; - if (g_ur_driver_->getVersion().major < 5) + if (g_ur_driver->getVersion().major < 5) { step_time_ = 0.008; } // Make sure script is running on the robot if (g_program_running == false) { - g_consume_rtde_packages_ = true; - g_ur_driver_->sendRobotProgram(); + g_consume_rtde_packages = true; + g_ur_driver->sendRobotProgram(); ASSERT_TRUE(waitForProgramRunning(1000)); } - g_consume_rtde_packages_ = false; + g_consume_rtde_packages = false; } static void TearDownTestSuite() { - g_rtde_read_thread_running_ = false; + g_rtde_read_thread_running = false; g_rtde_read_thread.join(); - g_dashboard_client_->disconnect(); + g_dashboard_client->disconnect(); } void readDataPackage(std::unique_ptr& data_pkg) { - if (g_consume_rtde_packages_ == true) + if (g_consume_rtde_packages == true) { URCL_LOG_ERROR("Unable to read packages while consuming, this should not happen!"); GTEST_FAIL(); } - std::lock_guard lk(g_read_package_mutex_); - data_pkg = g_ur_driver_->getDataPackage(); + std::lock_guard lk(g_read_package_mutex); + data_pkg = g_ur_driver->getDataPackage(); if (data_pkg == nullptr) { URCL_LOG_ERROR("Timed out waiting for a new package from the robot"); @@ -177,8 +177,8 @@ class UrDriverTest : public ::testing::Test 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 || + 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; @@ -188,8 +188,8 @@ class UrDriverTest : public ::testing::Test bool waitForProgramNotRunning(int milliseconds = 100) { - std::unique_lock lk(g_program_not_running_mutex_); - if (g_program_not_running_cv_.wait_for(lk, std::chrono::milliseconds(milliseconds)) == std::cv_status::no_timeout || + std::unique_lock lk(g_program_not_running_mutex); + if (g_program_not_running_cv.wait_for(lk, std::chrono::milliseconds(milliseconds)) == std::cv_status::no_timeout || g_program_running == false) { return true; @@ -203,14 +203,14 @@ class UrDriverTest : public ::testing::Test TEST_F(UrDriverTest, read_non_existing_script_file) { - g_consume_rtde_packages_ = true; + g_consume_rtde_packages = true; const std::string non_existing_script_file = ""; EXPECT_THROW(UrDriver::readScriptFile(non_existing_script_file), UrException); } TEST_F(UrDriverTest, read_existing_script_file) { - g_consume_rtde_packages_ = true; + g_consume_rtde_packages = true; char existing_script_file[] = "urscript.XXXXXX"; int fd = mkstemp(existing_script_file); if (fd == -1) @@ -227,73 +227,73 @@ TEST_F(UrDriverTest, read_existing_script_file) TEST_F(UrDriverTest, robot_receive_timeout) { - g_consume_rtde_packages_ = true; + g_consume_rtde_packages = true; // 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_ur_driver_->writeJointCommand(zeros, comm::ControlMode::MODE_IDLE, RobotReceiveTimeout::millisec(200)); + g_ur_driver->writeJointCommand(zeros, comm::ControlMode::MODE_IDLE, RobotReceiveTimeout::millisec(200)); EXPECT_TRUE(waitForProgramNotRunning(400)); // Start robot program - g_ur_driver_->sendRobotProgram(); + g_ur_driver->sendRobotProgram(); EXPECT_TRUE(waitForProgramRunning(1000)); // Robot program should time out after the robot receive timeout, whether it takes exactly 200 ms is not so important - g_ur_driver_->writeFreedriveControlMessage(control::FreedriveControlMessage::FREEDRIVE_NOOP, - RobotReceiveTimeout::millisec(200)); + g_ur_driver->writeFreedriveControlMessage(control::FreedriveControlMessage::FREEDRIVE_NOOP, + RobotReceiveTimeout::millisec(200)); EXPECT_TRUE(waitForProgramNotRunning(400)); // Start robot program - g_ur_driver_->sendRobotProgram(); + g_ur_driver->sendRobotProgram(); EXPECT_TRUE(waitForProgramRunning(1000)); // Robot program should time out after the robot receive timeout, whether it takes exactly 200 ms is not so important - g_ur_driver_->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_NOOP, -1, - RobotReceiveTimeout::millisec(200)); + g_ur_driver->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_NOOP, -1, + RobotReceiveTimeout::millisec(200)); EXPECT_TRUE(waitForProgramNotRunning(400)); // Start robot program - g_ur_driver_->sendRobotProgram(); + g_ur_driver->sendRobotProgram(); EXPECT_TRUE(waitForProgramRunning(1000)); // Robot program should time out after the robot receive timeout, whether it takes exactly 200 ms is not so important - g_ur_driver_->writeKeepalive(RobotReceiveTimeout::millisec(200)); + g_ur_driver->writeKeepalive(RobotReceiveTimeout::millisec(200)); EXPECT_TRUE(waitForProgramNotRunning(400)); } TEST_F(UrDriverTest, robot_receive_timeout_off) { - g_consume_rtde_packages_ = true; + g_consume_rtde_packages = true; // Program should keep running when setting receive timeout off - g_ur_driver_->writeKeepalive(RobotReceiveTimeout::off()); + g_ur_driver->writeKeepalive(RobotReceiveTimeout::off()); EXPECT_FALSE(waitForProgramNotRunning(1000)); // Program should keep running when setting receive timeout off - g_ur_driver_->writeFreedriveControlMessage(control::FreedriveControlMessage::FREEDRIVE_NOOP, - RobotReceiveTimeout::off()); + g_ur_driver->writeFreedriveControlMessage(control::FreedriveControlMessage::FREEDRIVE_NOOP, + RobotReceiveTimeout::off()); EXPECT_FALSE(waitForProgramNotRunning(1000)); // Program should keep running when setting receive timeout off - g_ur_driver_->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_NOOP, -1, - RobotReceiveTimeout::off()); + g_ur_driver->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_NOOP, -1, + RobotReceiveTimeout::off()); EXPECT_FALSE(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_ur_driver_->writeJointCommand(zeros, comm::ControlMode::MODE_SPEEDJ, RobotReceiveTimeout::off()); + g_ur_driver->writeJointCommand(zeros, comm::ControlMode::MODE_SPEEDJ, RobotReceiveTimeout::off()); EXPECT_TRUE(waitForProgramNotRunning(400)); } TEST_F(UrDriverTest, stop_robot_control) { - g_consume_rtde_packages_ = true; + g_consume_rtde_packages = true; vector6d_t zeros = { 0, 0, 0, 0, 0, 0 }; - g_ur_driver_->writeJointCommand(zeros, comm::ControlMode::MODE_IDLE, RobotReceiveTimeout::off()); + g_ur_driver->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_ur_driver_->stopControl(); + g_ur_driver->stopControl(); EXPECT_TRUE(waitForProgramNotRunning(400)); } @@ -311,7 +311,7 @@ TEST_F(UrDriverTest, target_outside_limits_servoj) // Send unfeasible targets to the robot readDataPackage(data_pkg); - g_ur_driver_->writeJointCommand(joint_target, comm::ControlMode::MODE_SERVOJ, RobotReceiveTimeout::millisec(200)); + g_ur_driver->writeJointCommand(joint_target, comm::ControlMode::MODE_SERVOJ, RobotReceiveTimeout::millisec(200)); // Ensure that the robot didn't move readDataPackage(data_pkg); @@ -323,8 +323,8 @@ TEST_F(UrDriverTest, target_outside_limits_servoj) } // Make sure the program is stopped - g_consume_rtde_packages_ = true; - g_ur_driver_->stopControl(); + g_consume_rtde_packages = true; + g_ur_driver->stopControl(); waitForProgramNotRunning(1000); } @@ -342,7 +342,7 @@ TEST_F(UrDriverTest, target_outside_limits_pose) // Send unfeasible targets to the robot readDataPackage(data_pkg); - g_ur_driver_->writeJointCommand(tcp_target, comm::ControlMode::MODE_POSE, RobotReceiveTimeout::millisec(200)); + g_ur_driver->writeJointCommand(tcp_target, comm::ControlMode::MODE_POSE, RobotReceiveTimeout::millisec(200)); // Ensure that the robot didn't move readDataPackage(data_pkg); @@ -354,29 +354,29 @@ TEST_F(UrDriverTest, target_outside_limits_pose) } // Make sure the program is stopped - g_consume_rtde_packages_ = true; - g_ur_driver_->stopControl(); + g_consume_rtde_packages = true; + g_ur_driver->stopControl(); waitForProgramNotRunning(1000); } TEST_F(UrDriverTest, send_robot_program_retry_on_failure) { // Start robot program - g_ur_driver_->sendRobotProgram(); + g_ur_driver->sendRobotProgram(); EXPECT_TRUE(waitForProgramRunning(1000)); // Check that sendRobotProgram is robust to the secondary stream being disconnected. This is what happens when // switching from Remote to Local and back to Remote mode for example. - g_ur_driver_->secondary_stream_->close(); + g_ur_driver->secondary_stream_->close(); - EXPECT_TRUE(g_ur_driver_->sendRobotProgram()); + EXPECT_TRUE(g_ur_driver->sendRobotProgram()); } TEST_F(UrDriverTest, reset_rtde_client) { double target_frequency = 50; - g_ur_driver_->resetRTDEClient(OUTPUT_RECIPE, INPUT_RECIPE, target_frequency); - ASSERT_EQ(g_ur_driver_->getControlFrequency(), target_frequency); + g_ur_driver->resetRTDEClient(OUTPUT_RECIPE, INPUT_RECIPE, target_frequency); + ASSERT_EQ(g_ur_driver->getControlFrequency(), target_frequency); } // TODO we should add more tests for the UrDriver class. @@ -389,7 +389,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; } }