diff --git a/.github/workflows/industrial_ci_humble_action.yml b/.github/workflows/industrial_ci_humble_action.yml index 7c1c1d9..7e3308b 100644 --- a/.github/workflows/industrial_ci_humble_action.yml +++ b/.github/workflows/industrial_ci_humble_action.yml @@ -19,6 +19,6 @@ jobs: - {ROS_DISTRO: humble, ROS_REPO: main} runs-on: ubuntu-latest steps: - - uses: actions/checkout@v1 + - uses: actions/checkout@v4 - uses: 'ros-industrial/industrial_ci@master' env: ${{matrix.env}} diff --git a/.github/workflows/industrial_ci_iron_action.yml b/.github/workflows/industrial_ci_iron_action.yml index 4e518c0..3d1ffd4 100644 --- a/.github/workflows/industrial_ci_iron_action.yml +++ b/.github/workflows/industrial_ci_iron_action.yml @@ -19,6 +19,6 @@ jobs: - {ROS_DISTRO: iron, ROS_REPO: main} runs-on: ubuntu-latest steps: - - uses: actions/checkout@v1 + - uses: actions/checkout@v4 - uses: 'ros-industrial/industrial_ci@master' env: ${{matrix.env}} diff --git a/.github/workflows/industrial_ci_jazzy_action.yml b/.github/workflows/industrial_ci_jazzy_action.yml new file mode 100644 index 0000000..ec906f8 --- /dev/null +++ b/.github/workflows/industrial_ci_jazzy_action.yml @@ -0,0 +1,24 @@ +name: Jazzy + +on: + push: + pull_request: + schedule: + # Run every Friday at 6:30 am to detect breaking APIs + - cron: '30 6 * * 5' + + +jobs: + industrial_ci: + env: + BEFORE_BUILD_TARGET_WORKSPACE: '.github/script/install_dependencies.sh' + strategy: + fail-fast: false + matrix: + env: + - {ROS_DISTRO: jazzy, ROS_REPO: main} + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + - uses: 'ros-industrial/industrial_ci@master' + env: ${{matrix.env}} diff --git a/.github/workflows/industrial_ci_rolling_action.yml b/.github/workflows/industrial_ci_rolling_action.yml index 78a21f9..6868f50 100644 --- a/.github/workflows/industrial_ci_rolling_action.yml +++ b/.github/workflows/industrial_ci_rolling_action.yml @@ -20,6 +20,6 @@ jobs: - {ROS_DISTRO: rolling, ROS_REPO: main} runs-on: ubuntu-latest steps: - - uses: actions/checkout@v1 + - uses: actions/checkout@v4 - uses: 'ros-industrial/industrial_ci@master' env: ${{matrix.env}} diff --git a/README.md b/README.md index 50dad2f..1c25ce4 100644 --- a/README.md +++ b/README.md @@ -14,6 +14,9 @@ build badge iron + + build badge jazzy + build badge rolling @@ -82,7 +85,7 @@ a) Using a component manager – this process is also outlined in the launch fil b) Within your custom executable, coupled with a (multithreaded-)executor. -If you prefer starting the node in your main function, ensure that you include `Schunk::schunk_egu_egk_gripper_driver` in CMake's `target_link_libraries()`. Additionally, always specify the IP address, setting the parameter overrides "IP" to your designated IP. Alternatively, create the `SchunkGripperNode`-Component and utilize the `reconnect` service, specifying your IP. +If you prefer starting the node in your main function, ensure that you include `Schunk::schunk_egu_egk_gripper_driver` in CMake's `target_link_libraries()`. Additionally, always specify the IP address, setting the parameter overrides "IP" to your designated IP. ## Actions All functionalities of the gripper, including movement, are treated as actions. This implies that when gripping, moving, or releasing a workpiece, you need to send a goal and can receive a result or feedback. Releasing a workpiece is the only action where you send an empty goal: @@ -108,10 +111,8 @@ Services are functionalities that do not involve movement or occur so rapidly th - `softreset` - `parameter_get` - `parameter_set` -- `reconnect` - `release_for_manual_movement` - `prepare_for_shutdown` -- `gripper_info` **Important:** During a soft reset, no topics will be published. This will last for approximately 7 seconds. Afterward, all publications resume, and you can modify parameters. @@ -121,10 +122,6 @@ Services are functionalities that do not involve movement or occur so rapidly th All other services can be used whenever you like. (**Note:** Fast stop is an abort of movement, so it always provokes an error). -`gripper_info` publishes some information about the gripper on the terminal screen. - -`reconnect` is the only method for altering the IP address during runtime. If nothing is connected to the IP address or a gripper is connected, it undergoes a change. If something else is linked to this IP, errors will occur, and the old address will be retained in such cases. Exercise caution when using this service! - With `parameter_get` and `parameter_set` you can read and set all allowed Parameter of the gripper. For getting and setting you need always the parameter instance. After that ## Parameters diff --git a/schunk_egu_egk_gripper_driver/include/schunk_egu_egk_gripper_driver/schunk_gripper_wrapper.hpp b/schunk_egu_egk_gripper_driver/include/schunk_egu_egk_gripper_driver/schunk_gripper_wrapper.hpp index f6924fa..dd9173f 100644 --- a/schunk_egu_egk_gripper_driver/include/schunk_egu_egk_gripper_driver/schunk_gripper_wrapper.hpp +++ b/schunk_egu_egk_gripper_driver/include/schunk_egu_egk_gripper_driver/schunk_gripper_wrapper.hpp @@ -47,7 +47,6 @@ #include "schunk_egu_egk_gripper_interfaces/action/release_workpiece.hpp" #include "schunk_egu_egk_gripper_interfaces/srv/release_for_manual_movement.hpp" #include "schunk_egu_egk_gripper_interfaces/srv/gripper_info.hpp" -#include "schunk_egu_egk_gripper_interfaces/srv/change_ip.hpp" #include "schunk_egu_egk_gripper_interfaces/action/grip.hpp" #include "schunk_egu_egk_gripper_interfaces/srv/parameter_get.hpp" #include "schunk_egu_egk_gripper_interfaces/srv/parameter_set.hpp" @@ -70,7 +69,6 @@ class SchunkGripperNode : public rclcpp::Node, public Gripper using Softreset = schunk_egu_egk_gripper_interfaces::srv::Softreset; using PrepareForShutdown = schunk_egu_egk_gripper_interfaces::srv::PrepareForShutdown; using GripperInfo= schunk_egu_egk_gripper_interfaces::srv::GripperInfo; - using ChangeIp = schunk_egu_egk_gripper_interfaces::srv::ChangeIp; using ParameterGet = schunk_egu_egk_gripper_interfaces::srv::ParameterGet; using ParameterSet = schunk_egu_egk_gripper_interfaces::srv::ParameterSet; @@ -145,11 +143,10 @@ class SchunkGripperNode : public rclcpp::Node, public Gripper void fast_stop_srv(const std::shared_ptr, std::shared_ptr); void parameter_get_srv(const std::shared_ptr, std::shared_ptr); void parameter_set_srv(const std::shared_ptr, std::shared_ptr); - void change_ip_srv(const std::shared_ptr, std::shared_ptr); void releaseForManualMov_srv(const std::shared_ptr, std::shared_ptr); void softreset_srv(const std::shared_ptr, std::shared_ptr); void prepare_for_shutdown_srv(const std::shared_ptr, std::shared_ptr); - void info_srv(const std::shared_ptr, std::shared_ptr); + void info_srv(const std::shared_ptr, std::shared_ptr res); //Action-basic-functions template @@ -198,7 +195,6 @@ class SchunkGripperNode : public rclcpp::Node, public Gripper rclcpp::CallbackGroup::SharedPtr messages_group; rclcpp::CallbackGroup::SharedPtr services_group; rclcpp::CallbackGroup::SharedPtr actions_group; - rclcpp::CallbackGroup::SharedPtr rest; public: @@ -224,7 +220,6 @@ class SchunkGripperNode : public rclcpp::Node, public Gripper rclcpp::Service::SharedPtr softreset_service; rclcpp::Service::SharedPtr prepare_for_shutdown_service; rclcpp::Service::SharedPtr info_service; - rclcpp::Service::SharedPtr change_ip_service; }; #endif diff --git a/schunk_egu_egk_gripper_driver/src/schunk_gripper_wrapper.cpp b/schunk_egu_egk_gripper_driver/src/schunk_gripper_wrapper.cpp index 5758742..6dd8db6 100644 --- a/schunk_egu_egk_gripper_driver/src/schunk_gripper_wrapper.cpp +++ b/schunk_egu_egk_gripper_driver/src/schunk_gripper_wrapper.cpp @@ -55,13 +55,10 @@ SchunkGripperNode::SchunkGripperNode(const rclcpp::NodeOptions &options) : services_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); // actions_group = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); - // rest = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); rclcpp::PublisherOptions option_messages; option_messages.callback_group = messages_group; - rclcpp::SubscriptionOptions option; - option.callback_group = rest; //ParameterDescription read_only rcl_interfaces::msg::ParameterDescriptor paramDesc; paramDesc.read_only = true; @@ -162,8 +159,7 @@ void SchunkGripperNode::advertiseServices() fast_stop_service = this->create_service("fast_stop", std::bind(&SchunkGripperNode::fast_stop_srv,this,std::placeholders::_1,std::placeholders::_2), rmw_qos_profile_services_default, services_group); parameter_get_service = this->create_service("parameter_get", std::bind(&SchunkGripperNode::parameter_get_srv,this,std::placeholders::_1,std::placeholders::_2), rmw_qos_profile_services_default, services_group); parameter_set_service= this->create_service("parameter_set", std::bind(&SchunkGripperNode::parameter_set_srv,this,std::placeholders::_1,std::placeholders::_2), rmw_qos_profile_services_default, services_group); - info_service = this->create_service("gripper_info", std::bind(&SchunkGripperNode::info_srv,this,std::placeholders::_1,std::placeholders::_2), rmw_qos_profile_services_default, rest); - change_ip_service = this->create_service("reconnect", std::bind(&SchunkGripperNode::change_ip_srv,this,std::placeholders::_1,std::placeholders::_2), rmw_qos_profile_services_default, services_group); + info_service = this->create_service("gripper_info", std::bind(&SchunkGripperNode::info_srv,this,std::placeholders::_1,std::placeholders::_2), rmw_qos_profile_services_default, services_group); } //Advertise Actions @@ -1195,7 +1191,7 @@ void SchunkGripperNode::brake_test_srv(const std::shared_ptr } if((gripperBitInput(SUCCESS) == true) && (handshake != gripperBitInput(COMMAND_RECEIVED_TOGGLE))) { - res->brake_test_successful = true; + res->success = true; res->error_code = error_str; RCLCPP_INFO(this->get_logger(),"Brake test successful!"); } @@ -1204,7 +1200,7 @@ void SchunkGripperNode::brake_test_srv(const std::shared_ptr runGets(); gripper_updater->force_update(); res->error_code = error_str; - res->brake_test_successful = false; + res->success = false; RCLCPP_INFO(this->get_logger(),"Brake test failed!"); } @@ -1213,53 +1209,12 @@ void SchunkGripperNode::brake_test_srv(const std::shared_ptr { connection_error = server_response; res->error_code = server_response; - res->brake_test_successful = false; + res->success = false; RCLCPP_ERROR(this->get_logger(), "Failed Connection! %s", connection_error.c_str()); } } -//Ip change service, if the ip should change during runtime of the program -void SchunkGripperNode::change_ip_srv(const std::shared_ptr req, std::shared_ptr res) -{ - try - { - std::lock_guard lock(lock_mutex); - res->ip_changed = changeIp(req->new_ip); - - if(res->ip_changed == false) - { - connection_error = "No gripper found. Using old IP"; - gripper_updater->force_update(); - return; - } - - if(res->ip_changed == true) - { - this->set_parameter(rclcpp::Parameter("IP", req->new_ip)); - } - if(start_connection == false && ip_changed_with_all_param == true) - { - advertiseConnectionRelevant(); - start_connection = true; - } - - if(ip_changed_with_all_param == true) - { - this->set_parameter(rclcpp::Parameter("model", model)); - - if(connection_error == "OK") - { - reconnect(); - } - } - } - catch(...) - { - RCLCPP_ERROR(this->get_logger(), "Handshake failed!"); - } - -} //Stop service callback void SchunkGripperNode::stop_srv(const std::shared_ptr, std::shared_ptr res) { @@ -1290,12 +1245,12 @@ void SchunkGripperNode::stop_srv(const std::shared_ptr, std::shar if((gripperBitInput(SUCCESS) == 1) && (handshake != gripperBitInput(COMMAND_RECEIVED_TOGGLE))) { - res->stopped = 1; + res->success = true; RCLCPP_WARN(this->get_logger(),"Stopped!"); } else { - res->stopped = 0; + res->success = false; last_command = 0; } @@ -1323,12 +1278,12 @@ void SchunkGripperNode::fast_stop_srv(const std::shared_ptr, if(gripperBitInput(GRIPPER_ERROR)) { - res->fast_stopped = 1; + res->success = true; RCLCPP_WARN(this->get_logger(),"Fast stopped!"); } else { - res->fast_stopped = 0; + res->success = false; RCLCPP_WARN(this->get_logger(),"Fast stop failed!"); } } @@ -1336,7 +1291,7 @@ void SchunkGripperNode::fast_stop_srv(const std::shared_ptr, { connection_error = server_err; RCLCPP_ERROR(this->get_logger(), "Failed Connection! %s", connection_error.c_str()); - res->fast_stopped = 0; + res->success = false; RCLCPP_WARN(this->get_logger(), "Fast stop failed!"); } gripper_updater->force_update(); @@ -1345,59 +1300,40 @@ void SchunkGripperNode::fast_stop_srv(const std::shared_ptr, void SchunkGripperNode::softreset_srv(const std::shared_ptr, std::shared_ptr res) { std::unique_lock lock(lock_mutex, std::defer_lock); + getParameter(SYSTEM_UPTIME_INST, 1, UINT32_DATA); + const auto uptime_before_reset = uint32_vector[0]; + auto uptime = uptime_before_reset; + + // Detect soft resets by checking the system uptime. + auto system_reset = [&](){ + try { + getParameter(SYSTEM_UPTIME_INST, 1, UINT32_DATA); + uptime = uint32_vector[0]; + } + catch(const char* error){} + return (uptime < uptime_before_reset) ? true : false; + }; + try { RCLCPP_INFO(this->get_logger(),"SOFTRESET"); - std::unique_lock lock_service(lock_service_post); - set_command = SOFT_RESET; - - sendService(lock); lock_service.unlock(); } - catch(const char* res){} - - bool connection_once_lost = false; - - if(handshake != gripperBitInput(COMMAND_RECEIVED_TOGGLE)) - { - for(int i = 0; rclcpp::ok() && i <= 1000 && check(); i++) - { - try - { - runGets(); //if connection is back, don't catch - if(connection_once_lost == true) - { - connection_error = "OK"; - break; - } - } - catch(const char* res) - { - connection_error = res; - std::chrono::milliseconds sleep_time(10); - std::this_thread::sleep_for(sleep_time); - connection_once_lost = true; - } - } + catch(const char* error){ + RCLCPP_INFO(this->get_logger(), "Softreset failed!"); + res->success = false; } - else RCLCPP_WARN(this->get_logger(), "Handshake failed!"); - if(connection_once_lost == true && rclcpp::ok() && connection_error == "OK") + while (!system_reset()) { - RCLCPP_INFO(this->get_logger(), "Softreset succeeded!"); - res->reset_success = true; - } - else - { - RCLCPP_INFO(this->get_logger(), "Softreset failed!"); - res->reset_success = false; + runGets(); } + res->success = true; lock.unlock(); gripper_updater->force_update(); - } //Prepare for shutdown service callback void SchunkGripperNode::prepare_for_shutdown_srv(const std::shared_ptr, std::shared_ptr res) @@ -1428,12 +1364,12 @@ void SchunkGripperNode::prepare_for_shutdown_srv(const std::shared_ptrget_logger(),"READY FOR SHUTDOWN"); - res->prepared_for_shutdown = true; + res->success = true; } else { RCLCPP_INFO(this->get_logger(),"COMMAND FAILED"); - res->prepared_for_shutdown = false; + res->success = false; } gripper_updater->force_update(); @@ -1493,30 +1429,33 @@ void SchunkGripperNode::releaseForManualMov_srv(const std::shared_ptrget_logger(),"YOU CAN TAKE THE WORKPIECE!\n"); RCLCPP_WARN(this->get_logger(),"If you want to end this mode, perform a fast stop and acknowledge!"); - res->released_for_manual_movement = true; + res->success = true; } else { RCLCPP_WARN(this->get_logger(),"COMMAND FAILED"); - res->released_for_manual_movement = false; + res->success = false; } gripper_updater->force_update(); } //Get infos of the gripper service -void SchunkGripperNode::info_srv(const std::shared_ptr, std::shared_ptr) +void SchunkGripperNode::info_srv(const std::shared_ptr, std::shared_ptr res) { + auto info_stream = std::stringstream(); const std::lock_guard lock(lock_mutex); try { - if(wrong_version) RCLCPP_WARN_STREAM(this->get_logger(),"Using not suitable software-version. Some informations may be misleading"); - + if(wrong_version) + { + info_stream << "Using not suitable software-version. Some informations may be misleading"; + } std::vector char_strings; uint32_t data; uint16_t data2; float data3; - RCLCPP_INFO_STREAM(this->get_logger(),"\n\n\nGRIPPER TYPE: " << model.c_str() - << "\nIP: " << ip << std::endl); + info_stream << "\n\n\nGRIPPER TYPE: " << model.c_str() + << "\nIP: " << ip << std::endl; getEnums(FIELDBUS_TYPE_INST,fieldbus_type); getParameter(MAC_ADDR_INST, 6, CHAR_DATA); @@ -1524,10 +1463,9 @@ void SchunkGripperNode::info_srv(const std::shared_ptr, st for(size_t i = 0; i < 6; i++){mac[i] = static_cast(static_cast(char_vector.at(i)));} - RCLCPP_INFO_STREAM(this->get_logger(), - "\nFieldbustype: " << json_data["string"] + info_stream << "\nFieldbustype: " << json_data["string"] << "\nMac-address: " << std::hex << mac[0] << ":" << mac[1] << ":" << mac[2] << ":" << mac[3] << ":" - << mac[4] << ":" << mac[5] << std::endl); + << mac[4] << ":" << mac[5] << std::endl; //Get the char-Parameter and save them as strings in char_strings @@ -1545,64 +1483,64 @@ void SchunkGripperNode::info_srv(const std::shared_ptr, st getWithInstance(SERIAL_NO_NUM_INST, &data); getWithInstance(SW_VERSION_NUM_INST, &data2); - RCLCPP_INFO_STREAM(this->get_logger(),"\nSerial number text: " << char_strings[0] + info_stream << "\nSerial number text: " << char_strings[0] <<"\nOrder number text: " << char_strings[1] <<"\nDevice serial number encoded: " << data <<"\nMain software build date: " << char_strings[2] <<"\nMain software build time: " << char_strings[3] <<"\nMain software version short: " << data2 <<"\nMain software version: " << char_strings[4] - <<"\nCommunication software version: " << comm_version << std::endl); + <<"\nCommunication software version: " << comm_version << std::endl; getWithInstance(UPTIME_INST, &data); - RCLCPP_INFO_STREAM(this->get_logger(),"\nSystem uptime: " << data << " s" << std::endl); + info_stream << "\nSystem uptime: " << data << " s" << std::endl; getWithInstance(DEAD_LOAD_KG_INST, &data3); - RCLCPP_INFO_STREAM(this->get_logger(),"\nNet mass of the Gripper: " << data3 << " kg" << std::endl); + info_stream << "\nNet mass of the Gripper: " << data3 << " kg" << std::endl; getParameter(TOOL_CENT_POINT_INST, 6, FLOAT_DATA); - RCLCPP_INFO_STREAM(this->get_logger(),"\nTool center point 6D-Frame: \n" + info_stream << "\nTool center point 6D-Frame: \n" << float_vector[0] << " mm " << float_vector[1] << " mm " << float_vector[2] << " mm\n" - << float_vector[3] << " " << float_vector[4] << " " << float_vector[5] << std::endl); + << float_vector[3] << " " << float_vector[4] << " " << float_vector[5] << std::endl; getParameter(CENT_OF_MASS_INST, 6, FLOAT_DATA); - RCLCPP_INFO_STREAM(this->get_logger(), "\nCenter of Mass 6D-frame: \n" + info_stream << "\nCenter of Mass 6D-frame: \n" << float_vector[0] << " mm " << float_vector[1] << " mm " << float_vector[2] << " mm\n" - << float_vector[3] << " kg*m^2 " << float_vector[4] << " kg*m^2 "<< float_vector[5] << " kg*m^2"<< std::endl); + << float_vector[3] << " kg*m^2 " << float_vector[4] << " kg*m^2 "<< float_vector[5] << " kg*m^2"<< std::endl; - RCLCPP_INFO_STREAM(this->get_logger(),"\nMin. absolute position: " << min_pos << " mm\n" + info_stream << "\nMin. absolute position: " << min_pos << " mm\n" << "Max. absolute position: " << max_pos << " mm\n" - << "Zero_pos_offset: " << zero_pos_ofs << " mm" << std::endl); + << "Zero_pos_offset: " << zero_pos_ofs << " mm" << std::endl; - RCLCPP_INFO_STREAM(this->get_logger(),"\nMin. velocity_of_movement: " << min_vel << " mm/s\n" - << "Max. velocity_of_movement: " << max_vel << " mm/s" <get_logger(),"\nMax. grip velocity_of_movement: "<< max_grp_vel << " mm/s\n" + info_stream << "\nMax. grip velocity_of_movement: "<< max_grp_vel << " mm/s\n" << "Min. grip force: " << min_grip_force << " N\n" - << "Max. grip force: " << max_grip_force << " N" << std::endl); + << "Max. grip force: " << max_grip_force << " N" << std::endl; if(model.find("EGU") != std::string::npos) { - RCLCPP_INFO_STREAM(this->get_logger(),"\nMax allowed grip force StrongGrip: " << max_allow_force << " N " << std::endl); + info_stream << "\nMax allowed grip force StrongGrip: " << max_allow_force << " N " << std::endl; } getWithInstance(USED_CUR_LIMIT_INST, &data3); - RCLCPP_INFO_STREAM(this->get_logger(),"\nUsed current limit: " << data3 << " A" << std::endl); + info_stream << "\nUsed current limit: " << data3 << " A" << std::endl; getWithInstance(MAX_PHYS_STROKE_INST, &data3); - RCLCPP_INFO_STREAM(this->get_logger(),"Max. physical stroke: " << data3 << " mm" << std::endl); + info_stream << "Max. physical stroke: " << data3 << " mm" << std::endl; getWithOffset(MIN_ERR_MOT_VOLT_OFFSET, 6, float_vector); - RCLCPP_INFO_STREAM(this->get_logger(),"\nMin. error motor voltage: " << float_vector[0] << " V\n" + info_stream << "\nMin. error motor voltage: " << float_vector[0] << " V\n" << "Max. error motor voltage: " << float_vector[1] << " V\n" << "Min. error logic voltage: " << float_vector[2] << " V\n" << "Max. error logic voltage: " << float_vector[3] << " V\n" << "Min. error logic temperature: " << float_vector[4] << " C\n" - << "Max. error logic temperature: " << float_vector[5] << " C" << std::endl); + << "Max. error logic temperature: " << float_vector[5] << " C" << std::endl; getWithInstance(MEAS_LGC_TEMP_INST, &data3); - RCLCPP_INFO_STREAM(this->get_logger(), "Measured logic temperature: " << data3 << " C" << std::endl); + info_stream << "Measured logic temperature: " << data3 << " C" << std::endl; getWithOffset(MEAS_LGC_VOLT_OFFSET, 8, float_vector); - RCLCPP_INFO_STREAM(this->get_logger(), + info_stream << "\nMeasured logic voltage: " << float_vector[0] << " V\n" << "Measured motor voltage: " << float_vector[1] << " V\n" << "Min. warning motor voltage: " << float_vector[2] << " V\n" @@ -1610,16 +1548,20 @@ void SchunkGripperNode::info_srv(const std::shared_ptr, st << "Min. warning logic voltage: " << float_vector[4] << " V\n" << "Max. warning logic voltage: " << float_vector[5] << " V\n" << "Min. warning logic temperature: " << float_vector[6] << " C\n" - << "Max. warning logic temperature: " << float_vector[7] << " C" << std::endl); + << "Max. warning logic temperature: " << float_vector[7] << " C" << std::endl; + res->info = info_stream.str(); + res->success = true; } - catch(const char* res) + catch(const char* error) { - connection_error = res; - RCLCPP_ERROR(this->get_logger(), "Failed Connection! %s", connection_error.c_str()); + connection_error = error; + res->success = false; + RCLCPP_ERROR(this->get_logger(), "Failed Connection! %s", connection_error.c_str()); } catch(const std::exception &e) { + res->success = false; RCLCPP_ERROR(this->get_logger(), "%s", e.what()); } } diff --git a/schunk_egu_egk_gripper_dummy/README.md b/schunk_egu_egk_gripper_dummy/README.md index 66e8d24..82e2346 100644 --- a/schunk_egu_egk_gripper_dummy/README.md +++ b/schunk_egu_egk_gripper_dummy/README.md @@ -10,7 +10,7 @@ pip install --user fastapi uvicorn ## Getting started 1. Start the dummy standalone with ```bash - uvicorn schunk_egu_egk_gripper_dummy.main:server --port 8000 --reload + ./start_dummy.sh ``` ## Run tests locally diff --git a/schunk_egu_egk_gripper_dummy/config/data.json b/schunk_egu_egk_gripper_dummy/config/data.json index 371216e..b28eda0 100644 --- a/schunk_egu_egk_gripper_dummy/config/data.json +++ b/schunk_egu_egk_gripper_dummy/config/data.json @@ -24,11 +24,14 @@ "00000000" ], "0x0230": [ - "42263D87" + "00000000" ], "0x0238": [ "00000000" ], + "0x0240": [ + "3C56B24F" + ], "0x0380": [ "0000" ], @@ -69,19 +72,19 @@ "00000000" ], "0x0628": [ - "40B00000" + "0000B040" ], "0x0630": [ - "42E60000" + "0000E642" ], "0x0650": [ "41B00000" ], "0x0658": [ - "42960000" + "00009642" ], "0x0660": [ - "43160000" + "0000B442" ], "0x06A8": [ "00000000" diff --git a/schunk_egu_egk_gripper_dummy/config/system_parameter_codes b/schunk_egu_egk_gripper_dummy/config/system_parameter_codes index 0b38c21..2464603 100644 --- a/schunk_egu_egk_gripper_dummy/config/system_parameter_codes +++ b/schunk_egu_egk_gripper_dummy/config/system_parameter_codes @@ -13,6 +13,7 @@ 0x0210 0x0230 0x0238 +0x0240 0x0380 0x03A8 0x03B0 diff --git a/schunk_egu_egk_gripper_dummy/schunk_egu_egk_gripper_dummy/main.py b/schunk_egu_egk_gripper_dummy/schunk_egu_egk_gripper_dummy/main.py index 46ec41f..ac3a06d 100644 --- a/schunk_egu_egk_gripper_dummy/schunk_egu_egk_gripper_dummy/main.py +++ b/schunk_egu_egk_gripper_dummy/schunk_egu_egk_gripper_dummy/main.py @@ -7,6 +7,7 @@ # Components dummy = Dummy() +dummy.start() server = FastAPI() client = ["http://localhost:8001"] diff --git a/schunk_egu_egk_gripper_dummy/src/dummy.py b/schunk_egu_egk_gripper_dummy/src/dummy.py index 2d7a045..3115813 100644 --- a/schunk_egu_egk_gripper_dummy/src/dummy.py +++ b/schunk_egu_egk_gripper_dummy/src/dummy.py @@ -38,6 +38,7 @@ def sample(self, t: float) -> Tuple[float, float]: class Dummy(object): def __init__(self): + self.starttime = time.time() self.thread = Thread(target=self._run) self.running = False self.done = False @@ -48,6 +49,7 @@ def __init__(self): self.plc_output = "0x0048" self.actual_position = "0x0230" self.actual_speed = "0x0238" + self.system_uptime = "0x1400" self.error_byte = 12 self.diagnostics_byte = 15 self.valid_status_bits = list(range(0, 10)) + [11, 12, 13, 14, 16, 17, 31] @@ -73,6 +75,7 @@ def __init__(self): self.plc_input_buffer = bytearray(bytes.fromhex(self.data[self.plc_input][0])) self.plc_output_buffer = bytearray(bytes.fromhex(self.data[self.plc_output][0])) + self.initial_state = [self.plc_input_buffer.hex().upper()] def start(self) -> None: if self.running: @@ -87,6 +90,8 @@ def stop(self) -> None: def _run(self) -> None: while not self.done: + elapsed = time.time() - self.starttime + self.set_system_uptime(int(elapsed)) time.sleep(1) print("Done") @@ -99,7 +104,7 @@ def move(self, target_pos: float, target_speed: float) -> None: ) start = time.time() actual_pos, actual_speed = motion.sample(0) - while abs(actual_pos) < abs(target_pos): + while abs(target_pos - actual_pos) > 0.001: # mm t = time.time() - start actual_pos, actual_speed = motion.sample(t) self.set_actual_position(actual_pos) @@ -140,7 +145,10 @@ def get_data(self, query: dict[str, str]) -> list: if offset + count >= len(self.metadata): return result for i in range(count): - result.append(self.metadata[offset + i]) + id = self.metadata[offset + i]["instance"] + inst = hex(id)[2:].upper().zfill(4) + inst = "0x" + inst + result.append(self.data[inst][0]) return result if "inst" in query and "count" in query: @@ -223,11 +231,25 @@ def get_control_bit(self, bit: int) -> int | bool: byte_index, bit_index = divmod(bit, 8) return 1 if self.plc_output_buffer[byte_index] & (1 << bit_index) != 0 else 0 + def set_control_bit(self, bit: int, value: bool) -> bool: + if bit < 0 or bit > 31: + return False + if bit in self.reserved_control_bits: + return False + byte_index, bit_index = divmod(bit, 8) + if value: + self.plc_output_buffer[byte_index] |= 1 << bit_index + else: + self.plc_output_buffer[byte_index] &= ~(1 << bit_index) + return True + def get_target_position(self) -> float: - return struct.unpack("f", self.plc_output_buffer[4:8])[0] + return struct.unpack("i", self.plc_output_buffer[4:8])[0] / 1000.0 # um to mm def get_target_speed(self) -> float: - return struct.unpack("f", self.plc_output_buffer[8:12])[0] + return ( + struct.unpack("i", self.plc_output_buffer[8:12])[0] / 1000.0 + ) # um/s to mm/s def set_actual_position(self, position: float) -> None: self.data[self.actual_position] = [ @@ -245,7 +267,27 @@ def get_actual_speed(self) -> float: read_speed = self.data[self.actual_speed][0] return struct.unpack("f", bytes.fromhex(read_speed))[0] + def set_system_uptime(self, uptime: int) -> None: + self.data[self.system_uptime] = [bytes(struct.pack("i", uptime)).hex().upper()] + + def get_system_uptime(self) -> int: + uptime = self.data[self.system_uptime][0] + return struct.unpack("i", bytes.fromhex(uptime))[0] + def process_control_bits(self) -> None: + """ + See the gripper's firmware documentation for EtherNet/IP [1]: + https://stb.cloud.schunk.com/media/IM0046706.PDF + + """ + # Reset success of previous commands + self.set_status_bit(bit=4, value=False) + self.set_status_bit(bit=8, value=False) + self.set_status_bit(bit=12, value=False) + self.set_status_bit(bit=13, value=False) + + # Command received toggle + self.toggle_status_bit(bit=5) # Acknowledge if self.get_control_bit(2) == 1: @@ -254,12 +296,71 @@ def process_control_bits(self) -> None: self.set_status_error("00") self.set_status_diagnostics("00") + # Brake test + if self.get_control_bit(30) == 1: + self.set_status_bit(bit=4, value=True) + + # Fast stop + if self.get_control_bit(0) == 0: # fail-safe behavior + self.set_status_bit(bit=7, value=True) + self.set_status_diagnostics("D9") + + # Controlled stop + if self.get_control_bit(1) == 1: + self.set_status_bit(bit=13, value=True) + self.set_status_bit(bit=4, value=True) + + # Manual release + if self.get_control_bit(5) == 1: + if self.get_status_bit(7) == 1: + self.set_status_bit(bit=8, value=True) + + # Shutdown + if self.get_control_bit(3) == 1: + self.set_status_bit(bit=2, value=True) + + # Release workpiece + if self.get_control_bit(bit=11) == 1: + self.set_status_bit(bit=4, value=True) + self.set_status_bit(bit=13, value=True) + self.set_status_bit(bit=14, value=False) + self.set_status_bit(bit=12, value=False) + self.set_status_bit(bit=17, value=False) + + # Soft reset + if self.get_control_bit(bit=4) == 1: + self.set_system_uptime(0) + self.data[self.plc_input] = self.initial_state + # Move to absolute position - if self.get_control_bit(13) == 1: - self.toggle_status_bit(5) + if self.get_control_bit(bit=13) == 1: self.move( target_pos=self.get_target_position(), target_speed=self.get_target_speed(), ) self.set_status_bit(bit=13, value=True) self.set_status_bit(bit=4, value=True) + + # Move to relative position + if self.get_control_bit(bit=14) == 1: + target_pos = ( + self.get_actual_position() + + self.get_target_position() # interpret relative + ) + self.move( + target_pos=target_pos, + target_speed=self.get_target_speed(), + ) + self.set_status_bit(bit=13, value=True) + self.set_status_bit(bit=4, value=True) + + # Grip workpiece + if self.get_control_bit(bit=12) == 1: + self.set_status_bit(bit=12, value=True) + self.set_status_bit(bit=4, value=True) + + # Grip workpiece at position + if self.get_control_bit(bit=16) == 1: + self.set_status_bit(bit=12, value=True) + self.set_status_bit(bit=4, value=True) + self.set_status_bit(bit=31, value=True) diff --git a/schunk_egu_egk_gripper_dummy/start_dummy.sh b/schunk_egu_egk_gripper_dummy/start_dummy.sh new file mode 100755 index 0000000..1551d71 --- /dev/null +++ b/schunk_egu_egk_gripper_dummy/start_dummy.sh @@ -0,0 +1 @@ +uvicorn schunk_egu_egk_gripper_dummy.main:server --port 8000 --reload diff --git a/schunk_egu_egk_gripper_dummy/tests/test_dummy.py b/schunk_egu_egk_gripper_dummy/tests/test_dummy.py index b8a610a..557fb4b 100644 --- a/schunk_egu_egk_gripper_dummy/tests/test_dummy.py +++ b/schunk_egu_egk_gripper_dummy/tests/test_dummy.py @@ -1,6 +1,7 @@ from src.dummy import Dummy import pytest import struct +import time # [1]: https://stb.cloud.schunk.com/media/IM0046706.PDF @@ -44,42 +45,168 @@ def test_dummy_starts_in_error_state(): def test_dummy_is_ready_after_acknowledge(): dummy = Dummy() - control_double_word = "04000000" - set_position = "00000000" - set_speed = "00000000" - gripping_force = "00000000" - command = { - "inst": dummy.plc_output, - "value": control_double_word + set_position + set_speed + gripping_force, - } - dummy.post(command) + dummy.set_control_bit(bit=2, value=True) + dummy.process_control_bits() assert dummy.get_status_bit(0) == 1 # ready assert dummy.get_status_bit(7) == 0 # no error assert dummy.get_status_error() == "0" assert dummy.get_status_diagnostics() == "0" -def test_dummy_moves_to_absolute_position(): +def test_dummy_always_toggles_command_received_bit(): dummy = Dummy() - target_pos = 12.345 - target_speed = 50.3 + before = dummy.get_status_bit(bit=5) # command received toggle + dummy.process_control_bits() + after = dummy.get_status_bit(bit=5) + assert after != before - control_double_word = "00200000" - set_position = bytes(struct.pack("f", target_pos)).hex().upper() - set_speed = bytes(struct.pack("f", target_speed)).hex().upper() + +def test_dummy_moves_to_absolute_position(): + dummy = Dummy() + target_positions = [12345, 10555, 77000, 1500] # mu + target_speeds = [50300, 40000, 10500, 20999] # mu / s + for target_pos, target_speed in zip(target_positions, target_speeds): + control_double_word = "00200000" # bit 13 + set_position = bytes(struct.pack("i", target_pos)).hex().upper() + set_speed = bytes(struct.pack("i", target_speed)).hex().upper() + gripping_force = "00000000" + command = { + "inst": dummy.plc_output, + "value": control_double_word + set_position + set_speed + gripping_force, + } + + dummy.post(command) + assert dummy.get_actual_position() == pytest.approx( + target_pos / 1000.0, rel=1e-3 + ) + assert dummy.get_status_bit(bit=13) == 1 # position reached + assert dummy.get_status_bit(bit=4) == 1 # command successfully processed + + +def test_dummy_moves_to_relative_position(): + dummy = Dummy() + target_pos = -5000 # mu + target_speed = 12000 # mu / s + control_double_word = "00400000" # bit 14 + set_position = bytes(struct.pack("i", target_pos)).hex().upper() + set_speed = bytes(struct.pack("i", target_speed)).hex().upper() gripping_force = "00000000" command = { "inst": dummy.plc_output, "value": control_double_word + set_position + set_speed + gripping_force, } - before = dummy.get_status_bit(bit=5) # command received toggle - - # Motion + before = dummy.get_actual_position() dummy.post(command) + after = dummy.get_actual_position() + assert after < before # we are decreasing + assert after == pytest.approx(before + target_pos / 1000.0, rel=1e-3) + assert dummy.get_status_bit(bit=13) == 1 # position reached + assert dummy.get_status_bit(bit=4) == 1 # command successfully processed + - # Done +def test_dummy_updates_internal_state_when_moving(): + dummy = Dummy() + query = {"offset": 15, "count": 3} # actual position, speed, and current + before = dummy.get_data(query) + + # Move + target_pos = 10.34 + target_speed = 15.0 + assert pytest.approx(dummy.get_actual_position()) != target_pos + dummy.move(target_pos=target_pos, target_speed=target_speed) assert pytest.approx(dummy.get_actual_position()) == target_pos - after = dummy.get_status_bit(bit=5) - assert after != before + + after = dummy.get_data(query) + assert before != after + + +def test_dummy_performs_break_test(): + dummy = Dummy() + dummy.set_control_bit(bit=30, value=True) + dummy.process_control_bits() + assert dummy.get_status_bit(bit=4) == 1 # command successfully processed + + +def test_dummy_performs_fast_stop(): + dummy = Dummy() + dummy.set_control_bit(bit=0, value=False) # fail-safe behavior + dummy.process_control_bits() + assert dummy.get_status_bit(bit=7) == 1 # error + assert dummy.get_status_diagnostics() == "D9" # ERR_FAST_STOP + + +def test_dummy_performs_controlled_stop(): + dummy = Dummy() + dummy.set_control_bit(bit=1, value=True) + dummy.process_control_bits() assert dummy.get_status_bit(bit=13) == 1 # position reached assert dummy.get_status_bit(bit=4) == 1 # command successfully processed + + +def test_dummy_supports_manual_release(): + dummy = Dummy() + + # Reject when not in error state + dummy.set_status_bit(bit=7, value=False) # clear error + dummy.set_status_diagnostics("00") + dummy.set_control_bit(bit=5, value=True) # release + dummy.process_control_bits() + assert dummy.get_status_bit(bit=8) == 0 # not released + + # Accept when in error state + dummy.set_control_bit(bit=0, value=False) # trigger fast stop + dummy.process_control_bits() + dummy.set_control_bit(bit=5, value=True) # release + dummy.process_control_bits() + assert dummy.get_status_bit(bit=8) == 1 # released for manual movement + + +def test_dummy_supports_prepare_for_shutdown(): + dummy = Dummy() + dummy.set_control_bit(bit=3, value=True) + dummy.process_control_bits() + assert dummy.get_status_bit(bit=2) == 1 # ready for shutdown + + +def test_dummy_supports_release_workpiece(): + dummy = Dummy() + dummy.set_control_bit(bit=11, value=True) + dummy.process_control_bits() + assert dummy.get_status_bit(bit=4) == 1 # command successfully processed + assert dummy.get_status_bit(bit=13) == 1 # position reached + assert dummy.get_status_bit(bit=14) == 0 # pre-grip started + assert dummy.get_status_bit(bit=12) == 0 # workpiece gripped + assert dummy.get_status_bit(bit=17) == 0 # wrong workpiece gripped + + +def test_dummy_supports_softreset(): + dummy = Dummy() + dummy.start() # fake some system uptime + initial = dummy.get_system_uptime() + time.sleep(1.5) + later = dummy.get_system_uptime() + time.sleep(1.5) + dummy.set_control_bit(bit=4, value=True) + dummy.process_control_bits() + after_reset = dummy.get_system_uptime() + dummy.stop() + + assert initial < later + assert after_reset < later # restart resets the uptime + + +def test_dummy_supports_grip(): + dummy = Dummy() + dummy.set_control_bit(bit=12, value=True) # grip workpiece + dummy.process_control_bits() + assert dummy.get_status_bit(bit=12) == 1 # workpiece gripped + assert dummy.get_status_bit(bit=4) == 1 # command successfully processed + + +def test_dummy_supports_grip_at_position(): + dummy = Dummy() + dummy.set_control_bit(bit=16, value=True) # grip workpiece at expected position + dummy.process_control_bits() + assert dummy.get_status_bit(bit=12) == 1 # workpiece gripped + assert dummy.get_status_bit(bit=4) == 1 # command successfully processed + assert dummy.get_status_bit(bit=31) == 1 # GPE activated diff --git a/schunk_egu_egk_gripper_dummy/tests/test_motion_profile.py b/schunk_egu_egk_gripper_dummy/tests/test_motion_profile.py new file mode 100644 index 0000000..120df57 --- /dev/null +++ b/schunk_egu_egk_gripper_dummy/tests/test_motion_profile.py @@ -0,0 +1,129 @@ +from src.dummy import LinearMotion +import pytest +import time + + +def test_motion_initializes_with_initial_state_and_target_state(): + initial_pos = 0.0 + initial_speed = 0.0 + target_pos = 0.1 + target_speed = 55.1 + motion = LinearMotion( + initial_pos=initial_pos, + initial_speed=initial_speed, + target_pos=target_pos, + target_speed=target_speed, + ) + assert pytest.approx(motion.initial_pos) == initial_pos + assert pytest.approx(motion.initial_speed) == initial_speed + assert pytest.approx(motion.target_pos) == target_pos + assert pytest.approx(motion.target_speed) == target_speed + + +def test_motion_corrects_negative_or_zero_speed_arguments(): + initial_pos = 0.0 + initial_speed = -0.3 + target_pos = 0.1 + target_speed = 0.0 + motion = LinearMotion( + initial_pos=initial_pos, + initial_speed=initial_speed, + target_pos=target_pos, + target_speed=target_speed, + ) + assert pytest.approx(motion.target_speed) == motion.min_speed + assert pytest.approx(motion.initial_speed) == 0.0 + + target_speed = -0.123 + motion = LinearMotion( + initial_pos=initial_pos, + initial_speed=initial_speed, + target_pos=target_pos, + target_speed=target_speed, + ) + assert pytest.approx(motion.target_speed) == motion.min_speed + + +def test_motion_samples_current_state_at_given_time(): + initial_pos = 0.0 + initial_speed = 0.0 + target_pos = 1.3 + target_speed = 55.1 + motion = LinearMotion( + initial_pos=initial_pos, + initial_speed=initial_speed, + target_pos=target_pos, + target_speed=target_speed, + ) + start = time.time() + time.sleep(0.01) + now = time.time() - start + current_pos, current_speed = motion.sample(t=now) + assert isinstance(current_pos, float) + assert isinstance(current_speed, float) + assert current_pos >= initial_pos + assert current_pos <= target_pos + assert current_speed >= initial_speed + assert current_speed <= target_speed + + +def test_motion_samples_initial_state_at_time_less_or_equal_zero(): + initial_pos = 0.0 + initial_speed = 0.0 + target_pos = 0.1 + target_speed = 55.1 + motion = LinearMotion( + initial_pos=initial_pos, + initial_speed=initial_speed, + target_pos=target_pos, + target_speed=target_speed, + ) + current_pos, current_speed = motion.sample(t=-0.001) + assert pytest.approx(current_pos) == motion.initial_pos + assert pytest.approx(current_speed) == motion.initial_speed + + +def test_motion_samples_target_position_at_time_after_finish(): + initial_pos = 0.0 + initial_speed = 0.0 + target_pos = 0.1 + target_speed = 55.1 + motion = LinearMotion( + initial_pos=initial_pos, + initial_speed=initial_speed, + target_pos=target_pos, + target_speed=target_speed, + ) + current_pos, current_speed = motion.sample(t=1000) + assert pytest.approx(current_pos) == motion.target_pos + assert pytest.approx(current_speed) == 0.0 + + +def test_motion_supports_both_directions(): + initial_pos = 5.0 + initial_speed = 0.0 + target_speed = 5.678 + + # Positive direction + target_pos = 12.34 + motion = LinearMotion( + initial_pos=initial_pos, + initial_speed=initial_speed, + target_pos=target_pos, + target_speed=target_speed, + ) + current_pos_1, _ = motion.sample(t=0.01) + current_pos_2, _ = motion.sample(t=0.02) + assert current_pos_1 < current_pos_2 + + # Negative direction + target_pos = 1.234 + motion = LinearMotion( + initial_pos=initial_pos, + initial_speed=initial_speed, + target_pos=target_pos, + target_speed=target_speed, + ) + current_pos_1, _ = motion.sample(t=0.01) + current_pos_2, _ = motion.sample(t=0.02) + assert current_pos_1 > current_pos_2 diff --git a/schunk_egu_egk_gripper_dummy/tests/test_plc_communication.py b/schunk_egu_egk_gripper_dummy/tests/test_plc_communication.py index 310692f..cee1a03 100644 --- a/schunk_egu_egk_gripper_dummy/tests/test_plc_communication.py +++ b/schunk_egu_egk_gripper_dummy/tests/test_plc_communication.py @@ -81,11 +81,13 @@ def test_dummy_rejects_writing_invalid_status_diagnostics(): assert not dummy.set_status_diagnostics(code) -def test_dummy_supports_reading_bits_in_plc_control(): +def test_dummy_supports_reading_and_writing_bits_in_plc_control(): dummy = Dummy() for bit in dummy.valid_control_bits: + dummy.set_control_bit(bit=bit, value=True) result = dummy.get_control_bit(bit=bit) assert isinstance(result, int) # successful calls get the bit's value + assert result == 1 def test_dummy_rejects_reading_reserved_control_bits(): @@ -95,6 +97,13 @@ def test_dummy_rejects_reading_reserved_control_bits(): assert not dummy.get_control_bit(bit) +def test_dummy_rejects_writing_reserved_control_bits(): + dummy = Dummy() + invalid_bits = [-1, 999] + for bit in invalid_bits + dummy.reserved_control_bits: + assert not dummy.set_control_bit(bit, True) + + def test_dummy_supports_toggling_status_bits(): dummy = Dummy() for bit in dummy.valid_status_bits: @@ -114,16 +123,16 @@ def test_dummy_rejects_toggling_reserved_status_bits(): def test_dummy_supports_reading_target_position(): dummy = Dummy() - target_pos = 0.0123 - dummy.plc_output_buffer[4:8] = bytes(struct.pack("f", target_pos)) - assert pytest.approx(dummy.get_target_position()) == target_pos + target_pos = 12300 # um + dummy.plc_output_buffer[4:8] = bytes(struct.pack("i", target_pos)) + assert pytest.approx(dummy.get_target_position(), rel=1e-3) == target_pos / 1000.0 def test_dummy_supports_reading_target_speed(): dummy = Dummy() - target_speed = 55.3 - dummy.plc_output_buffer[8:12] = bytes(struct.pack("f", target_speed)) - assert pytest.approx(dummy.get_target_speed()) == target_speed + target_speed = 55300 + dummy.plc_output_buffer[8:12] = bytes(struct.pack("i", target_speed)) + assert pytest.approx(dummy.get_target_speed(), rel=1e-3) == target_speed / 1000.0 def test_dummy_supports_writing_actual_position(): @@ -156,3 +165,10 @@ def test_dummy_supports_reading_actual_speed(): speed = 66.5 dummy.set_actual_speed(speed) assert pytest.approx(dummy.get_actual_speed()) == speed + + +def test_dummy_supports_reading_and_writing_system_uptime(): + dummy = Dummy() + uptime = 1234 # secs + dummy.set_system_uptime(uptime) + assert dummy.get_system_uptime() == uptime diff --git a/schunk_egu_egk_gripper_dummy/tests/test_requests.py b/schunk_egu_egk_gripper_dummy/tests/test_requests.py index 48667c3..c951791 100644 --- a/schunk_egu_egk_gripper_dummy/tests/test_requests.py +++ b/schunk_egu_egk_gripper_dummy/tests/test_requests.py @@ -1,5 +1,5 @@ from src.dummy import Dummy -from schunk_egu_egk_gripper_dummy.main import server +from schunk_egu_egk_gripper_dummy.main import server, dummy as client_dummy from fastapi.testclient import TestClient @@ -30,7 +30,19 @@ def test_dummy_survives_invalid_enum_requests(): def test_dummy_responds_correctly_to_data_offset_requests(): dummy = Dummy() query = {"offset": 15, "count": 3} - expected = [dummy.metadata[15], dummy.metadata[16], dummy.metadata[17]] + + def get_hex(number: int) -> str: + plain = hex(number)[2:].upper().zfill(4) + return "0x" + plain + + inst1 = get_hex(dummy.metadata[15]["instance"]) + inst2 = get_hex(dummy.metadata[16]["instance"]) + inst3 = get_hex(dummy.metadata[17]["instance"]) + data1 = dummy.data[inst1][0] + data2 = dummy.data[inst2][0] + data3 = dummy.data[inst3][0] + + expected = [data1, data2, data3] assert dummy.get_data(query) == expected @@ -83,6 +95,7 @@ def test_dummy_stores_post_requests(): def test_dummy_rejects_invalid_post_requests(): client = TestClient(server) + client_dummy.start() valid_data = "AABBCCDD" valid_inst = "0x0238" @@ -98,3 +111,17 @@ def test_dummy_rejects_invalid_post_requests(): invalid_inst = "0x9999" data = {"inst": invalid_inst, "value": valid_data} assert client.post("/adi/update.json", data=data).json() == {"result": 1} + client_dummy.stop() + + +def test_dummy_resets_success_status_bits_with_new_post_requests(): + dummy = Dummy() + dummy.set_status_bit(bit=13, value=True) # position reached + dummy.set_status_bit(bit=4, value=True) # command successful + dummy.set_status_bit(bit=12, value=True) # workpiece gripped + empty_command = "01" + "".zfill(30) # only fast stop active + data = {"inst": dummy.plc_output, "value": empty_command} + dummy.post(data) + assert dummy.get_status_bit(bit=13) == 0 + assert dummy.get_status_bit(bit=4) == 0 + assert dummy.get_status_bit(bit=12) == 0 diff --git a/schunk_egu_egk_gripper_dummy/tests/test_webserver.py b/schunk_egu_egk_gripper_dummy/tests/test_webserver.py index 91444c1..1a22094 100644 --- a/schunk_egu_egk_gripper_dummy/tests/test_webserver.py +++ b/schunk_egu_egk_gripper_dummy/tests/test_webserver.py @@ -1,23 +1,27 @@ -from schunk_egu_egk_gripper_dummy.main import server +from schunk_egu_egk_gripper_dummy.main import server, dummy from fastapi.testclient import TestClient def test_info_route_is_available(): client = TestClient(server) assert client.get("/adi/info.json").is_success + dummy.stop() def test_enum_route_is_available(): client = TestClient(server) assert client.get("/adi/enum.json").is_success + dummy.stop() def test_data_route_is_available(): client = TestClient(server) assert client.get("/adi/data.json").is_success + dummy.stop() def test_update_route_is_available(): client = TestClient(server) data = {"inst": 0, "value": "0"} assert client.post("/adi/update.json", data=data).is_success + dummy.stop() diff --git a/schunk_egu_egk_gripper_examples/src/gripper_example.cpp b/schunk_egu_egk_gripper_examples/src/gripper_example.cpp index 2aea3cc..d4e7c0c 100644 --- a/schunk_egu_egk_gripper_examples/src/gripper_example.cpp +++ b/schunk_egu_egk_gripper_examples/src/gripper_example.cpp @@ -36,7 +36,6 @@ #include "schunk_egu_egk_gripper_interfaces/srv/prepare_for_shutdown.hpp" #include "schunk_egu_egk_gripper_interfaces/srv/softreset.hpp" #include "schunk_egu_egk_gripper_interfaces/srv/brake_test.hpp" -#include "schunk_egu_egk_gripper_interfaces/srv/change_ip.hpp" #include "schunk_egu_egk_gripper_interfaces/srv/parameter_get.hpp" #include "schunk_egu_egk_gripper_interfaces/srv/parameter_set.hpp" #include "schunk_egu_egk_gripper_interfaces/srv/release_for_manual_movement.hpp" @@ -66,7 +65,6 @@ std::string name_space; using Softreset = schunk_egu_egk_gripper_interfaces::srv::Softreset; using PrepareForShutdown = schunk_egu_egk_gripper_interfaces::srv::PrepareForShutdown; using GripperInfo= schunk_egu_egk_gripper_interfaces::srv::GripperInfo; - using ChangeIp = schunk_egu_egk_gripper_interfaces::srv::ChangeIp; using ParameterGet = schunk_egu_egk_gripper_interfaces::srv::ParameterGet; using ParameterSet = schunk_egu_egk_gripper_interfaces::srv::ParameterSet; @@ -234,7 +232,7 @@ void moveRelativeAndStop(rclcpp_action::Client::SharedPt auto stop_req = std::make_shared(); auto resp = stop_client->async_send_request(stop_req); - bool stopped = resp.get()->stopped; + bool stopped = resp.get()->success; RCLCPP_INFO(rclcpp::get_logger("schunk_gripper_example"), "%s", stopped ? "Stopped!" : "Not stopped!"); auto res = move_rel_client->async_get_result(goal_handle.get()); diff --git a/schunk_egu_egk_gripper_interfaces/CMakeLists.txt b/schunk_egu_egk_gripper_interfaces/CMakeLists.txt index 26e6ca1..6c87a1e 100644 --- a/schunk_egu_egk_gripper_interfaces/CMakeLists.txt +++ b/schunk_egu_egk_gripper_interfaces/CMakeLists.txt @@ -26,7 +26,6 @@ rosidl_generate_interfaces( msg/State.msg srv/Acknowledge.srv srv/BrakeTest.srv - srv/ChangeIp.srv srv/FastStop.srv srv/GripperInfo.srv srv/ParameterGet.srv diff --git a/schunk_egu_egk_gripper_interfaces/srv/BrakeTest.srv b/schunk_egu_egk_gripper_interfaces/srv/BrakeTest.srv index e0cac6d..7e0b3bf 100644 --- a/schunk_egu_egk_gripper_interfaces/srv/BrakeTest.srv +++ b/schunk_egu_egk_gripper_interfaces/srv/BrakeTest.srv @@ -1,3 +1,3 @@ --- -bool brake_test_successful +bool success string error_code diff --git a/schunk_egu_egk_gripper_interfaces/srv/ChangeIp.srv b/schunk_egu_egk_gripper_interfaces/srv/ChangeIp.srv deleted file mode 100644 index 6308764..0000000 --- a/schunk_egu_egk_gripper_interfaces/srv/ChangeIp.srv +++ /dev/null @@ -1,3 +0,0 @@ -string new_ip ---- -bool ip_changed diff --git a/schunk_egu_egk_gripper_interfaces/srv/FastStop.srv b/schunk_egu_egk_gripper_interfaces/srv/FastStop.srv index 4d1c0b1..410e0f9 100644 --- a/schunk_egu_egk_gripper_interfaces/srv/FastStop.srv +++ b/schunk_egu_egk_gripper_interfaces/srv/FastStop.srv @@ -1,2 +1,2 @@ --- -bool fast_stopped +bool success diff --git a/schunk_egu_egk_gripper_interfaces/srv/GripperInfo.srv b/schunk_egu_egk_gripper_interfaces/srv/GripperInfo.srv index ed97d53..fafb7ac 100644 --- a/schunk_egu_egk_gripper_interfaces/srv/GripperInfo.srv +++ b/schunk_egu_egk_gripper_interfaces/srv/GripperInfo.srv @@ -1 +1,3 @@ --- +bool success +string info diff --git a/schunk_egu_egk_gripper_interfaces/srv/PrepareForShutdown.srv b/schunk_egu_egk_gripper_interfaces/srv/PrepareForShutdown.srv index bbc46d3..410e0f9 100644 --- a/schunk_egu_egk_gripper_interfaces/srv/PrepareForShutdown.srv +++ b/schunk_egu_egk_gripper_interfaces/srv/PrepareForShutdown.srv @@ -1,2 +1,2 @@ --- -bool prepared_for_shutdown +bool success diff --git a/schunk_egu_egk_gripper_interfaces/srv/ReleaseForManualMovement.srv b/schunk_egu_egk_gripper_interfaces/srv/ReleaseForManualMovement.srv index f2fb068..410e0f9 100644 --- a/schunk_egu_egk_gripper_interfaces/srv/ReleaseForManualMovement.srv +++ b/schunk_egu_egk_gripper_interfaces/srv/ReleaseForManualMovement.srv @@ -1,2 +1,2 @@ --- -bool released_for_manual_movement +bool success diff --git a/schunk_egu_egk_gripper_interfaces/srv/Softreset.srv b/schunk_egu_egk_gripper_interfaces/srv/Softreset.srv index 4102fa4..410e0f9 100644 --- a/schunk_egu_egk_gripper_interfaces/srv/Softreset.srv +++ b/schunk_egu_egk_gripper_interfaces/srv/Softreset.srv @@ -1,2 +1,2 @@ --- -bool reset_success +bool success diff --git a/schunk_egu_egk_gripper_interfaces/srv/Stop.srv b/schunk_egu_egk_gripper_interfaces/srv/Stop.srv index 91a705f..410e0f9 100644 --- a/schunk_egu_egk_gripper_interfaces/srv/Stop.srv +++ b/schunk_egu_egk_gripper_interfaces/srv/Stop.srv @@ -1,2 +1,2 @@ --- -bool stopped +bool success diff --git a/schunk_egu_egk_gripper_library/include/schunk_egu_egk_gripper_library/communication.hpp b/schunk_egu_egk_gripper_library/include/schunk_egu_egk_gripper_library/communication.hpp index 469d725..8762199 100644 --- a/schunk_egu_egk_gripper_library/include/schunk_egu_egk_gripper_library/communication.hpp +++ b/schunk_egu_egk_gripper_library/include/schunk_egu_egk_gripper_library/communication.hpp @@ -113,6 +113,8 @@ #define SW_BUILD_DATE_INST "0x1100" #define SW_BUILD_TIME_INST "0x1108" #define SW_VERSION_TXT_INST "0x1118" +#define SYSTEM_UPTIME_INST "0x1400" + //Datatypes in JSON #define BOOL_DATA 0 #define INT32_DATA 3 diff --git a/schunk_egu_egk_gripper_library/include/schunk_egu_egk_gripper_library/schunk_gripper_lib.hpp b/schunk_egu_egk_gripper_library/include/schunk_egu_egk_gripper_library/schunk_gripper_lib.hpp index a52b69c..0c88a8a 100644 --- a/schunk_egu_egk_gripper_library/include/schunk_egu_egk_gripper_library/schunk_gripper_lib.hpp +++ b/schunk_egu_egk_gripper_library/include/schunk_egu_egk_gripper_library/schunk_gripper_lib.hpp @@ -56,7 +56,6 @@ class Gripper : protected AnybusCom bool gripperBitOutput(const uint32_t&) const; //retrieve individual bits from the control double word void acknowledge(); //acknowledge - bool changeIp(const std::string &); //changeIP bool ip_changed_with_all_param; //IP and parameter changed diff --git a/schunk_egu_egk_gripper_library/src/schunk_gripper_lib.cpp b/schunk_egu_egk_gripper_library/src/schunk_gripper_lib.cpp index d3c4234..537b309 100644 --- a/schunk_egu_egk_gripper_library/src/schunk_gripper_lib.cpp +++ b/schunk_egu_egk_gripper_library/src/schunk_gripper_lib.cpp @@ -319,50 +319,7 @@ void Gripper::acknowledge() getWithInstance(PLC_SYNC_INPUT_INST); plc_sync_output[0] &= mask; } -//change the ip-address -bool Gripper::changeIp(const std::string &new_ip) -{ - if(new_ip.size() > 100) return false; - - std::string old_ip = ip; - ip = new_ip; - std::string old_model = model; - initAddresses(); - //Control if it is the same gripper - try - { - startGripper(); - - ip_changed_with_all_param = true; - - return true; - } - catch(const char* res) - { - ip_changed_with_all_param = false; - std::cout << "No Gripper found. New IP: " << ip << std::endl; - initAddresses(); - return true; - } - catch(const nlohmann::json::parse_error &e) - { - ip_changed_with_all_param = true; - ip = old_ip; - std::cout << "message: " << e.what() << "\nexception id: " << e.id << std::endl; - std::cout << "Setting to old IP: " << ip << std::endl; - initAddresses(); - return false; - } - catch(const std::exception &e) - { - ip_changed_with_all_param = true; - ip = old_ip; - std::cout << "Wrong data found. Setting to old IP: " << ip << std::endl; - initAddresses(); - return false; - } -} //send action directly after current (cyclic-) http or immediately void Gripper::sendAction() { diff --git a/schunk_egu_egk_gripper_tests/test/conftest.py b/schunk_egu_egk_gripper_tests/test/conftest.py index cd37991..4c35d3d 100644 --- a/schunk_egu_egk_gripper_tests/test/conftest.py +++ b/schunk_egu_egk_gripper_tests/test/conftest.py @@ -7,6 +7,8 @@ import launch_pytest import subprocess from ament_index_python.packages import get_package_share_directory +from test.helpers import get_current_state +import time # We avoid black's F811, F401 linting warnings @@ -15,14 +17,14 @@ # https://docs.pytest.org/en/7.1.x/reference/fixtures.html#conftest-py-sharing-fixtures-across-multiple-files # noqa: E501 -@pytest.fixture() +@pytest.fixture(scope="module") def isolated(): rclpy.init() yield rclpy.shutdown() -@pytest.fixture() +@pytest.fixture(scope="module") def gripper_dummy(): package_name = "schunk_egu_egk_gripper_dummy" dummy_dir = get_package_share_directory(package_name) @@ -37,7 +39,7 @@ def gripper_dummy(): print("Stopped gripper dummy") -@launch_pytest.fixture +@launch_pytest.fixture(scope="module") def launch_description(): setup = IncludeLaunchDescription( PathJoinSubstitution( @@ -53,3 +55,13 @@ def launch_description(): }.items(), ) return LaunchDescription([setup, launch_pytest.actions.ReadyToTest()]) + + +@pytest.fixture(scope="module") +def running_driver(gripper_dummy, isolated, launch_context): + + print("Waiting until the driver is connected") + while not get_current_state(variable="ready_for_operation") is True: + time.sleep(0.1) + yield + ... diff --git a/schunk_egu_egk_gripper_tests/test/helpers.py b/schunk_egu_egk_gripper_tests/test/helpers.py index 966c404..2a950ff 100644 --- a/schunk_egu_egk_gripper_tests/test/helpers.py +++ b/schunk_egu_egk_gripper_tests/test/helpers.py @@ -1,21 +1,28 @@ from threading import Thread, Event -import rclpy from rclpy.node import Node from typing import Any from schunk_egu_egk_gripper_interfaces.msg import State # type: ignore[attr-defined] import uuid +from rclpy.executors import MultiThreadedExecutor +import time +from rclpy.action import ActionClient class TopicGetsPublished(Node): def __init__(self, topic: str, type: Any): - node_name = "check_topic" + str(uuid.uuid4()).replace("-", "") - super().__init__(node_name) + self.node_name = "check_topic" + str(uuid.uuid4()).replace("-", "") + super().__init__(self.node_name) self.event = Event() self.data = None self.sub = self.create_subscription(type, topic, self.msg_cb, 3) - self.thread = Thread(target=lambda node: rclpy.spin(node), args=(self,)) + self.thread = Thread(target=self.spin) self.thread.start() + def spin(self) -> None: + executor = MultiThreadedExecutor() + executor.add_node(self) + executor.spin() + def msg_cb(self, data: Any) -> None: self.data = data self.event.set() @@ -25,3 +32,58 @@ def get_current_state(variable: str): topic = TopicGetsPublished("/state", State) topic.event.wait() return getattr(topic.data, variable) + + +class ServiceReturnsResult(Node): + def __init__(self, service: str, type: Any, msg: Any): + self.node_name = "call_service" + str(uuid.uuid4()).replace("-", "") + super().__init__(self.node_name) + self.event = Event() + srv = self.create_client(type, service) + srv.wait_for_service(1.0) + self.future = srv.call_async(msg) + self.result = None + self.thread = Thread(target=self.spin) + self.thread.start() + + def spin(self) -> None: + executor = MultiThreadedExecutor() + executor.add_node(self) + executor.spin_until_future_complete(self.future) + self.result = self.future.result() + self.event.set() + + +class ActionReturnsResult(Node): + def __init__(self, action: str, type: Any, goal: Any): + self.node_name = "call_action" + str(uuid.uuid4()).replace("-", "") + super().__init__(self.node_name) + self.event = Event() + + client = ActionClient(self, type, action) + client.wait_for_server(1.0) + + self.goal_future = client.send_goal_async(goal) + self.result = None + self.thread = Thread(target=self.spin) + self.thread.start() + + def spin(self) -> None: + executor = MultiThreadedExecutor() + executor.add_node(self) + executor.spin_until_future_complete(self.goal_future) + goal_handle = self.goal_future.result() + result_future = goal_handle.get_result_async() + executor.spin_until_future_complete(result_future) + self.result = result_future.result().result + self.event.set() + + +def check_each_in(elements: list, node_method: str) -> None: + node = Node("test") + until_ready = 2.0 # sec + time.sleep(until_ready) + existing = getattr(node, node_method)() + advertised = [i[0] for i in existing] + for element in elements: + assert element in advertised diff --git a/schunk_egu_egk_gripper_tests/test/test_actions.py b/schunk_egu_egk_gripper_tests/test/test_actions.py new file mode 100644 index 0000000..6524906 --- /dev/null +++ b/schunk_egu_egk_gripper_tests/test/test_actions.py @@ -0,0 +1,118 @@ +import pytest +from test.conftest import launch_description +from test.helpers import check_each_in, ActionReturnsResult, get_current_state +from schunk_egu_egk_gripper_interfaces.action import ( # type: ignore[attr-defined] + GripWithVelocity, + GripWithPositionAndVelocity, + MoveToAbsolutePosition, + MoveToRelativePosition, + ReleaseWorkpiece, +) +from control_msgs.action import GripperCommand + + +@pytest.mark.launch(fixture=launch_description) +def test_driver_advertices_all_relevant_actions(running_driver): + action_list = [ + "/grip", + "/grip_with_position", + "/gripper_control", + "/move_to_absolute_position", + "/move_to_relative_position", + "/release_workpiece", + ] + action_list = [a + "/_action/status" for a in action_list] + check_each_in(action_list, "get_topic_names_and_types") + + +@pytest.mark.launch(fixture=launch_description) +def test_driver_grips(running_driver): + goal = ( + GripWithVelocity.Goal() + ) # Our dummy is an EGK, which uses an additional velocity + goal.gripping_force = 65.0 # % of max force + goal.velocity_of_movement = 11.0 + goal.grip_direction = False # close + action = ActionReturnsResult("/grip", GripWithVelocity, goal) + action.event.wait() + assert action.result.workpiece_gripped + assert action.result.no_workpiece_detected is False + assert action.result.workpiece_lost is False + + +@pytest.mark.launch(fixture=launch_description) +def test_driver_grips_with_position(running_driver): + goal = ( + GripWithPositionAndVelocity.Goal() + ) # Our dummy is an EGK, which uses an additional velocity + goal.absolute_position = 18.88 + goal.gripping_force = 65.0 # % of max force + goal.velocity_of_movement = 13.0 + goal.grip_direction = False # close + action = ActionReturnsResult( + "/grip_with_position", GripWithPositionAndVelocity, goal + ) + action.event.wait() + assert action.result.workpiece_gripped + assert action.result.no_workpiece_detected is False + assert action.result.workpiece_lost is False + + +@pytest.mark.launch(fixture=launch_description) +def test_driver_supports_gripper_control(running_driver): + + runs = [ + {"position": 0.0, "max_effort": 77.0}, # grip workpiece + {"position": 82.4, "max_effort": 0.0}, # move to abs. position + {"position": 66.3, "max_effort": 85.1}, # grip with position + ] + + for run in runs: + goal = GripperCommand.Goal() + goal.command.position = run["position"] + goal.command.max_effort = run["max_effort"] + action = ActionReturnsResult("/gripper_control", GripperCommand, goal) + action.event.wait() + assert action.result.reached_goal + assert action.result.stalled + + +@pytest.mark.launch(fixture=launch_description) +def test_driver_moves_to_absolute_position(running_driver): + test_positions = [43.55, 17.02, 38.55, 103.7] + test_speeds = [55.66, 10.5, 40.0, 88.8] + for test_pos, test_speed in zip(test_positions, test_speeds): + goal = MoveToAbsolutePosition.Goal() + goal.absolute_position = test_pos + goal.velocity_of_movement = test_speed + action = ActionReturnsResult( + "/move_to_absolute_position", MoveToAbsolutePosition, goal + ) + action.event.wait() + assert action.result.position_reached + assert pytest.approx(action.result.absolute_position) == goal.absolute_position + + +@pytest.mark.launch(fixture=launch_description) +def test_driver_moves_to_relative_position(running_driver): + initial_pos = get_current_state(variable="actual_position") + goal = MoveToRelativePosition.Goal() + goal.signed_relative_position = -10.03 + goal.velocity_of_movement = 15.0 + action = ActionReturnsResult( + "/move_to_relative_position", MoveToRelativePosition, goal + ) + action.event.wait() + assert action.result.position_reached + assert ( + pytest.approx(action.result.absolute_position) + == initial_pos + goal.signed_relative_position + ) + + +@pytest.mark.launch(fixture=launch_description) +def test_driver_releases_workpieces(running_driver): + goal = ReleaseWorkpiece.Goal() + action = ActionReturnsResult("/release_workpiece", ReleaseWorkpiece, goal) + action.event.wait() + assert action.result.released_workpiece diff --git a/schunk_egu_egk_gripper_tests/test/test_functionality.py b/schunk_egu_egk_gripper_tests/test/test_functionality.py deleted file mode 100644 index e0b9d5f..0000000 --- a/schunk_egu_egk_gripper_tests/test/test_functionality.py +++ /dev/null @@ -1,46 +0,0 @@ -import pytest -import rclpy -from rclpy.node import Node -from test.conftest import launch_description -from rclpy.action import ActionClient -from schunk_egu_egk_gripper_interfaces.action import ( # type: ignore[attr-defined] - MoveToAbsolutePosition, -) -from schunk_egu_egk_gripper_interfaces.srv import ( # type: ignore[attr-defined] - Acknowledge, -) -from test.helpers import get_current_state - - -@pytest.mark.launch(fixture=launch_description) -def test_driver_starts_in_ready_state(launch_context, isolated, gripper_dummy): - assert get_current_state(variable="ready_for_operation") is True - - -@pytest.mark.launch(fixture=launch_description) -def test_driver_is_ready_after_acknowledge(launch_context, isolated, gripper_dummy): - node = Node("test") - activate_srv = node.create_client(Acknowledge, "/acknowledge") - while not activate_srv.wait_for_service(1.0): - pass - future = activate_srv.call_async(Acknowledge.Request()) - rclpy.spin_until_future_complete(node, future) - assert future.result().success is True - - -@pytest.mark.launch(fixture=launch_description) -def test_driver_moves_to_absolute_position(launch_context, isolated, gripper_dummy): - node = Node("move_test") - - client = ActionClient(node, MoveToAbsolutePosition, "/move_to_absolute_position") - client.wait_for_server() - goal = MoveToAbsolutePosition.Goal() - - goal_future = client.send_goal_async(goal) - rclpy.spin_until_future_complete(node, goal_future) - goal_handle = goal_future.result() - - result_future = goal_handle.get_result_async() - rclpy.spin_until_future_complete(node, result_future) - result = result_future.result().result - assert result.position_reached diff --git a/schunk_egu_egk_gripper_tests/test/test_http_interface.py b/schunk_egu_egk_gripper_tests/test/test_http_interface.py deleted file mode 100644 index 7bf76ca..0000000 --- a/schunk_egu_egk_gripper_tests/test/test_http_interface.py +++ /dev/null @@ -1,14 +0,0 @@ -import pytest -from test.conftest import launch_description -import time -from sensor_msgs.msg import JointState -from test.helpers import TopicGetsPublished - - -@pytest.mark.launch(fixture=launch_description) -def test_driver_connnects_to_gripper_dummy(launch_context, isolated, gripper_dummy): - until_dummy_ready = 3 - timeout = 3 - - time.sleep(until_dummy_ready) - assert TopicGetsPublished("/joint_states", JointState).event.wait(timeout) diff --git a/schunk_egu_egk_gripper_tests/test/test_launch.py b/schunk_egu_egk_gripper_tests/test/test_launch.py index 3b85f46..6b86c54 100644 --- a/schunk_egu_egk_gripper_tests/test/test_launch.py +++ b/schunk_egu_egk_gripper_tests/test/test_launch.py @@ -6,7 +6,7 @@ @pytest.mark.launch(fixture=launch_description) -def test_normal_startup_works(launch_context, isolated, gripper_dummy): +def test_normal_startup_works(launch_context, isolated): node = Node("test_startup") until_ready = 2.0 # sec time.sleep(until_ready) diff --git a/schunk_egu_egk_gripper_tests/test/test_ros_interfaces.py b/schunk_egu_egk_gripper_tests/test/test_ros_interfaces.py deleted file mode 100644 index 48651fe..0000000 --- a/schunk_egu_egk_gripper_tests/test/test_ros_interfaces.py +++ /dev/null @@ -1,59 +0,0 @@ -#!/usr/bin/env python3 -import pytest -from rclpy.node import Node -import time -from test.conftest import launch_description - - -def check_each_in(elements: list, node_method: str) -> None: - node = Node("test") - until_ready = 2.0 # sec - time.sleep(until_ready) - existing = getattr(node, node_method)() - advertised = [i[0] for i in existing] - for element in elements: - assert element in advertised - - -@pytest.mark.launch(fixture=launch_description) -def test_driver_advertices_all_relevant_topics(launch_context, isolated, gripper_dummy): - topic_list = [ - "/diagnostics", - "/joint_states", - "/state", - ] - check_each_in(topic_list, "get_topic_names_and_types") - - -@pytest.mark.launch(fixture=launch_description) -def test_driver_advertices_all_relevant_services( - launch_context, isolated, gripper_dummy -): - service_list = [ - "/acknowledge", - "/brake_test", - "/fast_stop", - "/gripper_info", - "/prepare_for_shutdown", - "/reconnect", - "/release_for_manual_movement", - "/softreset", - "/stop", - ] - check_each_in(service_list, "get_service_names_and_types") - - -@pytest.mark.launch(fixture=launch_description) -def test_driver_advertices_all_relevant_actions( - launch_context, isolated, gripper_dummy -): - action_list = [ - "/grip", - "/grip_with_position", - "/gripper_control", - "/move_to_absolute_position", - "/move_to_relative_position", - "/release_workpiece", - ] - action_list = [a + "/_action/status" for a in action_list] - check_each_in(action_list, "get_topic_names_and_types") diff --git a/schunk_egu_egk_gripper_tests/test/test_services.py b/schunk_egu_egk_gripper_tests/test/test_services.py new file mode 100644 index 0000000..978ce49 --- /dev/null +++ b/schunk_egu_egk_gripper_tests/test/test_services.py @@ -0,0 +1,92 @@ +import pytest +from test.conftest import launch_description +from test.helpers import check_each_in +from schunk_egu_egk_gripper_interfaces.srv import ( # type: ignore[attr-defined] + Acknowledge, + BrakeTest, + FastStop, + Stop, + ReleaseForManualMovement, + GripperInfo, + PrepareForShutdown, + Softreset, +) +from test.helpers import ServiceReturnsResult + + +@pytest.mark.launch(fixture=launch_description) +def test_driver_advertices_all_relevant_services(running_driver): + service_list = [ + "/acknowledge", + "/brake_test", + "/fast_stop", + "/gripper_info", + "/prepare_for_shutdown", + "/release_for_manual_movement", + "/softreset", + "/stop", + ] + check_each_in(service_list, "get_service_names_and_types") + + +@pytest.mark.launch(fixture=launch_description) +def test_driver_is_ready_after_acknowledge(running_driver): + service = ServiceReturnsResult("/acknowledge", Acknowledge, Acknowledge.Request()) + service.event.wait(timeout=1) + assert service.result.success is True + + +@pytest.mark.launch(fixture=launch_description) +def test_driver_supports_break_test(running_driver): + service = ServiceReturnsResult("/brake_test", BrakeTest, BrakeTest.Request()) + service.event.wait(timeout=1) + assert service.result.success is True + + +@pytest.mark.launch(fixture=launch_description) +def test_driver_supports_fast_stop(running_driver): + service = ServiceReturnsResult("/fast_stop", FastStop, FastStop.Request()) + service.event.wait(timeout=1) + assert service.result.success is True + + +@pytest.mark.launch(fixture=launch_description) +def test_driver_supports_gripper_info(running_driver): + service = ServiceReturnsResult("/gripper_info", GripperInfo, GripperInfo.Request()) + service.event.wait(timeout=1) + assert service.result.success is True + assert service.result.info != "" + + +@pytest.mark.launch(fixture=launch_description) +def test_driver_supports_prepare_for_shutdown(running_driver): + service = ServiceReturnsResult( + "/prepare_for_shutdown", PrepareForShutdown, PrepareForShutdown.Request() + ) + service.event.wait(timeout=1) + assert service.result.success is True + + +@pytest.mark.launch(fixture=launch_description) +def test_driver_supports_release_for_manual_movement(running_driver): + service = ServiceReturnsResult( + "/release_for_manual_movement", + ReleaseForManualMovement, + ReleaseForManualMovement.Request(), + ) + service.event.wait(timeout=1) + assert service.result.success is True + + +@pytest.mark.launch(fixture=launch_description) +def test_driver_supports_softreset(running_driver): + service = ServiceReturnsResult("/softreset", Softreset, Softreset.Request()) + service.event.wait(timeout=1) + assert service.result.success is True + + +@pytest.mark.launch(fixture=launch_description) +def test_driver_supports_stop(running_driver): + service = ServiceReturnsResult("/stop", Stop, Stop.Request()) + service.event.wait(timeout=1) + assert service.result.success is True diff --git a/schunk_egu_egk_gripper_tests/test/test_topics.py b/schunk_egu_egk_gripper_tests/test/test_topics.py new file mode 100644 index 0000000..315e5a0 --- /dev/null +++ b/schunk_egu_egk_gripper_tests/test/test_topics.py @@ -0,0 +1,13 @@ +import pytest +from test.conftest import launch_description +from test.helpers import check_each_in + + +@pytest.mark.launch(fixture=launch_description) +def test_driver_advertices_all_relevant_topics(running_driver): + topic_list = [ + "/diagnostics", + "/joint_states", + "/state", + ] + check_each_in(topic_list, "get_topic_names_and_types")