diff --git a/.flake8 b/.flake8 index f578405..f40261f 100644 --- a/.flake8 +++ b/.flake8 @@ -2,3 +2,6 @@ [flake8] # Use black's line length (default 88) instead of the flake8 default of 79: max-line-length = 88 + +per-file-ignores = + schunk_egu_egk_gripper_dummy/*.py:E203 diff --git a/.github/script/install_dependencies.sh b/.github/script/install_dependencies.sh index 5b7b55b..1c7ac32 100755 --- a/.github/script/install_dependencies.sh +++ b/.github/script/install_dependencies.sh @@ -1,3 +1,19 @@ #/usr/bin/bash cd $HOME -apt-get install curl libcurl4-openssl-dev +apt-get install -y curl libcurl4-openssl-dev + +# Python dependencies +python_deps="fastapi uvicorn httpx requests coverage python-multipart" +os_name=$(lsb_release -cs) + +case $os_name in + jammy) # Ubuntu 22.04 + pip install --user $python_deps + ;; + kinetic) # Ubuntu 24.04 + pip install --break-system-packages $python_deps + ;; + *) # Newer + pip install --break-system-packages $python_deps + ;; +esac diff --git a/.github/workflows/industrial_ci_rolling_action.yml b/.github/workflows/industrial_ci_rolling_action.yml index 9bc91ab..78a21f9 100644 --- a/.github/workflows/industrial_ci_rolling_action.yml +++ b/.github/workflows/industrial_ci_rolling_action.yml @@ -16,7 +16,7 @@ jobs: fail-fast: false matrix: env: - - {ROS_DISTRO: rolling, ROS_REPO: testing} + #- {ROS_DISTRO: rolling, ROS_REPO: testing} - {ROS_DISTRO: rolling, ROS_REPO: main} runs-on: ubuntu-latest steps: diff --git a/.gitignore b/.gitignore index a9d2efb..aff6c7f 100644 --- a/.gitignore +++ b/.gitignore @@ -2,3 +2,6 @@ /install/ /log/ /.vscode + +# Ignore test coverage artifacts +*.coverage diff --git a/schunk_egu_egk_gripper_driver/launch/schunk.launch.py b/schunk_egu_egk_gripper_driver/launch/schunk.launch.py index 901d178..cb17c84 100644 --- a/schunk_egu_egk_gripper_driver/launch/schunk.launch.py +++ b/schunk_egu_egk_gripper_driver/launch/schunk.launch.py @@ -27,7 +27,12 @@ def generate_launch_description(): default_value="10.49.60.86", description="IP address of the gripper on your network", ) - args = [ip] + port = DeclareLaunchArgument( + "port", + default_value="80", + description="TCP/IP port of the gripper", + ) + args = [ip, port] container = Node( name="gripper_container", @@ -44,9 +49,10 @@ def generate_launch_description(): package="schunk_egu_egk_gripper_driver", plugin="SchunkGripperNode", name="schunk_gripper_driver", - namespace="EGK_50_M_B", + namespace="", parameters=[ {"IP": LaunchConfiguration("IP")}, + {"port": LaunchConfiguration("port")}, {"state_frq": 60.0}, {"rate": 10.0}, {"use_brk": False}, 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 5f4f68e..5758742 100644 --- a/schunk_egu_egk_gripper_driver/src/schunk_gripper_wrapper.cpp +++ b/schunk_egu_egk_gripper_driver/src/schunk_gripper_wrapper.cpp @@ -44,7 +44,10 @@ std::map inst_param = //Initialize the ROS Driver SchunkGripperNode::SchunkGripperNode(const rclcpp::NodeOptions &options) : rclcpp::Node("schunk_gripper_driver", options), - Gripper(this->declare_parameter("IP", "0.0.0.0", parameter_descriptor("IP-Address of the gripper"))), + Gripper( + this->declare_parameter("IP", "0.0.0.0", parameter_descriptor("IP-Address of the gripper")), + this->declare_parameter("port", 80, parameter_descriptor("TCP/IP port of the gripper")) + ), limiting_rate(1000) //limiting_rate for loops { //Callback groups @@ -1150,12 +1153,12 @@ void SchunkGripperNode::acknowledge_srv(const std::shared_ptracknowledged = true; + res->success = true; RCLCPP_WARN(this->get_logger(),"Acknowledged"); } else { - res->acknowledged = false; + res->success = false; RCLCPP_WARN(this->get_logger(),"Acknowledge failed!"); } } @@ -1163,7 +1166,7 @@ void SchunkGripperNode::acknowledge_srv(const std::shared_ptrget_logger(), "Failed Connection! %s", connection_error.c_str()); - res->acknowledged = false; + res->success = false; RCLCPP_WARN(this->get_logger(), "Acknowledge failed!"); } last_command = 0; diff --git a/schunk_egu_egk_gripper_dummy/README.md b/schunk_egu_egk_gripper_dummy/README.md new file mode 100644 index 0000000..66e8d24 --- /dev/null +++ b/schunk_egu_egk_gripper_dummy/README.md @@ -0,0 +1,25 @@ +# Schunk EGU/EGK Dummy +A minimalist protocol simulator for system tests. + +## Dependencies + +```bash +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 + ``` + +## Run tests locally + +```bash +pip install --user pytest httpx coverage +``` + +```bash +coverage run -m pytest tests/ +coverage report +``` diff --git a/schunk_egu_egk_gripper_dummy/config/README.md b/schunk_egu_egk_gripper_dummy/config/README.md new file mode 100644 index 0000000..186aab9 --- /dev/null +++ b/schunk_egu_egk_gripper_dummy/config/README.md @@ -0,0 +1,22 @@ +# Reading system parameters from a gripper +The system parameters are mentioned in the gripper's datasheet [Commissioning Instructions, Firmware 5.2 EGU with EtherNet/IP interface](https://stb.cloud.schunk.com/media/IM0046706.PDF). + +## Enums +The easiest approach is reading enums from a web browser. +Enums need to be accessed via the _DEC_ identifier. +For instance, reading available error codes for the enum `HEX=0x0118` (`DEC=280`) would be +```browser +http:///adi/enum.json?inst=280 +``` + +## Metadata + +```bash +http:///adi/metadata.json?offset=0&count=300 +``` + +## Data +Use this script that reads the data directly from the gripper: +```bash +./read_system_parameters.py +``` diff --git a/schunk_egu_egk_gripper_dummy/config/data.json b/schunk_egu_egk_gripper_dummy/config/data.json new file mode 100644 index 0000000..371216e --- /dev/null +++ b/schunk_egu_egk_gripper_dummy/config/data.json @@ -0,0 +1,170 @@ +{ + "0x0040": [ + "800000800000A25800000000D90000EF" + ], + "0x0048": [ + "05000000000000000000000000000000" + ], + "0x0100": [ + "0000" + ], + "0x0118": [ + "EF" + ], + "0x0120": [ + "00" + ], + "0x0128": [ + "001F" + ], + "0x0130": [ + "3238343A32363A3532203C307846343E206D6F766520626C6F636B6564206F6363757272656420696E2063757272656E74207374617465203420286170706C69636174696F6E5F73746174655F6D616368696E652E683A3A313334392900000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000" + ], + "0x0210": [ + "00000000" + ], + "0x0230": [ + "42263D87" + ], + "0x0238": [ + "00000000" + ], + "0x0380": [ + "0000" + ], + "0x03A8": [ + "3F828F5C" + ], + "0x03B0": [ + "000000000000000042A70000000000000000000000000000" + ], + "0x03B8": [ + "BF251EB8BEAD844D423A7AE1448EC57E44E16CEE45066271" + ], + "0x0500": [ + "12" + ], + "0x0528": [ + "3F800000" + ], + "0x0540": [ + "40A00000" + ], + "0x0580": [ + "40000000" + ], + "0x0588": [ + "42A60000" + ], + "0x05A8": [ + "40A00000" + ], + "0x0600": [ + "00000000" + ], + "0x0608": [ + "42A60000" + ], + "0x0610": [ + "00000000" + ], + "0x0628": [ + "40B00000" + ], + "0x0630": [ + "42E60000" + ], + "0x0650": [ + "41B00000" + ], + "0x0658": [ + "42960000" + ], + "0x0660": [ + "43160000" + ], + "0x06A8": [ + "00000000" + ], + "0x0800": [ + "4199999A" + ], + "0x0808": [ + "41F00000" + ], + "0x0810": [ + "4199999A" + ], + "0x0818": [ + "41E66666" + ], + "0x0820": [ + "C0A00000" + ], + "0x0828": [ + "42AA0000" + ], + "0x0840": [ + "421EAE6D" + ], + "0x0870": [ + "41BA5871" + ], + "0x0878": [ + "41C04897" + ], + "0x0880": [ + "41ACCCCD" + ], + "0x0888": [ + "41D33333" + ], + "0x0890": [ + "41ACCCCD" + ], + "0x0898": [ + "41D33333" + ], + "0x08A0": [ + "40A00000" + ], + "0x08A8": [ + "42960000" + ], + "0x1000": [ + "4E534E53303731390000000000000000" + ], + "0x1008": [ + "31323334350000000000000000000000" + ], + "0x1020": [ + "3B036ECF" + ], + "0x1100": [ + "536570202034203230323300" + ], + "0x1108": [ + "31383A30343A313400" + ], + "0x1110": [ + "01F6" + ], + "0x1118": [ + "352E322E302E38313839360000000000000000000000" + ], + "0x1120": [ + "322E312E3100000000000000" + ], + "0x1130": [ + "01" + ], + "0x1138": [ + "0030114844C7" + ], + "0x1330": [ + "01" + ], + "0x1400": [ + "00000A69" + ] +} diff --git a/schunk_egu_egk_gripper_dummy/config/enum.json b/schunk_egu_egk_gripper_dummy/config/enum.json new file mode 100644 index 0000000..8fe8663 --- /dev/null +++ b/schunk_egu_egk_gripper_dummy/config/enum.json @@ -0,0 +1,2178 @@ +{ + "0x0118": [ + { + "value": 0, + "string": "ERR_NONE" + }, + { + "value": 1, + "string": "" + }, + { + "value": 2, + "string": "" + }, + { + "value": 3, + "string": "ERR_NO_RIGHTS" + }, + { + "value": 4, + "string": "INF_UNKNOWN_CMD" + }, + { + "value": 5, + "string": "INF_FAILED" + }, + { + "value": 6, + "string": "" + }, + { + "value": 7, + "string": "" + }, + { + "value": 8, + "string": "" + }, + { + "value": 9, + "string": "" + }, + { + "value": 10, + "string": "" + }, + { + "value": 11, + "string": "" + }, + { + "value": 12, + "string": "" + }, + { + "value": 13, + "string": "" + }, + { + "value": 14, + "string": "" + }, + { + "value": 15, + "string": "" + }, + { + "value": 16, + "string": "" + }, + { + "value": 17, + "string": "" + }, + { + "value": 18, + "string": "INF_WRONG_TYPE" + }, + { + "value": 19, + "string": "" + }, + { + "value": 20, + "string": "INF_NO_AUTHORITY" + }, + { + "value": 21, + "string": "" + }, + { + "value": 22, + "string": "" + }, + { + "value": 23, + "string": "" + }, + { + "value": 24, + "string": "" + }, + { + "value": 25, + "string": "" + }, + { + "value": 26, + "string": "" + }, + { + "value": 27, + "string": "INF_VAL_LIM_MAX" + }, + { + "value": 28, + "string": "INF_VAL_LIM_MIN" + }, + { + "value": 29, + "string": "" + }, + { + "value": 30, + "string": "" + }, + { + "value": 31, + "string": "" + }, + { + "value": 32, + "string": "" + }, + { + "value": 33, + "string": "" + }, + { + "value": 34, + "string": "" + }, + { + "value": 35, + "string": "" + }, + { + "value": 36, + "string": "" + }, + { + "value": 37, + "string": "" + }, + { + "value": 38, + "string": "" + }, + { + "value": 39, + "string": "" + }, + { + "value": 40, + "string": "ERR_BT_FAILED" + }, + { + "value": 41, + "string": "" + }, + { + "value": 42, + "string": "" + }, + { + "value": 43, + "string": "" + }, + { + "value": 44, + "string": "" + }, + { + "value": 45, + "string": "" + }, + { + "value": 46, + "string": "" + }, + { + "value": 47, + "string": "" + }, + { + "value": 48, + "string": "" + }, + { + "value": 49, + "string": "" + }, + { + "value": 50, + "string": "" + }, + { + "value": 51, + "string": "" + }, + { + "value": 52, + "string": "" + }, + { + "value": 53, + "string": "" + }, + { + "value": 54, + "string": "" + }, + { + "value": 55, + "string": "" + }, + { + "value": 56, + "string": "" + }, + { + "value": 57, + "string": "" + }, + { + "value": 58, + "string": "" + }, + { + "value": 59, + "string": "" + }, + { + "value": 60, + "string": "" + }, + { + "value": 61, + "string": "" + }, + { + "value": 62, + "string": "" + }, + { + "value": 63, + "string": "" + }, + { + "value": 64, + "string": "" + }, + { + "value": 65, + "string": "" + }, + { + "value": 66, + "string": "" + }, + { + "value": 67, + "string": "" + }, + { + "value": 68, + "string": "" + }, + { + "value": 69, + "string": "" + }, + { + "value": 70, + "string": "" + }, + { + "value": 71, + "string": "" + }, + { + "value": 72, + "string": "" + }, + { + "value": 73, + "string": "" + }, + { + "value": 74, + "string": "" + }, + { + "value": 75, + "string": "" + }, + { + "value": 76, + "string": "" + }, + { + "value": 77, + "string": "" + }, + { + "value": 78, + "string": "" + }, + { + "value": 79, + "string": "" + }, + { + "value": 80, + "string": "" + }, + { + "value": 81, + "string": "" + }, + { + "value": 82, + "string": "" + }, + { + "value": 83, + "string": "" + }, + { + "value": 84, + "string": "" + }, + { + "value": 85, + "string": "" + }, + { + "value": 86, + "string": "" + }, + { + "value": 87, + "string": "" + }, + { + "value": 88, + "string": "" + }, + { + "value": 89, + "string": "" + }, + { + "value": 90, + "string": "" + }, + { + "value": 91, + "string": "" + }, + { + "value": 92, + "string": "" + }, + { + "value": 93, + "string": "" + }, + { + "value": 94, + "string": "" + }, + { + "value": 95, + "string": "" + }, + { + "value": 96, + "string": "" + }, + { + "value": 97, + "string": "" + }, + { + "value": 98, + "string": "" + }, + { + "value": 99, + "string": "" + }, + { + "value": 100, + "string": "" + }, + { + "value": 101, + "string": "" + }, + { + "value": 102, + "string": "" + }, + { + "value": 103, + "string": "" + }, + { + "value": 104, + "string": "" + }, + { + "value": 105, + "string": "" + }, + { + "value": 106, + "string": "" + }, + { + "value": 107, + "string": "" + }, + { + "value": 108, + "string": "ERR_MOT_TEMP_LO" + }, + { + "value": 109, + "string": "ERR_MOT_TEMP_HI" + }, + { + "value": 110, + "string": "" + }, + { + "value": 111, + "string": "" + }, + { + "value": 112, + "string": "ERR_LGC_TEMP_LO" + }, + { + "value": 113, + "string": "ERR_LGC_TEMP_HI" + }, + { + "value": 114, + "string": "ERR_LGC_VOLT_LO" + }, + { + "value": 115, + "string": "ERR_LGC_VOLT_HI" + }, + { + "value": 116, + "string": "ERR_MOT_VOLT_LO" + }, + { + "value": 117, + "string": "ERR_MOT_VOLT_HI" + }, + { + "value": 118, + "string": "" + }, + { + "value": 119, + "string": "" + }, + { + "value": 120, + "string": "" + }, + { + "value": 121, + "string": "" + }, + { + "value": 122, + "string": "" + }, + { + "value": 123, + "string": "" + }, + { + "value": 124, + "string": "ERR_REBOOT" + }, + { + "value": 125, + "string": "" + }, + { + "value": 126, + "string": "ERR_POWER_STAGE" + }, + { + "value": 127, + "string": "" + }, + { + "value": 128, + "string": "" + }, + { + "value": 129, + "string": "" + }, + { + "value": 130, + "string": "" + }, + { + "value": 131, + "string": "" + }, + { + "value": 132, + "string": "" + }, + { + "value": 133, + "string": "" + }, + { + "value": 134, + "string": "" + }, + { + "value": 135, + "string": "" + }, + { + "value": 136, + "string": "" + }, + { + "value": 137, + "string": "ERR_ZV_NOT_FOUND" + }, + { + "value": 138, + "string": "ERR_ENC_PHASE" + }, + { + "value": 139, + "string": "ERR_ENC_SIN_LO" + }, + { + "value": 140, + "string": "ERR_ENC_SIN_HI" + }, + { + "value": 141, + "string": "ERR_ENC_COS_LO" + }, + { + "value": 142, + "string": "ERR_ENC_COS_HI" + }, + { + "value": 143, + "string": "ERR_ENC_SHORTCUT" + }, + { + "value": 144, + "string": "WRN_LGC_TEMP_LO" + }, + { + "value": 145, + "string": "WRN_LGC_TEMP_HI" + }, + { + "value": 146, + "string": "WRN_MOT_TEMP_LO" + }, + { + "value": 147, + "string": "WRN_MOT_TEMP_HI" + }, + { + "value": 148, + "string": "WRN_NOT_FEASIBLE" + }, + { + "value": 149, + "string": "WRN_POS_LIMIT" + }, + { + "value": 150, + "string": "WRN_LGC_VOLT_LO" + }, + { + "value": 151, + "string": "WRN_LGC_VOLT_HI" + }, + { + "value": 152, + "string": "WRN_MOT_VOLT_LO" + }, + { + "value": 153, + "string": "WRN_MOT_VOLT_HI" + }, + { + "value": 154, + "string": "" + }, + { + "value": 155, + "string": "WRN_FLASH_FAILED" + }, + { + "value": 156, + "string": "WRN_ERASE_FAILED" + }, + { + "value": 157, + "string": "WRN_FACT_FAILED" + }, + { + "value": 158, + "string": "WRN_AUTH_FAILED" + }, + { + "value": 159, + "string": "WRN_SD_NOT_PREP" + }, + { + "value": 160, + "string": "" + }, + { + "value": 161, + "string": "WRN_NO_PART" + }, + { + "value": 162, + "string": "WRN_BCKUP_RSTORE" + }, + { + "value": 163, + "string": "" + }, + { + "value": 164, + "string": "" + }, + { + "value": 165, + "string": "" + }, + { + "value": 166, + "string": "" + }, + { + "value": 167, + "string": "" + }, + { + "value": 168, + "string": "" + }, + { + "value": 169, + "string": "" + }, + { + "value": 170, + "string": "" + }, + { + "value": 171, + "string": "" + }, + { + "value": 172, + "string": "" + }, + { + "value": 173, + "string": "" + }, + { + "value": 174, + "string": "" + }, + { + "value": 175, + "string": "" + }, + { + "value": 176, + "string": "" + }, + { + "value": 177, + "string": "" + }, + { + "value": 178, + "string": "" + }, + { + "value": 179, + "string": "" + }, + { + "value": 180, + "string": "ERR_ENC_PULSES" + }, + { + "value": 181, + "string": "ERR_ENC_NO_INDEX" + }, + { + "value": 182, + "string": "ERR_ENC_MN_INDEX" + }, + { + "value": 183, + "string": "ERR_ENC_HALL_ILL" + }, + { + "value": 184, + "string": "ERR_ENC_NO_HALL" + }, + { + "value": 185, + "string": "ERR_ENC_ABS_FAIL" + }, + { + "value": 186, + "string": "ERR_CHP_TIMEOUT" + }, + { + "value": 187, + "string": "ERR_CHP_FAILED" + }, + { + "value": 188, + "string": "" + }, + { + "value": 189, + "string": "" + }, + { + "value": 190, + "string": "" + }, + { + "value": 191, + "string": "" + }, + { + "value": 192, + "string": "" + }, + { + "value": 193, + "string": "" + }, + { + "value": 194, + "string": "" + }, + { + "value": 195, + "string": "" + }, + { + "value": 196, + "string": "" + }, + { + "value": 197, + "string": "" + }, + { + "value": 198, + "string": "" + }, + { + "value": 199, + "string": "" + }, + { + "value": 200, + "string": "" + }, + { + "value": 201, + "string": "" + }, + { + "value": 202, + "string": "" + }, + { + "value": 203, + "string": "" + }, + { + "value": 204, + "string": "" + }, + { + "value": 205, + "string": "" + }, + { + "value": 206, + "string": "" + }, + { + "value": 207, + "string": "" + }, + { + "value": 208, + "string": "" + }, + { + "value": 209, + "string": "" + }, + { + "value": 210, + "string": "" + }, + { + "value": 211, + "string": "" + }, + { + "value": 212, + "string": "ERR_INVAL_PHRASE" + }, + { + "value": 213, + "string": "ERR_SOFT_LOW" + }, + { + "value": 214, + "string": "ERR_SOFT_HIGH" + }, + { + "value": 215, + "string": "" + }, + { + "value": 216, + "string": "" + }, + { + "value": 217, + "string": "ERR_FAST_STOP" + }, + { + "value": 218, + "string": "" + }, + { + "value": 219, + "string": "" + }, + { + "value": 220, + "string": "" + }, + { + "value": 221, + "string": "" + }, + { + "value": 222, + "string": "ERR_CURRENT" + }, + { + "value": 223, + "string": "ERR_I2T" + }, + { + "value": 224, + "string": "" + }, + { + "value": 225, + "string": "ERR_INTERNAL" + }, + { + "value": 226, + "string": "" + }, + { + "value": 227, + "string": "" + }, + { + "value": 228, + "string": "ERR_TOO_FAST" + }, + { + "value": 229, + "string": "" + }, + { + "value": 230, + "string": "" + }, + { + "value": 231, + "string": "ERR_FLASH_FAILED" + }, + { + "value": 232, + "string": "" + }, + { + "value": 233, + "string": "" + }, + { + "value": 234, + "string": "" + }, + { + "value": 235, + "string": "" + }, + { + "value": 236, + "string": "" + }, + { + "value": 237, + "string": "" + }, + { + "value": 238, + "string": "" + }, + { + "value": 239, + "string": "ERR_COMM_LOST" + }, + { + "value": 240, + "string": "ERR_REF_ABORT_TO" + }, + { + "value": 241, + "string": "ERR_MOV_ABORT_TO" + }, + { + "value": 242, + "string": "ERR_NO_REF" + }, + { + "value": 243, + "string": "" + }, + { + "value": 244, + "string": "ERR_MOVE_BLOCKED" + }, + { + "value": 245, + "string": "ERR_UNKNOWN_HW" + }, + { + "value": 246, + "string": "ERR_BLOCK_FAILED" + }, + { + "value": 247, + "string": "ERR_NO_COMM" + }, + { + "value": 248, + "string": "ERR_WRONG_HW" + }, + { + "value": 249, + "string": "ERR_WRONG_MODULE" + }, + { + "value": 250, + "string": "ERR_SD_FAILED" + }, + { + "value": 251, + "string": "ERR_FLASH_LOST" + }, + { + "value": 252, + "string": "ERR_SW_COMM" + } + ], + "0x0120": [ + { + "value": 0, + "string": "ERR_NONE" + }, + { + "value": 1, + "string": "" + }, + { + "value": 2, + "string": "" + }, + { + "value": 3, + "string": "ERR_NO_RIGHTS" + }, + { + "value": 4, + "string": "INF_UNKNOWN_CMD" + }, + { + "value": 5, + "string": "INF_FAILED" + }, + { + "value": 6, + "string": "" + }, + { + "value": 7, + "string": "" + }, + { + "value": 8, + "string": "" + }, + { + "value": 9, + "string": "" + }, + { + "value": 10, + "string": "" + }, + { + "value": 11, + "string": "" + }, + { + "value": 12, + "string": "" + }, + { + "value": 13, + "string": "" + }, + { + "value": 14, + "string": "" + }, + { + "value": 15, + "string": "" + }, + { + "value": 16, + "string": "" + }, + { + "value": 17, + "string": "" + }, + { + "value": 18, + "string": "INF_WRONG_TYPE" + }, + { + "value": 19, + "string": "" + }, + { + "value": 20, + "string": "INF_NO_AUTHORITY" + }, + { + "value": 21, + "string": "" + }, + { + "value": 22, + "string": "" + }, + { + "value": 23, + "string": "" + }, + { + "value": 24, + "string": "" + }, + { + "value": 25, + "string": "" + }, + { + "value": 26, + "string": "" + }, + { + "value": 27, + "string": "INF_VAL_LIM_MAX" + }, + { + "value": 28, + "string": "INF_VAL_LIM_MIN" + }, + { + "value": 29, + "string": "" + }, + { + "value": 30, + "string": "" + }, + { + "value": 31, + "string": "" + }, + { + "value": 32, + "string": "" + }, + { + "value": 33, + "string": "" + }, + { + "value": 34, + "string": "" + }, + { + "value": 35, + "string": "" + }, + { + "value": 36, + "string": "" + }, + { + "value": 37, + "string": "" + }, + { + "value": 38, + "string": "" + }, + { + "value": 39, + "string": "" + }, + { + "value": 40, + "string": "ERR_BT_FAILED" + }, + { + "value": 41, + "string": "" + }, + { + "value": 42, + "string": "" + }, + { + "value": 43, + "string": "" + }, + { + "value": 44, + "string": "" + }, + { + "value": 45, + "string": "" + }, + { + "value": 46, + "string": "" + }, + { + "value": 47, + "string": "" + }, + { + "value": 48, + "string": "" + }, + { + "value": 49, + "string": "" + }, + { + "value": 50, + "string": "" + }, + { + "value": 51, + "string": "" + }, + { + "value": 52, + "string": "" + }, + { + "value": 53, + "string": "" + }, + { + "value": 54, + "string": "" + }, + { + "value": 55, + "string": "" + }, + { + "value": 56, + "string": "" + }, + { + "value": 57, + "string": "" + }, + { + "value": 58, + "string": "" + }, + { + "value": 59, + "string": "" + }, + { + "value": 60, + "string": "" + }, + { + "value": 61, + "string": "" + }, + { + "value": 62, + "string": "" + }, + { + "value": 63, + "string": "" + }, + { + "value": 64, + "string": "" + }, + { + "value": 65, + "string": "" + }, + { + "value": 66, + "string": "" + }, + { + "value": 67, + "string": "" + }, + { + "value": 68, + "string": "" + }, + { + "value": 69, + "string": "" + }, + { + "value": 70, + "string": "" + }, + { + "value": 71, + "string": "" + }, + { + "value": 72, + "string": "" + }, + { + "value": 73, + "string": "" + }, + { + "value": 74, + "string": "" + }, + { + "value": 75, + "string": "" + }, + { + "value": 76, + "string": "" + }, + { + "value": 77, + "string": "" + }, + { + "value": 78, + "string": "" + }, + { + "value": 79, + "string": "" + }, + { + "value": 80, + "string": "" + }, + { + "value": 81, + "string": "" + }, + { + "value": 82, + "string": "" + }, + { + "value": 83, + "string": "" + }, + { + "value": 84, + "string": "" + }, + { + "value": 85, + "string": "" + }, + { + "value": 86, + "string": "" + }, + { + "value": 87, + "string": "" + }, + { + "value": 88, + "string": "" + }, + { + "value": 89, + "string": "" + }, + { + "value": 90, + "string": "" + }, + { + "value": 91, + "string": "" + }, + { + "value": 92, + "string": "" + }, + { + "value": 93, + "string": "" + }, + { + "value": 94, + "string": "" + }, + { + "value": 95, + "string": "" + }, + { + "value": 96, + "string": "" + }, + { + "value": 97, + "string": "" + }, + { + "value": 98, + "string": "" + }, + { + "value": 99, + "string": "" + }, + { + "value": 100, + "string": "" + }, + { + "value": 101, + "string": "" + }, + { + "value": 102, + "string": "" + }, + { + "value": 103, + "string": "" + }, + { + "value": 104, + "string": "" + }, + { + "value": 105, + "string": "" + }, + { + "value": 106, + "string": "" + }, + { + "value": 107, + "string": "" + }, + { + "value": 108, + "string": "ERR_MOT_TEMP_LO" + }, + { + "value": 109, + "string": "ERR_MOT_TEMP_HI" + }, + { + "value": 110, + "string": "" + }, + { + "value": 111, + "string": "" + }, + { + "value": 112, + "string": "ERR_LGC_TEMP_LO" + }, + { + "value": 113, + "string": "ERR_LGC_TEMP_HI" + }, + { + "value": 114, + "string": "ERR_LGC_VOLT_LO" + }, + { + "value": 115, + "string": "ERR_LGC_VOLT_HI" + }, + { + "value": 116, + "string": "ERR_MOT_VOLT_LO" + }, + { + "value": 117, + "string": "ERR_MOT_VOLT_HI" + }, + { + "value": 118, + "string": "" + }, + { + "value": 119, + "string": "" + }, + { + "value": 120, + "string": "" + }, + { + "value": 121, + "string": "" + }, + { + "value": 122, + "string": "" + }, + { + "value": 123, + "string": "" + }, + { + "value": 124, + "string": "ERR_REBOOT" + }, + { + "value": 125, + "string": "" + }, + { + "value": 126, + "string": "ERR_POWER_STAGE" + }, + { + "value": 127, + "string": "" + }, + { + "value": 128, + "string": "" + }, + { + "value": 129, + "string": "" + }, + { + "value": 130, + "string": "" + }, + { + "value": 131, + "string": "" + }, + { + "value": 132, + "string": "" + }, + { + "value": 133, + "string": "" + }, + { + "value": 134, + "string": "" + }, + { + "value": 135, + "string": "" + }, + { + "value": 136, + "string": "" + }, + { + "value": 137, + "string": "ERR_ZV_NOT_FOUND" + }, + { + "value": 138, + "string": "ERR_ENC_PHASE" + }, + { + "value": 139, + "string": "ERR_ENC_SIN_LO" + }, + { + "value": 140, + "string": "ERR_ENC_SIN_HI" + }, + { + "value": 141, + "string": "ERR_ENC_COS_LO" + }, + { + "value": 142, + "string": "ERR_ENC_COS_HI" + }, + { + "value": 143, + "string": "ERR_ENC_SHORTCUT" + }, + { + "value": 144, + "string": "WRN_LGC_TEMP_LO" + }, + { + "value": 145, + "string": "WRN_LGC_TEMP_HI" + }, + { + "value": 146, + "string": "WRN_MOT_TEMP_LO" + }, + { + "value": 147, + "string": "WRN_MOT_TEMP_HI" + }, + { + "value": 148, + "string": "WRN_NOT_FEASIBLE" + }, + { + "value": 149, + "string": "WRN_POS_LIMIT" + }, + { + "value": 150, + "string": "WRN_LGC_VOLT_LO" + }, + { + "value": 151, + "string": "WRN_LGC_VOLT_HI" + }, + { + "value": 152, + "string": "WRN_MOT_VOLT_LO" + }, + { + "value": 153, + "string": "WRN_MOT_VOLT_HI" + }, + { + "value": 154, + "string": "" + }, + { + "value": 155, + "string": "WRN_FLASH_FAILED" + }, + { + "value": 156, + "string": "WRN_ERASE_FAILED" + }, + { + "value": 157, + "string": "WRN_FACT_FAILED" + }, + { + "value": 158, + "string": "WRN_AUTH_FAILED" + }, + { + "value": 159, + "string": "WRN_SD_NOT_PREP" + }, + { + "value": 160, + "string": "" + }, + { + "value": 161, + "string": "WRN_NO_PART" + }, + { + "value": 162, + "string": "WRN_BCKUP_RSTORE" + }, + { + "value": 163, + "string": "" + }, + { + "value": 164, + "string": "" + }, + { + "value": 165, + "string": "" + }, + { + "value": 166, + "string": "" + }, + { + "value": 167, + "string": "" + }, + { + "value": 168, + "string": "" + }, + { + "value": 169, + "string": "" + }, + { + "value": 170, + "string": "" + }, + { + "value": 171, + "string": "" + }, + { + "value": 172, + "string": "" + }, + { + "value": 173, + "string": "" + }, + { + "value": 174, + "string": "" + }, + { + "value": 175, + "string": "" + }, + { + "value": 176, + "string": "" + }, + { + "value": 177, + "string": "" + }, + { + "value": 178, + "string": "" + }, + { + "value": 179, + "string": "" + }, + { + "value": 180, + "string": "ERR_ENC_PULSES" + }, + { + "value": 181, + "string": "ERR_ENC_NO_INDEX" + }, + { + "value": 182, + "string": "ERR_ENC_MN_INDEX" + }, + { + "value": 183, + "string": "ERR_ENC_HALL_ILL" + }, + { + "value": 184, + "string": "ERR_ENC_NO_HALL" + }, + { + "value": 185, + "string": "ERR_ENC_ABS_FAIL" + }, + { + "value": 186, + "string": "ERR_CHP_TIMEOUT" + }, + { + "value": 187, + "string": "ERR_CHP_FAILED" + }, + { + "value": 188, + "string": "" + }, + { + "value": 189, + "string": "" + }, + { + "value": 190, + "string": "" + }, + { + "value": 191, + "string": "" + }, + { + "value": 192, + "string": "" + }, + { + "value": 193, + "string": "" + }, + { + "value": 194, + "string": "" + }, + { + "value": 195, + "string": "" + }, + { + "value": 196, + "string": "" + }, + { + "value": 197, + "string": "" + }, + { + "value": 198, + "string": "" + }, + { + "value": 199, + "string": "" + }, + { + "value": 200, + "string": "" + }, + { + "value": 201, + "string": "" + }, + { + "value": 202, + "string": "" + }, + { + "value": 203, + "string": "" + }, + { + "value": 204, + "string": "" + }, + { + "value": 205, + "string": "" + }, + { + "value": 206, + "string": "" + }, + { + "value": 207, + "string": "" + }, + { + "value": 208, + "string": "" + }, + { + "value": 209, + "string": "" + }, + { + "value": 210, + "string": "" + }, + { + "value": 211, + "string": "" + }, + { + "value": 212, + "string": "ERR_INVAL_PHRASE" + }, + { + "value": 213, + "string": "ERR_SOFT_LOW" + }, + { + "value": 214, + "string": "ERR_SOFT_HIGH" + }, + { + "value": 215, + "string": "" + }, + { + "value": 216, + "string": "" + }, + { + "value": 217, + "string": "ERR_FAST_STOP" + }, + { + "value": 218, + "string": "" + }, + { + "value": 219, + "string": "" + }, + { + "value": 220, + "string": "" + }, + { + "value": 221, + "string": "" + }, + { + "value": 222, + "string": "ERR_CURRENT" + }, + { + "value": 223, + "string": "ERR_I2T" + }, + { + "value": 224, + "string": "" + }, + { + "value": 225, + "string": "ERR_INTERNAL" + }, + { + "value": 226, + "string": "" + }, + { + "value": 227, + "string": "" + }, + { + "value": 228, + "string": "ERR_TOO_FAST" + }, + { + "value": 229, + "string": "" + }, + { + "value": 230, + "string": "" + }, + { + "value": 231, + "string": "ERR_FLASH_FAILED" + }, + { + "value": 232, + "string": "" + }, + { + "value": 233, + "string": "" + }, + { + "value": 234, + "string": "" + }, + { + "value": 235, + "string": "" + }, + { + "value": 236, + "string": "" + }, + { + "value": 237, + "string": "" + }, + { + "value": 238, + "string": "" + }, + { + "value": 239, + "string": "ERR_COMM_LOST" + }, + { + "value": 240, + "string": "ERR_REF_ABORT_TO" + }, + { + "value": 241, + "string": "ERR_MOV_ABORT_TO" + }, + { + "value": 242, + "string": "ERR_NO_REF" + }, + { + "value": 243, + "string": "" + }, + { + "value": 244, + "string": "ERR_MOVE_BLOCKED" + }, + { + "value": 245, + "string": "ERR_UNKNOWN_HW" + }, + { + "value": 246, + "string": "ERR_BLOCK_FAILED" + }, + { + "value": 247, + "string": "ERR_NO_COMM" + }, + { + "value": 248, + "string": "ERR_WRONG_HW" + }, + { + "value": 249, + "string": "ERR_WRONG_MODULE" + }, + { + "value": 250, + "string": "ERR_SD_FAILED" + }, + { + "value": 251, + "string": "ERR_FLASH_LOST" + }, + { + "value": 252, + "string": "ERR_SW_COMM" + } + ], + "0x0500": [ + { + "value": 0, + "string": "EGI_40" + }, + { + "value": 1, + "string": "EGI_80" + }, + { + "value": 2, + "string": "EGL2_90" + }, + { + "value": 3, + "string": "UG4_DIO_80" + }, + { + "value": 4, + "string": "EGL_C" + }, + { + "value": 5, + "string": "EGH" + }, + { + "value": 6, + "string": "EGU_50_N_B" + }, + { + "value": 7, + "string": "EGU_60_N_B" + }, + { + "value": 8, + "string": "EGU_70_N_B" + }, + { + "value": 9, + "string": "EGU_80_N_B" + }, + { + "value": 10, + "string": "EGU_50_M_B" + }, + { + "value": 11, + "string": "EGU_60_M_B" + }, + { + "value": 12, + "string": "EGU_70_M_B" + }, + { + "value": 13, + "string": "EGU_80_M_B" + }, + { + "value": 14, + "string": "EGK_25_N_B" + }, + { + "value": 15, + "string": "EGK_40_N_B" + }, + { + "value": 16, + "string": "EGK_50_N_B" + }, + { + "value": 17, + "string": "EGK_25_M_B" + }, + { + "value": 18, + "string": "EGK_40_M_B" + }, + { + "value": 19, + "string": "EGK_50_M_B" + }, + { + "value": 20, + "string": "EGU_50_N_SD" + }, + { + "value": 21, + "string": "EGU_60_N_SD" + }, + { + "value": 22, + "string": "EGU_70_N_SD" + }, + { + "value": 23, + "string": "EGU_80_N_SD" + }, + { + "value": 24, + "string": "EGU_50_M_SD" + }, + { + "value": 25, + "string": "EGU_60_M_SD" + }, + { + "value": 26, + "string": "EGU_70_M_SD" + }, + { + "value": 27, + "string": "EGU_80_M_SD" + } + ], + "0x1130": [ + { + "value": 0, + "string": "UNKNOWN" + }, + { + "value": 1, + "string": "PROFINET" + }, + { + "value": 2, + "string": "EtherNet/IP (TM)" + }, + { + "value": 3, + "string": "EtherCAT" + }, + { + "value": 4, + "string": "Modbus TCP" + }, + { + "value": 5, + "string": "Common Ethernet" + }, + { + "value": 6, + "string": "IOLink" + }, + { + "value": 7, + "string": "Modbus RTU" + } + ] +} diff --git a/schunk_egu_egk_gripper_dummy/config/metadata.json b/schunk_egu_egk_gripper_dummy/config/metadata.json new file mode 100644 index 0000000..78f0eb1 --- /dev/null +++ b/schunk_egu_egk_gripper_dummy/config/metadata.json @@ -0,0 +1,1679 @@ +[ + { + "instance": 64, + "name": "plc_sync_input", + "min": "00000000", + "max": "FFFFFFFF", + "datatype": 6, + "numelements": 4, + "access": 9 + }, + { + "instance": 72, + "name": "plc_sync_output", + "min": "00000000", + "max": "FFFFFFFF", + "datatype": 6, + "numelements": 4, + "access": 19 + }, + { + "instance": 256, + "name": "command_code", + "min": "0000", + "max": "FFFF", + "datatype": 5, + "numelements": 1, + "access": 3 + }, + { + "instance": 264, + "name": "system_state", + "min": "00000000", + "max": "FFFFFFFF", + "datatype": 6, + "numelements": 1, + "access": 1 + }, + { + "instance": 272, + "name": "ctrl_authority", + "min": "00", + "max": "92", + "datatype": 8, + "numelements": 1, + "access": 3 + }, + { + "instance": 280, + "name": "err_code", + "min": "00", + "max": "FC", + "datatype": 8, + "numelements": 1, + "access": 1 + }, + { + "instance": 288, + "name": "wrn_code", + "min": "00", + "max": "FC", + "datatype": 8, + "numelements": 1, + "access": 1 + }, + { + "instance": 296, + "name": "sys_msg_req", + "min": "0000", + "max": "FFFF", + "datatype": 5, + "numelements": 1, + "access": 3 + }, + { + "instance": 304, + "name": "sys_msg_buffer", + "min": null, + "max": null, + "datatype": 7, + "numelements": 214, + "access": 1 + }, + { + "instance": 512, + "name": "set_pos", + "min": "FF7FFFFF", + "max": "7F7FFFFF", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 520, + "name": "set_vel", + "min": "FF7FFFFF", + "max": "7F7FFFFF", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 528, + "name": "used_cur_limit", + "min": "FF7FFFFF", + "max": "7F7FFFFF", + "datatype": 18, + "numelements": 1, + "access": 1 + }, + { + "instance": 536, + "name": "set_acc", + "min": "FF7FFFFF", + "max": "7F7FFFFF", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 544, + "name": "set_force", + "min": "FF7FFFFF", + "max": "7F7FFFFF", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 552, + "name": "grp_dir", + "min": "00", + "max": "01", + "datatype": 0, + "numelements": 1, + "access": 3 + }, + { + "instance": 560, + "name": "actual_pos", + "min": "FF7FFFFF", + "max": "7F7FFFFF", + "datatype": 18, + "numelements": 1, + "access": 1 + }, + { + "instance": 568, + "name": "actual_vel", + "min": "FF7FFFFF", + "max": "7F7FFFFF", + "datatype": 18, + "numelements": 1, + "access": 1 + }, + { + "instance": 576, + "name": "actual_cur", + "min": "FF7FFFFF", + "max": "7F7FFFFF", + "datatype": 18, + "numelements": 1, + "access": 1 + }, + { + "instance": 584, + "name": "actual_pos_mse", + "min": "FF7FFFFF", + "max": "7F7FFFFF", + "datatype": 18, + "numelements": 1, + "access": 1 + }, + { + "instance": 592, + "name": "grp_pos_lock", + "min": "00", + "max": "01", + "datatype": 0, + "numelements": 1, + "access": 3 + }, + { + "instance": 768, + "name": "ds_sin_ofs_raw", + "min": "0000", + "max": "FFFF", + "datatype": 5, + "numelements": 1, + "access": 3 + }, + { + "instance": 776, + "name": "ds_cos_ofs_raw", + "min": "0000", + "max": "FFFF", + "datatype": 5, + "numelements": 1, + "access": 3 + }, + { + "instance": 784, + "name": "ds_line_count", + "min": "00000000", + "max": "7F7FFFFF", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 792, + "name": "ds_vel", + "min": "FF7FFFFF", + "max": "7F7FFFFF", + "datatype": 18, + "numelements": 1, + "access": 1 + }, + { + "instance": 800, + "name": "ds_angle", + "min": "FF7FFFFF", + "max": "7F7FFFFF", + "datatype": 18, + "numelements": 1, + "access": 1 + }, + { + "instance": 808, + "name": "pole_pairs", + "min": "0001", + "max": "FFFF", + "datatype": 5, + "numelements": 1, + "access": 3 + }, + { + "instance": 816, + "name": "mse_gear_ratio", + "min": "00000000", + "max": "7F7FFFFF", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 824, + "name": "brk_lag_time", + "min": "0000", + "max": "01F4", + "datatype": 5, + "numelements": 1, + "access": 3 + }, + { + "instance": 832, + "name": "zv_angle", + "min": "FF7FFFFF", + "max": "7F7FFFFF", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 840, + "name": "ds_cur_filter_k1", + "min": "00000000", + "max": "3F800000", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 856, + "name": "ds_pos_sens_inv", + "min": "00", + "max": "01", + "datatype": 0, + "numelements": 1, + "access": 3 + }, + { + "instance": 864, + "name": "ds_vel_ma_len", + "min": "3DCCCCCD", + "max": "40000000", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 872, + "name": "zvs_ofs", + "min": "00000000", + "max": "40C90FDB", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 880, + "name": "zvs_volt_len", + "min": "00000000", + "max": "41C00000", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 888, + "name": "brk_delay_time", + "min": "000A", + "max": "01F4", + "datatype": 5, + "numelements": 1, + "access": 3 + }, + { + "instance": 896, + "name": "grp_prehold_time", + "min": "0000", + "max": "EA60", + "datatype": 5, + "numelements": 1, + "access": 3 + }, + { + "instance": 904, + "name": "ofs_del_time", + "min": "000A", + "max": "01F4", + "datatype": 5, + "numelements": 1, + "access": 3 + }, + { + "instance": 912, + "name": "chp_ofs_on", + "min": "00000000", + "max": "42100000", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 920, + "name": "chp_ofs_off", + "min": "00000000", + "max": "42100000", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 928, + "name": "chp_mode", + "min": "00", + "max": "01", + "datatype": 8, + "numelements": 1, + "access": 3 + }, + { + "instance": 936, + "name": "dead_load_kg", + "min": "3DCCCCCD", + "max": "41700000", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 944, + "name": "tool_cent_point", + "min": "FF7FFFFF", + "max": "7F7FFFFF", + "datatype": 18, + "numelements": 6, + "access": 3 + }, + { + "instance": 952, + "name": "cent_of_mass", + "min": "FF7FFFFF", + "max": "7F7FFFFF", + "datatype": 18, + "numelements": 6, + "access": 3 + }, + { + "instance": 960, + "name": "brk_release_time", + "min": "0000", + "max": "0708", + "datatype": 5, + "numelements": 1, + "access": 3 + }, + { + "instance": 1024, + "name": "ctrl_cur_kr", + "min": "00000000", + "max": "7F7FFFFF", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 1032, + "name": "ctrl_cur_tn", + "min": "00000000", + "max": "7F7FFFFF", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 1040, + "name": "ctrl_cur_td", + "min": "00000000", + "max": "7F7FFFFF", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 1056, + "name": "ctrl_vel_kr", + "min": "00000000", + "max": "7F7FFFFF", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 1064, + "name": "ctrl_vel_tn", + "min": "00000000", + "max": "7F7FFFFF", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 1072, + "name": "ctrl_vel_td", + "min": "00000000", + "max": "7F7FFFFF", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 1088, + "name": "ctrl_pos_kr", + "min": "00000000", + "max": "7F7FFFFF", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 1096, + "name": "ctrl_pos_tn", + "min": "00000000", + "max": "7F7FFFFF", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 1104, + "name": "ctrl_pos_td", + "min": "00000000", + "max": "7F7FFFFF", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 1120, + "name": "inv_driving_dir", + "min": "00", + "max": "01", + "datatype": 0, + "numelements": 1, + "access": 3 + }, + { + "instance": 1128, + "name": "max_vector_len", + "min": "00000000", + "max": "7F7FFFFF", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 1136, + "name": "ctrl_ffwd_vel", + "min": "00000000", + "max": "7F7FFFFF", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 1280, + "name": "module_type", + "min": "00", + "max": "1B", + "datatype": 8, + "numelements": 1, + "access": 3 + }, + { + "instance": 1296, + "name": "set_vector_len", + "min": "0000", + "max": "FFFF", + "datatype": 5, + "numelements": 1, + "access": 3 + }, + { + "instance": 1304, + "name": "set_vector_vel", + "min": "8000", + "max": "7FFF", + "datatype": 2, + "numelements": 1, + "access": 3 + }, + { + "instance": 1312, + "name": "mb_cur_threshold", + "min": "3C23D70A", + "max": "7F7FFFFF", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 1320, + "name": "wp_lost_dst", + "min": "3DCCCCCD", + "max": "42480000", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 1328, + "name": "target_pos_win", + "min": "00000000", + "max": "7F7FFFFF", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 1336, + "name": "ctrl_extra_time", + "min": "00000000", + "max": "7F7FFFFF", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 1344, + "name": "wp_release_delta", + "min": "3F800000", + "max": "42480000", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 1352, + "name": "easing_time", + "min": "0000", + "max": "07D0", + "datatype": 5, + "numelements": 1, + "access": 3 + }, + { + "instance": 1368, + "name": "ref_dst", + "min": "00000000", + "max": "7F7FFFFF", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 1376, + "name": "mot_const_kt", + "min": "00000000", + "max": "7F7FFFFF", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 1384, + "name": "grp_calib_param", + "min": "FF7FFFFF", + "max": "7F7FFFFF", + "datatype": 18, + "numelements": 4, + "access": 3 + }, + { + "instance": 1400, + "name": "mb_dst_threshold", + "min": "38D1B717", + "max": "7F7FFFFF", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 1408, + "name": "grp_pos_margin", + "min": "3F800000", + "max": "41200000", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 1416, + "name": "max_phys_stroke", + "min": "3F800000", + "max": "43FA0000", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 1424, + "name": "ref_type", + "min": "00", + "max": "03", + "datatype": 8, + "numelements": 1, + "access": 3 + }, + { + "instance": 1432, + "name": "zvs_step_req", + "min": "00000000", + "max": "40C90FDB", + "datatype": 18, + "numelements": 1, + "access": 1 + }, + { + "instance": 1440, + "name": "zvs_step_done", + "min": "00000000", + "max": "40C90FDB", + "datatype": 18, + "numelements": 1, + "access": 1 + }, + { + "instance": 1448, + "name": "grp_prepos_delta", + "min": "3F800000", + "max": "42480000", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 1456, + "name": "mb_detect_time", + "min": "0050", + "max": "01F4", + "datatype": 5, + "numelements": 1, + "access": 3 + }, + { + "instance": 1536, + "name": "min_pos", + "min": "FF7FFFFF", + "max": "7F7FFFFF", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 1544, + "name": "max_pos", + "min": "FF7FFFFF", + "max": "7F7FFFFF", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 1552, + "name": "zero_pos_ofs", + "min": "C61C4000", + "max": "461C4000", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 1560, + "name": "max_mot_cur", + "min": "00000000", + "max": "7F7FFFFF", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 1568, + "name": "nom_mot_cur", + "min": "00000000", + "max": "7F7FFFFF", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 1576, + "name": "min_vel", + "min": "3F800000", + "max": "7F7FFFFF", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 1584, + "name": "max_vel", + "min": "41700000", + "max": "42E60000", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 1592, + "name": "min_acc", + "min": "42C80000", + "max": "7F7FFFFF", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 1600, + "name": "max_acc", + "min": "437A0000", + "max": "447A0000", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 1616, + "name": "max_grp_vel", + "min": "3F800000", + "max": "42480000", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 1624, + "name": "min_grp_force", + "min": "41700000", + "max": "43160000", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 1632, + "name": "max_grp_force", + "min": "42DC0000", + "max": "43960000", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 1664, + "name": "ds_sin_min_raw", + "min": "0000", + "max": "FFFF", + "datatype": 5, + "numelements": 1, + "access": 3 + }, + { + "instance": 1672, + "name": "ds_sin_max_raw", + "min": "0000", + "max": "FFFF", + "datatype": 5, + "numelements": 1, + "access": 3 + }, + { + "instance": 1680, + "name": "ds_cos_min_raw", + "min": "0000", + "max": "FFFF", + "datatype": 5, + "numelements": 1, + "access": 3 + }, + { + "instance": 1688, + "name": "ds_cos_max_raw", + "min": "0000", + "max": "FFFF", + "datatype": 5, + "numelements": 1, + "access": 3 + }, + { + "instance": 1696, + "name": "max_grp_force_sm", + "min": "00000000", + "max": "00000000", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 1704, + "name": "max_allow_force", + "min": "00000000", + "max": "00000000", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 1712, + "name": "min_pos_base", + "min": "00000000", + "max": "42A60000", + "datatype": 18, + "numelements": 1, + "access": 1 + }, + { + "instance": 1720, + "name": "max_pos_base", + "min": "00000000", + "max": "42A60000", + "datatype": 18, + "numelements": 1, + "access": 1 + }, + { + "instance": 2048, + "name": "min_err_mot_volt", + "min": "41980000", + "max": "42100000", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 2056, + "name": "max_err_mot_volt", + "min": "41980000", + "max": "42100000", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 2064, + "name": "min_err_lgc_volt", + "min": "40A00000", + "max": "42100000", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 2072, + "name": "max_err_lgc_volt", + "min": "40A00000", + "max": "42100000", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 2080, + "name": "min_err_lgc_temp", + "min": "C1A00000", + "max": "42B20000", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 2088, + "name": "max_err_lgc_temp", + "min": "C1980000", + "max": "42B40000", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 2096, + "name": "min_err_mot_temp", + "min": "C1A00000", + "max": "42EE0000", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 2104, + "name": "max_err_mot_temp", + "min": "C1980000", + "max": "43480000", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 2112, + "name": "meas_lgc_temp", + "min": "FF7FFFFF", + "max": "7F7FFFFF", + "datatype": 18, + "numelements": 1, + "access": 1 + }, + { + "instance": 2120, + "name": "meas_mot_temp", + "min": "FF7FFFFF", + "max": "7F7FFFFF", + "datatype": 18, + "numelements": 1, + "access": 1 + }, + { + "instance": 2128, + "name": "min_rec_lgc_temp", + "min": "C2200000", + "max": "42F00000", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 2136, + "name": "max_rec_lgc_temp", + "min": "FF7FFFFF", + "max": "7F7FFFFF", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 2144, + "name": "min_rec_mot_temp", + "min": "C2200000", + "max": "43480000", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 2152, + "name": "max_rec_mot_temp", + "min": "FF7FFFFF", + "max": "7F7FFFFF", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 2160, + "name": "meas_lgc_volt", + "min": "FF7FFFFF", + "max": "7F7FFFFF", + "datatype": 18, + "numelements": 1, + "access": 1 + }, + { + "instance": 2168, + "name": "meas_mot_volt", + "min": "FF7FFFFF", + "max": "7F7FFFFF", + "datatype": 18, + "numelements": 1, + "access": 1 + }, + { + "instance": 2176, + "name": "min_wrn_mot_volt", + "min": "41980000", + "max": "42100000", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 2184, + "name": "max_wrn_mot_volt", + "min": "41980000", + "max": "42100000", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 2192, + "name": "min_wrn_lgc_volt", + "min": "41300000", + "max": "42400000", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 2200, + "name": "max_wrn_lgc_volt", + "min": "41300000", + "max": "42400000", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 2208, + "name": "min_wrn_lgc_temp", + "min": "C1A00000", + "max": "42B20000", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 2216, + "name": "max_wrn_lgc_temp", + "min": "C1980000", + "max": "42B40000", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 2224, + "name": "min_wrn_mot_temp", + "min": "FF7FFFFF", + "max": "7F7FFFFF", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 2232, + "name": "max_wrn_mot_temp", + "min": "C1980000", + "max": "42F00000", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 2240, + "name": "mot_resistor", + "min": "00000000", + "max": "447A0000", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 2248, + "name": "mot_tp_volt_off", + "min": "C1C00000", + "max": "41C00000", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 4096, + "name": "serial_no_txt", + "min": null, + "max": null, + "datatype": 7, + "numelements": 16, + "access": 3 + }, + { + "instance": 4104, + "name": "order_no_txt", + "min": null, + "max": null, + "datatype": 7, + "numelements": 16, + "access": 3 + }, + { + "instance": 4112, + "name": "production_date", + "min": null, + "max": null, + "datatype": 7, + "numelements": 16, + "access": 3 + }, + { + "instance": 4120, + "name": "processor_id", + "min": "00000000", + "max": "FFFFFFFF", + "datatype": 6, + "numelements": 1, + "access": 1 + }, + { + "instance": 4128, + "name": "serial_no_num", + "min": "00000000", + "max": "FFFFFFFF", + "datatype": 6, + "numelements": 1, + "access": 3 + }, + { + "instance": 4352, + "name": "sw_build_date", + "min": null, + "max": null, + "datatype": 7, + "numelements": 12, + "access": 1 + }, + { + "instance": 4360, + "name": "sw_build_time", + "min": null, + "max": null, + "datatype": 7, + "numelements": 9, + "access": 1 + }, + { + "instance": 4368, + "name": "sw_version_num", + "min": "0000", + "max": "FFFF", + "datatype": 5, + "numelements": 1, + "access": 1 + }, + { + "instance": 4376, + "name": "sw_version_txt", + "min": null, + "max": null, + "datatype": 7, + "numelements": 22, + "access": 1 + }, + { + "instance": 4384, + "name": "comm_version_txt", + "min": null, + "max": null, + "datatype": 7, + "numelements": 12, + "access": 1 + }, + { + "instance": 4392, + "name": "build_info", + "min": null, + "max": null, + "datatype": { + "error": 2 + }, + "numelements": 21, + "access": { + "error": 2 + } + }, + { + "instance": 4400, + "name": "fieldbus_type", + "min": "00", + "max": "07", + "datatype": 8, + "numelements": 1, + "access": 1 + }, + { + "instance": 4408, + "name": "mac_addr", + "min": "00", + "max": "FF", + "datatype": 4, + "numelements": 6, + "access": 1 + }, + { + "instance": 4528, + "name": "compat_mode", + "min": "00", + "max": "01", + "datatype": 0, + "numelements": 1, + "access": 3 + }, + { + "instance": 4608, + "name": "actual_user", + "min": "00", + "max": "03", + "datatype": 8, + "numelements": 1, + "access": 1 + }, + { + "instance": 4616, + "name": "login", + "min": null, + "max": null, + "datatype": 7, + "numelements": 8, + "access": 2 + }, + { + "instance": 4624, + "name": "vary_service_pwd", + "min": null, + "max": null, + "datatype": 7, + "numelements": 8, + "access": 2 + }, + { + "instance": 4632, + "name": "vary_root_pwd", + "min": null, + "max": null, + "datatype": 7, + "numelements": 8, + "access": 2 + }, + { + "instance": 4864, + "name": "start_fw_flash", + "min": "00", + "max": "05", + "datatype": 8, + "numelements": 1, + "access": 2 + }, + { + "instance": 4872, + "name": "firmware_path", + "min": null, + "max": null, + "datatype": 7, + "numelements": 30, + "access": 1 + }, + { + "instance": 4888, + "name": "ref_angle", + "min": "C0C90FDB", + "max": "40C90FDB", + "datatype": 18, + "numelements": 1, + "access": 1 + }, + { + "instance": 4896, + "name": "reset_reasons", + "min": "000000000000000000000000000000000000000000000000", + "max": "FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF", + "datatype": { + "error": 2 + }, + "numelements": 6, + "access": { + "error": 2 + } + }, + { + "instance": 4904, + "name": "ref_pos", + "min": "FF7FFFFF", + "max": "7F7FFFFF", + "datatype": 18, + "numelements": 1, + "access": 1 + }, + { + "instance": 4912, + "name": "enable_softreset", + "min": "00", + "max": "01", + "datatype": 0, + "numelements": 1, + "access": 3 + }, + { + "instance": 4920, + "name": "ref_ofs", + "min": "FF7FFFFF", + "max": "7F7FFFFF", + "datatype": 18, + "numelements": 1, + "access": 1 + }, + { + "instance": 4928, + "name": "ref_vel", + "min": "3F800000", + "max": "7F7FFFFF", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 4936, + "name": "ref_acc", + "min": "42C80000", + "max": "447A0000", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 4992, + "name": "jog_acc", + "min": "42C80000", + "max": "447A0000", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 5112, + "name": "init_ended", + "min": "00", + "max": "01", + "datatype": 0, + "numelements": 1, + "access": 1 + }, + { + "instance": 5120, + "name": "system_uptime", + "min": "00000000", + "max": "FFFFFFFF", + "datatype": 6, + "numelements": 1, + "access": 1 + }, + { + "instance": 5128, + "name": "busy_h", + "min": "00000000", + "max": "FFFFFFFF", + "datatype": 6, + "numelements": 1, + "access": 1 + }, + { + "instance": 5136, + "name": "ctrl_active_h", + "min": "00000000", + "max": "FFFFFFFF", + "datatype": 6, + "numelements": 1, + "access": 1 + }, + { + "instance": 5144, + "name": "busy_h_s", + "min": "0000", + "max": "FFFF", + "datatype": 5, + "numelements": 1, + "access": 1 + }, + { + "instance": 5152, + "name": "ctrl_active_h_s", + "min": "FF7FFFFF", + "max": "7F7FFFFF", + "datatype": 18, + "numelements": 1, + "access": 1 + }, + { + "instance": 5160, + "name": "statistics", + "min": "00000000", + "max": "FFFFFFFF", + "datatype": 6, + "numelements": 24, + "access": 1 + }, + { + "instance": 5168, + "name": "summed_dst", + "min": "0000000000000000", + "max": "FFFFFFFFFFFFFFFF", + "datatype": 17, + "numelements": 1, + "access": 1 + }, + { + "instance": 5176, + "name": "zv_hall_sect", + "min": "0000", + "max": "0008", + "datatype": 5, + "numelements": 1, + "access": 3 + }, + { + "instance": 5192, + "name": "zvs_to_idx_ofs", + "min": "C0C90FDB", + "max": "40C90FDB", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 5400, + "name": "pos_sens_sin_raw", + "min": "0000", + "max": "FFFF", + "datatype": 5, + "numelements": 1, + "access": 1 + }, + { + "instance": 5408, + "name": "pos_sens_cos_raw", + "min": "0000", + "max": "FFFF", + "datatype": 5, + "numelements": 1, + "access": 1 + }, + { + "instance": 5416, + "name": "cur_u_comp", + "min": "00000000", + "max": "7F7FFFFF", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 5424, + "name": "cur_v_comp", + "min": "00000000", + "max": "7F7FFFFF", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 5432, + "name": "cur_w_comp", + "min": "00000000", + "max": "7F7FFFFF", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 5440, + "name": "max_angle_dev", + "min": "00000000", + "max": "40C9999A", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 5448, + "name": "max_mse_chk_viol", + "min": "0000", + "max": "FFFE", + "datatype": 5, + "numelements": 1, + "access": 3 + }, + { + "instance": 5456, + "name": "mse_ticks", + "min": "0000", + "max": "FFFF", + "datatype": 5, + "numelements": 1, + "access": 3 + }, + { + "instance": 5464, + "name": "mse_diff", + "min": "3727C5AC", + "max": "3F7FFF58", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 5472, + "name": "gse_zero_pos", + "min": "00000000", + "max": "00007FFF", + "datatype": 3, + "numelements": 1, + "access": 3 + }, + { + "instance": 5480, + "name": "gse_gear_ratio", + "min": "00000000", + "max": "7F7FFFFF", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 5496, + "name": "zero_pos_fix_dst", + "min": "00000000", + "max": "7F7FFFFF", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 5504, + "name": "gse_ticks", + "min": "0000", + "max": "FFFF", + "datatype": 5, + "numelements": 1, + "access": 3 + }, + { + "instance": 5520, + "name": "idx_lost_limit", + "min": "0000", + "max": "FFFF", + "datatype": 5, + "numelements": 1, + "access": 3 + }, + { + "instance": 5528, + "name": "idx_exs_limit", + "min": "0000", + "max": "FFFF", + "datatype": 5, + "numelements": 1, + "access": 3 + }, + { + "instance": 5536, + "name": "gs_to_ms_lag", + "min": "00000000", + "max": "7F7FFFFF", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 5632, + "name": "bt_cur", + "min": "3DCCCCCD", + "max": "402147AE", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 5640, + "name": "bt_duration", + "min": "01F4", + "max": "03E8", + "datatype": 5, + "numelements": 1, + "access": 3 + }, + { + "instance": 5648, + "name": "bt_result", + "min": "00", + "max": "01", + "datatype": 0, + "numelements": 1, + "access": 3 + }, + { + "instance": 5888, + "name": "hw_platform", + "min": "00000000", + "max": "FFFFFFFF", + "datatype": 6, + "numelements": 1, + "access": 1 + }, + { + "instance": 5928, + "name": "internal_params", + "min": "00000000000000000000000000000000000000000000000000000000000000000000000000", + "max": "FFFFFFFFFFFFFFFF01FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF", + "datatype": { + "error": 2 + }, + "numelements": 11, + "access": { + "error": 2 + } + }, + { + "instance": 5936, + "name": "mvm_int", + "min": "00000000", + "max": "FFFFFFFF", + "datatype": 6, + "numelements": 1, + "access": 1 + }, + { + "instance": 5952, + "name": "mech_efficiency", + "min": "3727C5AC", + "max": "3F800000", + "datatype": 18, + "numelements": 1, + "access": 3 + }, + { + "instance": 8488, + "name": "dbg_msg_req", + "min": "0000", + "max": "FFFF", + "datatype": 5, + "numelements": 1, + "access": 3 + }, + { + "instance": 8496, + "name": "dbg_msg_buffer", + "min": "0000", + "max": "FFFF", + "datatype": 5, + "numelements": 128, + "access": 1 + } +] diff --git a/schunk_egu_egk_gripper_dummy/config/read_system_parameters.py b/schunk_egu_egk_gripper_dummy/config/read_system_parameters.py new file mode 100755 index 0000000..dd73ef4 --- /dev/null +++ b/schunk_egu_egk_gripper_dummy/config/read_system_parameters.py @@ -0,0 +1,52 @@ +#!/usr/bin/env python3 +import requests # type: ignore +import json +import argparse + +parser = argparse.ArgumentParser() +parser.add_argument( + "ip", help="The IP address of the gripper on the network.", type=str +) +args = parser.parse_args() + +parameter_codes = "./system_parameter_codes" +parameters = "./data.json" + + +def read_parameter_codes(filepath: str) -> list[str]: + def contains_hex(line: str) -> bool: + return True if "0x" in line and line[0] != "#" else False + + def remove_comments(line: str) -> str: + if line.find("#") != -1: + line = line[0 : line.find("#")] + line = line.strip() + return line + + with open(filepath, "r") as f: + lines = f.read().split("\n") + lines = list(filter(contains_hex, lines)) + lines = list(map(remove_comments, lines)) + return lines + + +def main(): + codes = read_parameter_codes(parameter_codes) + + values = {} + for code in codes: + print(f"reading code {code}") + try: + response = requests.get( + f"http://{args.ip}/adi/data.json?inst={int(code, 16)}&count=1" + ) + values[code] = response.json() + except Exception as e: + print(f"error reading code {code}: {e}") + + with open(parameters, "w") as f: + json.dump(values, f, indent=4) + + +if __name__ == "__main__": + main() diff --git a/schunk_egu_egk_gripper_dummy/config/system_parameter_codes b/schunk_egu_egk_gripper_dummy/config/system_parameter_codes new file mode 100644 index 0000000..0b38c21 --- /dev/null +++ b/schunk_egu_egk_gripper_dummy/config/system_parameter_codes @@ -0,0 +1,61 @@ +# See Commissioning Instructions, Firmware 5.2 EGU with EtherNet/IP interface +# Universal gripper, electric +# https://stb.cloud.schunk.com/media/IM0046706.PDF + + +0x0040 +0x0048 +0x0100 +0x0118 # enum +0x0120 # enum +0x0128 +0x0130 +0x0210 +0x0230 +0x0238 +0x0380 +0x03A8 +0x03B0 +0x03B8 +0x0500 # enum +0x0528 +0x0540 +0x0580 +0x0588 +0x05A8 +0x0600 +0x0608 +0x0610 +0x0628 +0x0630 +0x0650 +0x0658 +0x0660 +0x06A8 +0x0800 +0x0808 +0x0810 +0x0818 +0x0820 +0x0828 +0x0840 +0x0870 +0x0878 +0x0880 +0x0888 +0x0890 +0x0898 +0x08A0 +0x08A8 +0x1000 +0x1008 +0x1020 +0x1100 +0x1108 +0x1110 +0x1118 +0x1120 +0x1130 # enum +0x1138 +0x1330 +0x1400 diff --git a/schunk_egu_egk_gripper_dummy/doc/plc_control_double_word.png b/schunk_egu_egk_gripper_dummy/doc/plc_control_double_word.png new file mode 100644 index 0000000..b25d51f Binary files /dev/null and b/schunk_egu_egk_gripper_dummy/doc/plc_control_double_word.png differ diff --git a/schunk_egu_egk_gripper_dummy/doc/plc_data_exchange.png b/schunk_egu_egk_gripper_dummy/doc/plc_data_exchange.png new file mode 100644 index 0000000..97003d9 Binary files /dev/null and b/schunk_egu_egk_gripper_dummy/doc/plc_data_exchange.png differ diff --git a/schunk_egu_egk_gripper_dummy/doc/plc_diagnostics_double_word.png b/schunk_egu_egk_gripper_dummy/doc/plc_diagnostics_double_word.png new file mode 100644 index 0000000..f16354b Binary files /dev/null and b/schunk_egu_egk_gripper_dummy/doc/plc_diagnostics_double_word.png differ diff --git a/schunk_egu_egk_gripper_dummy/doc/plc_input_data_frame.png b/schunk_egu_egk_gripper_dummy/doc/plc_input_data_frame.png new file mode 100644 index 0000000..1a0031b Binary files /dev/null and b/schunk_egu_egk_gripper_dummy/doc/plc_input_data_frame.png differ diff --git a/schunk_egu_egk_gripper_dummy/doc/plc_output_data_frame.png b/schunk_egu_egk_gripper_dummy/doc/plc_output_data_frame.png new file mode 100644 index 0000000..862cc96 Binary files /dev/null and b/schunk_egu_egk_gripper_dummy/doc/plc_output_data_frame.png differ diff --git a/schunk_egu_egk_gripper_dummy/doc/plc_status_double_word.png b/schunk_egu_egk_gripper_dummy/doc/plc_status_double_word.png new file mode 100644 index 0000000..349bd25 Binary files /dev/null and b/schunk_egu_egk_gripper_dummy/doc/plc_status_double_word.png differ diff --git a/schunk_egu_egk_gripper_dummy/package.xml b/schunk_egu_egk_gripper_dummy/package.xml new file mode 100644 index 0000000..2191cf0 --- /dev/null +++ b/schunk_egu_egk_gripper_dummy/package.xml @@ -0,0 +1,13 @@ + + + + schunk_egu_egk_gripper_dummy + 0.0.0 + A minimalist dummy for simulating the gripper's communication + Stefan Scherzinger + GPL-3.0-or-later + + + ament_python + + diff --git a/schunk_egu_egk_gripper_dummy/resource/schunk_egu_egk_gripper_dummy b/schunk_egu_egk_gripper_dummy/resource/schunk_egu_egk_gripper_dummy new file mode 100644 index 0000000..e69de29 diff --git a/schunk_egu_egk_gripper_dummy/schunk_egu_egk_gripper_dummy/__init__.py b/schunk_egu_egk_gripper_dummy/schunk_egu_egk_gripper_dummy/__init__.py new file mode 100644 index 0000000..e69de29 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 new file mode 100644 index 0000000..46ec41f --- /dev/null +++ b/schunk_egu_egk_gripper_dummy/schunk_egu_egk_gripper_dummy/main.py @@ -0,0 +1,51 @@ +from src.dummy import Dummy + +from fastapi import FastAPI, Request, Form, BackgroundTasks +from fastapi.middleware.cors import CORSMiddleware +from typing import Optional +import string + +# Components +dummy = Dummy() +server = FastAPI() +client = ["http://localhost:8001"] + + +server.add_middleware( + CORSMiddleware, + allow_origins=client, + allow_credentials=True, + allow_methods=["*"], + allow_headers=[""], +) + + +@server.post("/adi/update.json") +async def post( + inst: str = Form(...), + value: str = Form(...), + elem: Optional[int] = Form(None), + callback: Optional[str] = Form(None), + background_tasks: BackgroundTasks = None, +): + msg = {"inst": inst, "value": value, "elem": elem, "callback": callback} + + if msg["inst"] not in dummy.data: + return {"result": 1} + if not all(digit in string.hexdigits for digit in str(msg["value"])): + return {"result": 1} + background_tasks.add_task(dummy.post, msg) + return {"result": 0} + + +@server.get("/adi/{path}") +async def get(request: Request): + path = request.path_params["path"] + params = request.query_params + if path == "info.json": + return dummy.get_info(params) + if path == "enum.json": + return dummy.get_enum(params) + if path == "data.json": + return dummy.get_data(params) + return None diff --git a/schunk_egu_egk_gripper_dummy/setup.cfg b/schunk_egu_egk_gripper_dummy/setup.cfg new file mode 100644 index 0000000..31b0841 --- /dev/null +++ b/schunk_egu_egk_gripper_dummy/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/schunk_egu_egk_gripper_dummy +[install] +install_scripts=$base/lib/schunk_egu_egk_gripper_dummy diff --git a/schunk_egu_egk_gripper_dummy/setup.py b/schunk_egu_egk_gripper_dummy/setup.py new file mode 100644 index 0000000..1614c37 --- /dev/null +++ b/schunk_egu_egk_gripper_dummy/setup.py @@ -0,0 +1,28 @@ +from setuptools import find_packages, setup +import os +from glob import glob + +package_name = "schunk_egu_egk_gripper_dummy" + +setup( + name=package_name, + version="0.0.0", + packages=find_packages(exclude=["tests"]), + data_files=[ + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + (os.path.join("share", package_name), [package_name + "/main.py"]), + (os.path.join("share", package_name, "src"), glob("src/*.py")), + (os.path.join("share", package_name, "config"), glob("config/*.json")), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="Stefan Scherzinger", + maintainer_email="stefan.scherzinger@de.schunk.com", + description="A minimalist dummy for simulating the gripper's communication", + license="GPL-3.0-or-later", + tests_require=["pytest", "coverage"], + entry_points={ + "console_scripts": [], + }, +) diff --git a/schunk_egu_egk_gripper_dummy/src/__init__.py b/schunk_egu_egk_gripper_dummy/src/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/schunk_egu_egk_gripper_dummy/src/dummy.py b/schunk_egu_egk_gripper_dummy/src/dummy.py new file mode 100644 index 0000000..2d7a045 --- /dev/null +++ b/schunk_egu_egk_gripper_dummy/src/dummy.py @@ -0,0 +1,265 @@ +from threading import Thread +import time +import os +from pathlib import Path +import json +import struct +from typing import Tuple + + +class LinearMotion(object): + def __init__( + self, + initial_pos: float, + initial_speed: float, + target_pos: float, + target_speed: float, + ): + self.min_speed = 0.001 + self.initial_pos = initial_pos + self.initial_speed = max(0.0, initial_speed) + self.target_pos = target_pos + self.target_speed = max(self.min_speed, target_speed) + self.time_finish = abs(self.target_pos / self.target_speed) + + def sample(self, t: float) -> Tuple[float, float]: + if t <= 0.0: + return (self.initial_pos, self.initial_speed) + if t >= self.time_finish: + return (self.target_pos, 0.0) + + v = self.target_speed + if self.target_pos < self.initial_pos: + v = -v + current_pos = v * t + self.initial_pos + current_speed = abs(v) + return (current_pos, current_speed) + + +class Dummy(object): + def __init__(self): + self.thread = Thread(target=self._run) + self.running = False + self.done = False + self.enum = None + self.metadata = None + self.data = None + self.plc_input = "0x0040" + self.plc_output = "0x0048" + self.actual_position = "0x0230" + self.actual_speed = "0x0238" + self.error_byte = 12 + self.diagnostics_byte = 15 + self.valid_status_bits = list(range(0, 10)) + [11, 12, 13, 14, 16, 17, 31] + self.valid_control_bits = list(range(0, 10)) + [11, 12, 13, 14, 16, 30, 31] + self.reserved_status_bits = [10, 15] + list(range(18, 31)) + self.reserved_control_bits = [10, 15] + list(range(17, 30)) + + enum_config = os.path.join( + Path(__file__).resolve().parents[1], "config/enum.json" + ) + metadata_config = os.path.join( + Path(__file__).resolve().parents[1], "config/metadata.json" + ) + data_config = os.path.join( + Path(__file__).resolve().parents[1], "config/data.json" + ) + with open(enum_config, "r") as f: + self.enum = json.load(f) + with open(metadata_config, "r") as f: + self.metadata = json.load(f) + with open(data_config, "r") as f: + self.data = json.load(f) + + 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])) + + def start(self) -> None: + if self.running: + return + self.thread.start() + self.running = True + + def stop(self) -> None: + self.done = True + self.thread.join() + self.running = False + + def _run(self) -> None: + while not self.done: + time.sleep(1) + print("Done") + + def move(self, target_pos: float, target_speed: float) -> None: + motion = LinearMotion( + initial_pos=self.get_actual_position(), + initial_speed=self.get_actual_speed(), + target_pos=target_pos, + target_speed=target_speed, + ) + start = time.time() + actual_pos, actual_speed = motion.sample(0) + while abs(actual_pos) < abs(target_pos): + t = time.time() - start + actual_pos, actual_speed = motion.sample(t) + self.set_actual_position(actual_pos) + self.set_actual_speed(actual_speed) + time.sleep(0.01) + + def post(self, msg: dict) -> dict: + if msg["inst"] == self.plc_output: + self.plc_output_buffer = bytearray(bytes.fromhex(msg["value"])) + else: + self.data[msg["inst"]] = [msg["value"]] + + # Behavior + self.process_control_bits() + return {"result": 0} + + def get_info(self, query: dict[str, str]) -> dict: + return {"dataformat": 0} # 0: Little endian, 1: Big endian + + def get_enum(self, query: dict[str, str]) -> list: + if "inst" not in query or "value" not in query: + return [] + inst = query["inst"] + value = int(query["value"]) + if inst in self.enum: + string = self.enum[inst][value]["string"] + return [{"string": string, "value": value}] + else: + return [] + + def get_data(self, query: dict[str, str]) -> list: + result: list = [] + if "offset" in query and "count" in query: + offset = int(query["offset"]) + count = int(query["count"]) + if offset < 0 or count < 0: + return result + if offset + count >= len(self.metadata): + return result + for i in range(count): + result.append(self.metadata[offset + i]) + return result + + if "inst" in query and "count" in query: + inst = query["inst"] + count = int(query["count"]) + if count != 1: + return result + if inst not in self.data: + return result + if inst == self.plc_input: + return self.get_plc_input() + if inst == self.plc_output: + return self.get_plc_output() + return self.data[inst] + else: + return [] + + def get_plc_input(self): + return [self.plc_input_buffer.hex().upper()] + + def get_plc_output(self): + return [self.plc_output_buffer.hex().upper()] + + def set_status_bit(self, bit: int, value: bool) -> bool: + if bit < 0 or bit > 31: + return False + if bit in self.reserved_status_bits: + return False + byte_index, bit_index = divmod(bit, 8) + if value: + self.plc_input_buffer[byte_index] |= 1 << bit_index + else: + self.plc_input_buffer[byte_index] &= ~(1 << bit_index) + return True + + def get_status_bit(self, bit: int) -> int | bool: + if bit < 0 or bit > 31: + return False + if bit in self.reserved_status_bits: + return False + byte_index, bit_index = divmod(bit, 8) + return 1 if self.plc_input_buffer[byte_index] & (1 << bit_index) != 0 else 0 + + def toggle_status_bit(self, bit: int) -> bool: + if bit < 0 or bit > 31: + return False + if bit in self.reserved_status_bits: + return False + byte_index, bit_index = divmod(bit, 8) + self.plc_input_buffer[byte_index] ^= 1 << bit_index + return True + + def set_status_error(self, error: str) -> bool: + try: + self.plc_input_buffer[self.error_byte] = int(error, 16) + return True + except ValueError: + return False + + def get_status_error(self) -> str: + return hex(self.plc_input_buffer[self.error_byte]).replace("0x", "").upper() + + def set_status_diagnostics(self, diagnostics: str) -> bool: + try: + self.plc_input_buffer[self.diagnostics_byte] = int(diagnostics, 16) + return True + except ValueError: + return False + + def get_status_diagnostics(self) -> str: + return ( + hex(self.plc_input_buffer[self.diagnostics_byte]).replace("0x", "").upper() + ) + + def get_control_bit(self, bit: int) -> int | 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) + return 1 if self.plc_output_buffer[byte_index] & (1 << bit_index) != 0 else 0 + + def get_target_position(self) -> float: + return struct.unpack("f", self.plc_output_buffer[4:8])[0] + + def get_target_speed(self) -> float: + return struct.unpack("f", self.plc_output_buffer[8:12])[0] + + def set_actual_position(self, position: float) -> None: + self.data[self.actual_position] = [ + bytes(struct.pack("f", position)).hex().upper() + ] + + def set_actual_speed(self, speed: float) -> None: + self.data[self.actual_speed] = [bytes(struct.pack("f", speed)).hex().upper()] + + def get_actual_position(self) -> float: + read_pos = self.data[self.actual_position][0] + return struct.unpack("f", bytes.fromhex(read_pos))[0] + + def get_actual_speed(self) -> float: + read_speed = self.data[self.actual_speed][0] + return struct.unpack("f", bytes.fromhex(read_speed))[0] + + def process_control_bits(self) -> None: + + # Acknowledge + if self.get_control_bit(2) == 1: + self.set_status_bit(bit=0, value=True) + self.set_status_bit(bit=7, value=False) + self.set_status_error("00") + self.set_status_diagnostics("00") + + # Move to absolute position + if self.get_control_bit(13) == 1: + self.toggle_status_bit(5) + 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) diff --git a/schunk_egu_egk_gripper_dummy/tests/__init__.py b/schunk_egu_egk_gripper_dummy/tests/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/schunk_egu_egk_gripper_dummy/tests/test_dummy.py b/schunk_egu_egk_gripper_dummy/tests/test_dummy.py new file mode 100644 index 0000000..b8a610a --- /dev/null +++ b/schunk_egu_egk_gripper_dummy/tests/test_dummy.py @@ -0,0 +1,85 @@ +from src.dummy import Dummy +import pytest +import struct + +# [1]: https://stb.cloud.schunk.com/media/IM0046706.PDF + + +def test_dummy_starts_a_background_thread(): + dummy = Dummy() + assert not dummy.running + dummy.start() + assert dummy.running + dummy.stop() + assert not dummy.running + + +def test_dummy_survives_repeated_starts_and_stops(): + dummy = Dummy() + for _ in range(3): + dummy.start() + assert dummy.running + + for _ in range(3): + dummy.stop() + assert not dummy.running + + +def test_dummy_reads_configuration_on_startup(): + dummy = Dummy() + assert dummy.enum is not None + assert dummy.data is not None + assert dummy.metadata is not None + + +def test_dummy_starts_in_error_state(): + # See p. 24 in + # Booting and establishing operational readiness [1] + dummy = Dummy() + assert dummy.get_status_bit(0) == 0 # not ready for operation + assert dummy.get_status_bit(7) == 1 # there's an error + assert dummy.get_status_error() == "D9" # ERR_FAST_STOP + assert dummy.get_status_diagnostics() == "EF" # ERR_COMM_LOST + + +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) + 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(): + dummy = Dummy() + target_pos = 12.345 + target_speed = 50.3 + + control_double_word = "00200000" + set_position = bytes(struct.pack("f", target_pos)).hex().upper() + set_speed = bytes(struct.pack("f", 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 + dummy.post(command) + + # Done + assert pytest.approx(dummy.get_actual_position()) == target_pos + after = dummy.get_status_bit(bit=5) + assert after != before + assert dummy.get_status_bit(bit=13) == 1 # position reached + assert dummy.get_status_bit(bit=4) == 1 # command successfully processed diff --git a/schunk_egu_egk_gripper_dummy/tests/test_plc_communication.py b/schunk_egu_egk_gripper_dummy/tests/test_plc_communication.py new file mode 100644 index 0000000..310692f --- /dev/null +++ b/schunk_egu_egk_gripper_dummy/tests/test_plc_communication.py @@ -0,0 +1,158 @@ +from src.dummy import Dummy +import struct +import pytest + +# [1]: https://stb.cloud.schunk.com/media/IM0046706.PDF + + +def test_dummy_initializes_plc_data_buffers(): + dummy = Dummy() + assert dummy.data[dummy.plc_input][0] == dummy.plc_input_buffer.hex().upper() + assert dummy.data[dummy.plc_output][0] == dummy.plc_output_buffer.hex().upper() + + +def test_dummy_returns_plc_data(): + dummy = Dummy() + assert dummy.data[dummy.plc_input] == dummy.get_plc_input() + assert dummy.data[dummy.plc_output] == dummy.get_plc_output() + + +def test_dummy_rejects_writing_reserved_status_bits(): + dummy = Dummy() + invalid_bits = [-1, 999] + for bit in invalid_bits + dummy.reserved_status_bits: + assert not dummy.set_status_bit(bit, True) + + +def test_dummy_rejects_reading_reserved_status_bits(): + dummy = Dummy() + for bit in dummy.reserved_status_bits: + assert isinstance(dummy.get_status_bit(bit), bool) # call fails + assert not dummy.get_status_bit(bit) + + +def test_dummy_supports_reading_and_writing_bits_in_plc_status(): + dummy = Dummy() + for bit in dummy.valid_status_bits: + dummy.set_status_bit(bit=bit, value=True) + result = dummy.get_status_bit(bit=bit) + assert isinstance(result, int) # successful calls get the bit's value + assert result == 1 + + +def test_dummy_only_touches_specified_status_bits(): + dummy = Dummy() + before = dummy.get_plc_input() + for bit in dummy.valid_status_bits: + initial_value = dummy.get_status_bit(bit=bit) + dummy.set_status_bit(bit=bit, value=True) + dummy.set_status_bit(bit=bit, value=initial_value) + + assert dummy.get_plc_input() == before + + +def test_dummy_supports_reading_and_writing_status_error(): + dummy = Dummy() + error_codes = ["AA", "bb", "0xcc"] + expected = ["AA", "BB", "CC"] + for error, expected in zip(error_codes, expected): + dummy.set_status_error(error) + assert dummy.get_status_error() == expected + + +def test_dummy_rejects_writing_invalid_status_error(): + dummy = Dummy() + invalid_codes = ["zz", "-1", "aaa"] + for error in invalid_codes: + assert not dummy.set_status_error(error) + + +def test_dummy_supports_reading_and_writing_status_diagnostics(): + dummy = Dummy() + diagnostics_code = "EF" + dummy.set_status_diagnostics(diagnostics_code) + assert dummy.get_status_diagnostics() == diagnostics_code + + +def test_dummy_rejects_writing_invalid_status_diagnostics(): + dummy = Dummy() + invalid_codes = ["zz", "-1", "aaa"] + for code in invalid_codes: + assert not dummy.set_status_diagnostics(code) + + +def test_dummy_supports_reading_bits_in_plc_control(): + dummy = Dummy() + for bit in dummy.valid_control_bits: + result = dummy.get_control_bit(bit=bit) + assert isinstance(result, int) # successful calls get the bit's value + + +def test_dummy_rejects_reading_reserved_control_bits(): + dummy = Dummy() + for bit in dummy.reserved_control_bits: + assert isinstance(dummy.get_control_bit(bit), bool) # call fails + assert not dummy.get_control_bit(bit) + + +def test_dummy_supports_toggling_status_bits(): + dummy = Dummy() + for bit in dummy.valid_status_bits: + before = dummy.get_status_bit(bit) + dummy.toggle_status_bit(bit=bit) + after = dummy.get_status_bit(bit) + assert after != before + dummy.toggle_status_bit(bit=bit) + assert dummy.get_status_bit(bit=bit) == before + + +def test_dummy_rejects_toggling_reserved_status_bits(): + dummy = Dummy() + for bit in dummy.reserved_status_bits: + assert not dummy.toggle_status_bit(bit) + + +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 + + +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 + + +def test_dummy_supports_writing_actual_position(): + dummy = Dummy() + pos = 0.123 + dummy.set_actual_position(pos) + read_pos = dummy.data[dummy.actual_position][0] + read_pos = struct.unpack("f", bytes.fromhex(read_pos))[0] + assert pytest.approx(read_pos) == pos + + +def test_dummy_supports_writing_actual_speed(): + dummy = Dummy() + speed = 66.5 + dummy.set_actual_speed(speed) + read_speed = dummy.data[dummy.actual_speed][0] + read_speed = struct.unpack("f", bytes.fromhex(read_speed))[0] + assert pytest.approx(read_speed) == speed + + +def test_dummy_supports_reading_actual_position(): + dummy = Dummy() + pos = 0.123 + dummy.set_actual_position(pos) + assert pytest.approx(dummy.get_actual_position()) == pos + + +def test_dummy_supports_reading_actual_speed(): + dummy = Dummy() + speed = 66.5 + dummy.set_actual_speed(speed) + assert pytest.approx(dummy.get_actual_speed()) == speed diff --git a/schunk_egu_egk_gripper_dummy/tests/test_requests.py b/schunk_egu_egk_gripper_dummy/tests/test_requests.py new file mode 100644 index 0000000..48667c3 --- /dev/null +++ b/schunk_egu_egk_gripper_dummy/tests/test_requests.py @@ -0,0 +1,100 @@ +from src.dummy import Dummy +from schunk_egu_egk_gripper_dummy.main import server +from fastapi.testclient import TestClient + + +def test_dummy_responds_correctly_to_info_requests(): + dummy = Dummy() + query = "" + expected = {"dataformat": 0} + assert dummy.get_info(query) == expected + + +def test_dummy_responds_correctly_to_enum_requests(): + dummy = Dummy() + inst = "0x0118" + value = 0 + query = {"inst": inst, "value": value} + expected = [dummy.enum[inst][value]] + assert dummy.get_enum(query) == expected + + +def test_dummy_survives_invalid_enum_requests(): + dummy = Dummy() + invalid_inst = "0x0" + query = {"inst": invalid_inst, "value": 0} + expected = [] + assert dummy.get_enum(query) == expected + + +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]] + assert dummy.get_data(query) == expected + + +def test_dummy_responds_correctly_to_data_instance_requests(): + dummy = Dummy() + inst = "0x0040" + query = {"inst": inst, "count": 1} + expected = dummy.data[inst] + assert dummy.get_data(query) == expected + + +def test_dummy_survives_invalid_data_requests(): + dummy = Dummy() + query = {"offset": 1000, "count": "2"} + expected = [] + assert dummy.get_data(query) == expected + query = {"offset": 100, "count": "90"} + expected = [] + assert dummy.get_data(query) == expected + query = {"offset": 1000, "count": "-1"} + expected = [] + assert dummy.get_data(query) == expected + query = {"offset": -1, "count": "1000"} + expected = [] + assert dummy.get_data(query) == expected + query = {"inst": "0x0040", "count": "0"} + expected = [] + assert dummy.get_data(query) == expected + query = {"inst": "0x0040", "count": "2"} + expected = [] + assert dummy.get_data(query) == expected + + +def test_dummy_stores_post_requests(): + dummy = Dummy() + + # Using the plc command variable + msg = "00112233445566778899AABBCCDDEEFF" + data = {"inst": dummy.plc_output, "value": msg} + dummy.post(data) + assert dummy.get_plc_output() == [msg] + + # Using general variables + msg = "AABBCCDD" + inst = "0x0238" + data = {"inst": inst, "value": msg} + dummy.post(data) + assert dummy.data[inst] == [msg] + + +def test_dummy_rejects_invalid_post_requests(): + client = TestClient(server) + + valid_data = "AABBCCDD" + valid_inst = "0x0238" + data = {"inst": valid_inst, "value": valid_data} + assert client.post("/adi/update.json", data=data).json() == {"result": 0} + + invalid_data = "hello:)" + valid_inst = "0x0238" + data = {"inst": valid_inst, "value": invalid_data} + assert client.post("/adi/update.json", data=data).json() == {"result": 1} + + valid_data = "AABBCCDD" + invalid_inst = "0x9999" + data = {"inst": invalid_inst, "value": valid_data} + assert client.post("/adi/update.json", data=data).json() == {"result": 1} diff --git a/schunk_egu_egk_gripper_dummy/tests/test_webserver.py b/schunk_egu_egk_gripper_dummy/tests/test_webserver.py new file mode 100644 index 0000000..91444c1 --- /dev/null +++ b/schunk_egu_egk_gripper_dummy/tests/test_webserver.py @@ -0,0 +1,23 @@ +from schunk_egu_egk_gripper_dummy.main import server +from fastapi.testclient import TestClient + + +def test_info_route_is_available(): + client = TestClient(server) + assert client.get("/adi/info.json").is_success + + +def test_enum_route_is_available(): + client = TestClient(server) + assert client.get("/adi/enum.json").is_success + + +def test_data_route_is_available(): + client = TestClient(server) + assert client.get("/adi/data.json").is_success + + +def test_update_route_is_available(): + client = TestClient(server) + data = {"inst": 0, "value": "0"} + assert client.post("/adi/update.json", data=data).is_success diff --git a/schunk_egu_egk_gripper_examples/src/gripper_example.cpp b/schunk_egu_egk_gripper_examples/src/gripper_example.cpp index 1b31f82..2aea3cc 100644 --- a/schunk_egu_egk_gripper_examples/src/gripper_example.cpp +++ b/schunk_egu_egk_gripper_examples/src/gripper_example.cpp @@ -280,7 +280,7 @@ void acknowledge(rclcpp::Client::SharedPtr acknowledge_client) RCLCPP_INFO(rclcpp::get_logger("schunk_gripper_example"), "Call acknowledge-server."); //To end the error: acknowledge - bool acknowledged = acknowledge_client->async_send_request(acknowledge_srv).get()->acknowledged; + bool acknowledged = acknowledge_client->async_send_request(acknowledge_srv).get()->success; RCLCPP_INFO(rclcpp::get_logger("schunk_gripper_example"), "%s", acknowledged ? "Acknowledged" : "Not acknowledged"); } @@ -548,7 +548,7 @@ int main(int argc, char** argv) if(diagnostic_msg.status[0].level == diagnostic_msgs::msg::DiagnosticStatus::ERROR) { auto response = acknowledge_client->async_send_request(acknowledge_req); - if(response.get()->acknowledged) + if(response.get()->success) RCLCPP_INFO(node->get_logger(), "AN ERROR WAS ACKNOWLEDGED!"); } diff --git a/schunk_egu_egk_gripper_interfaces/srv/Acknowledge.srv b/schunk_egu_egk_gripper_interfaces/srv/Acknowledge.srv index 4c2d7d7..410e0f9 100644 --- a/schunk_egu_egk_gripper_interfaces/srv/Acknowledge.srv +++ b/schunk_egu_egk_gripper_interfaces/srv/Acknowledge.srv @@ -1,2 +1,2 @@ --- -bool acknowledged +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 56e37ec..469d725 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 @@ -171,6 +171,7 @@ class AnybusCom void initAddresses(); std::string ip; //IP to the connected gripper + int port = 80; //TCP/IP Port of the gripper uint8_t endian_format; //Flag for big/little Endian bool not_double_word; //Flag if double word is requested (double words are always big Endian) @@ -243,7 +244,7 @@ class AnybusCom void performCurlRequest(std::string post); - AnybusCom(const std::string &ip); + AnybusCom(const std::string &ip, int port); ~AnybusCom(); }; //Get something with Instance @@ -266,6 +267,7 @@ inline void AnybusCom::getWithInstance(const char inst[7], paramtype *param, con curl_easy_setopt(curl, CURLOPT_HTTPGET, 1); curl_easy_setopt(curl, CURLOPT_WRITEDATA, &response); curl_easy_setopt(curl, CURLOPT_TIMEOUT, 1L); + curl_easy_setopt(curl, CURLOPT_PORT, port); res = curl_easy_perform(curl); if (res != CURLE_OK) @@ -302,6 +304,7 @@ inline void AnybusCom::getWithOffset(const std::string &offset, const size_t & c curl_easy_setopt(curl,CURLOPT_WRITEFUNCTION, writeCallback); curl_easy_setopt(curl, CURLOPT_WRITEDATA, &response); curl_easy_setopt(curl, CURLOPT_TIMEOUT, 1); + curl_easy_setopt(curl, CURLOPT_PORT, port); res = curl_easy_perform(curl); 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 15e6bad..a52b69c 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 @@ -87,7 +87,7 @@ class Gripper : protected AnybusCom public: - Gripper(const std::string &ip); //Gripper initialisation + Gripper(const std::string &ip, int port); //Gripper initialisation ~Gripper(); }; diff --git a/schunk_egu_egk_gripper_library/src/communication.cpp b/schunk_egu_egk_gripper_library/src/communication.cpp index a65bbf4..026906f 100644 --- a/schunk_egu_egk_gripper_library/src/communication.cpp +++ b/schunk_egu_egk_gripper_library/src/communication.cpp @@ -37,12 +37,11 @@ size_t writeCallback(void* contents, size_t size, size_t nmemb, void* userp) } //Initialize the plcs and Addresses -AnybusCom::AnybusCom(const std::string &ip) : ip(ip) +AnybusCom::AnybusCom(const std::string &ip, int port) : ip(ip), port(port) { - initAddresses(); //Init addresses for post and get - + initAddresses(); // for post and get curl = curl_easy_init(); - + curl_easy_setopt(curl, CURLOPT_PORT, port); } //Split a hexadecimal String, which represents an Array into its parts (HERE THE DATATYPE IS ALWAYS 4 Bytes) @@ -173,20 +172,12 @@ void AnybusCom::postParameter(std::string inst, std::string value) //Inits used Addresses with the ip void AnybusCom::initAddresses() { - data_address = "http:///adi/data.json?"; - update_address = "http:///adi/update.json"; - enum_address = "http:///adi/enum.json?"; - info_address = "http:///adi/info.json"; - metadata_address = "http:///adi/metadata.json?"; - if(ip.size() >= 100) ip = "0.0.0.0"; - - data_address.insert(7, ip); - update_address.insert(7, ip); - enum_address.insert(7,ip); - info_address.insert(7,ip); - metadata_address.insert(7,ip); - + data_address = "http://" + ip + ":" + std::to_string(port) + "/adi/data.json?"; + update_address = "http://" + ip + ":" + std::to_string(port) + "/adi/update.json"; + enum_address = "http://" + ip + ":" + std::to_string(port) + "/adi/enum.json?"; + info_address = "http://" + ip + ":" + std::to_string(port) + "/adi/info.json"; + metadata_address = "http://" + ip + ":" + std::to_string(port) + "/adi/metadata.json?"; } //Translates the received string of double_word to an integer[4] an saves it in plc_sync_input @@ -272,6 +263,7 @@ void AnybusCom::getEnums(const char inst[7], const uint16_t &enumNum) curl_easy_setopt(curl, CURLOPT_HTTPGET, 1); curl_easy_setopt(curl, CURLOPT_WRITEDATA, &response); curl_easy_setopt(curl, CURLOPT_TIMEOUT, 1L); + curl_easy_setopt(curl, CURLOPT_PORT, port); res = curl_easy_perform(curl); 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 24ed23c..d3c4234 100644 --- a/schunk_egu_egk_gripper_library/src/schunk_gripper_lib.cpp +++ b/schunk_egu_egk_gripper_library/src/schunk_gripper_lib.cpp @@ -44,8 +44,8 @@ std::map commands_str {"SOFT RESET", SOFT_RESET} }; //Start th gripper, so it is ready to operate -Gripper::Gripper(const std::string &ip): -AnybusCom(ip), +Gripper::Gripper(const std::string &ip, int port): +AnybusCom(ip, port), post_requested(false) { try diff --git a/schunk_egu_egk_gripper_tests/package.xml b/schunk_egu_egk_gripper_tests/package.xml index 98191f3..b04fa99 100644 --- a/schunk_egu_egk_gripper_tests/package.xml +++ b/schunk_egu_egk_gripper_tests/package.xml @@ -12,6 +12,7 @@ ament_lint_auto schunk_egu_egk_gripper_driver + schunk_egu_egk_gripper_dummy launch launch_ros launch_pytest diff --git a/schunk_egu_egk_gripper_tests/test/conftest.py b/schunk_egu_egk_gripper_tests/test/conftest.py index 0b0f1c0..cd37991 100644 --- a/schunk_egu_egk_gripper_tests/test/conftest.py +++ b/schunk_egu_egk_gripper_tests/test/conftest.py @@ -1,5 +1,13 @@ import pytest import rclpy +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +import launch_pytest +import subprocess +from ament_index_python.packages import get_package_share_directory + # We avoid black's F811, F401 linting warnings # by using pytest's special conftest.py file. @@ -12,3 +20,36 @@ def isolated(): rclpy.init() yield rclpy.shutdown() + + +@pytest.fixture() +def gripper_dummy(): + package_name = "schunk_egu_egk_gripper_dummy" + dummy_dir = get_package_share_directory(package_name) + start_cmd = " uvicorn main:server --port 8000" + p = subprocess.Popen( + "exec" + start_cmd, stdin=subprocess.PIPE, cwd=dummy_dir, shell=True + ) + + print("Started gripper dummy") + yield + p.kill() + print("Stopped gripper dummy") + + +@launch_pytest.fixture +def launch_description(): + setup = IncludeLaunchDescription( + PathJoinSubstitution( + [ + FindPackageShare("schunk_egu_egk_gripper_driver"), + "launch", + "schunk.launch.py", + ] + ), + launch_arguments={ + "IP": "127.0.0.1", + "port": "8000", + }.items(), + ) + return LaunchDescription([setup, launch_pytest.actions.ReadyToTest()]) diff --git a/schunk_egu_egk_gripper_tests/test/helpers.py b/schunk_egu_egk_gripper_tests/test/helpers.py new file mode 100644 index 0000000..966c404 --- /dev/null +++ b/schunk_egu_egk_gripper_tests/test/helpers.py @@ -0,0 +1,27 @@ +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 + + +class TopicGetsPublished(Node): + def __init__(self, topic: str, type: Any): + node_name = "check_topic" + str(uuid.uuid4()).replace("-", "") + super().__init__(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.start() + + def msg_cb(self, data: Any) -> None: + self.data = data + self.event.set() + + +def get_current_state(variable: str): + topic = TopicGetsPublished("/state", State) + topic.event.wait() + return getattr(topic.data, variable) diff --git a/schunk_egu_egk_gripper_tests/test/test_functionality.py b/schunk_egu_egk_gripper_tests/test/test_functionality.py new file mode 100644 index 0000000..e0b9d5f --- /dev/null +++ b/schunk_egu_egk_gripper_tests/test/test_functionality.py @@ -0,0 +1,46 @@ +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 new file mode 100644 index 0000000..7bf76ca --- /dev/null +++ b/schunk_egu_egk_gripper_tests/test/test_http_interface.py @@ -0,0 +1,14 @@ +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 e8c3ae5..3b85f46 100644 --- a/schunk_egu_egk_gripper_tests/test/test_launch.py +++ b/schunk_egu_egk_gripper_tests/test/test_launch.py @@ -1,31 +1,12 @@ #!/usr/bin/env python3 import pytest -from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription -from launch.substitutions import PathJoinSubstitution -from launch_ros.substitutions import FindPackageShare -import launch_pytest - from rclpy.node import Node import time - - -@launch_pytest.fixture -def launch_description(): - setup = IncludeLaunchDescription( - PathJoinSubstitution( - [ - FindPackageShare("schunk_egu_egk_gripper_driver"), - "launch", - "schunk.launch.py", - ] - ) - ) - return LaunchDescription([setup, launch_pytest.actions.ReadyToTest()]) +from test.conftest import launch_description @pytest.mark.launch(fixture=launch_description) -def test_normal_startup_works(launch_context, isolated): +def test_normal_startup_works(launch_context, isolated, gripper_dummy): 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 new file mode 100644 index 0000000..48651fe --- /dev/null +++ b/schunk_egu_egk_gripper_tests/test/test_ros_interfaces.py @@ -0,0 +1,59 @@ +#!/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")