Added ping tool

Author: vytasrgl

README.md

@@ -1,7 +1,7 @@
 ## REQUIREMENT
  - Python 3.8
  - Serial port access, for Ubuntu current user needs to be in dialout group
- - Feetech SCS or STS servos. 
+ - Feetech SCS or STS servos.
 ## INSTALLATION
 For Ubuntu:
 
@@ -15,9 +15,34 @@ pip install -r requirements.txt
 
 ## Tools
 
+### Ping
+Pings all available servos on multiple boards connected
+```bash
+./ping.py --help
+```
+```
+  -h, --help            show this help message and exit
+  --hardware_regex HARDWARE_REGEX
+                        Serial port filter for detecting multiple boards (default: 1A86:7523)
+  --baud BAUD           Baudrate (default: 1000000)
+  --from_id FROM_ID     From ID (default: 0)
+  --to_id TO_ID         To ID (default: 110)
+  --json JSON           Output as JSON (default: False)
+```
+Examples:
+```
+ ./ping.py --from_id 80 --to_id 90
+```
+Output:
+```
+{'/dev/ttyUSB0': [(83, 6662)]}
+```
+Where device port is `/dev/ttyUSB0` and `83` is servo ID and `6662` internal servo model.
+
+
 ### Meassure
 Measures the time it takes for servo to move from one position to another
-Useage:
+Usage:
 ```bash
 ./meassure.py --help
 ```
@@ -43,4 +68,4 @@ Examples:
 ```bash
 ./meassure.py --id 1 --protocol 0 --from 300 --to 910 --goback true --percieved 5
 
-```
+```

ping.py

@@ -0,0 +1,74 @@
+#!/usr/bin/env python
+#
+# Copyright (c) 2022 Hanson Robotics.
+#
+# This file is part of Hanson AI.
+# See https://www.hansonrobotics.com/hanson-ai for further info.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+#
+
+import argparse
+import json
+from pprint import pprint as pp
+from serial.tools.list_ports import grep as grep_serial_ports
+import scservo_sdk as sdk
+
+
+def argument_parser():
+    parser = argparse.ArgumentParser(description="Ping all feetech servos", formatter_class=argparse.ArgumentDefaultsHelpFormatter)
+    # By default we search for all CH430 ports with
+    parser.add_argument("--hardware_regex", type=str, default='1A86:7523', help='Serial port filter for detecting multiple boards')
+    parser.add_argument("--baud", type=int, default=1000000, help='Baudrate')
+    parser.add_argument("--from_id", type=int, default=0, help='From ID')
+    parser.add_argument("--to_id", type=int, default=110, help='To ID')
+    parser.add_argument("--json", type=bool, default=False, help="Output as JSON")
+    args = parser.parse_args()
+    return vars(args)
+
+def pingservos(port, baud, from_id, to_id):
+    port = sdk.PortHandler(port)
+    # Ping does not depend on the protocol version so can use 0 by default
+    handler = sdk.PacketHandler(0)
+    servos = []
+    try:
+        port.openPort()
+        port.setBaudRate(baud)
+        for id in range(from_id, to_id+1):
+            # Ping the servo
+            model_number, result, error = handler.ping(port, id)
+            if result == sdk.COMM_SUCCESS:
+                servos.append((id, model_number))
+        return servos
+    except Exception as e:
+        print(e)
+        return None
+
+def find_all_servos(args):
+    ports = [p.device for p in grep_serial_ports(args['hardware_regex'])]
+    devices = {}
+    for p in ports:
+        devices[p] = pingservos(p, args['baud'], args['from_id'], args['to_id'])
+    return devices
+
+
+def main():
+    args = argument_parser()
+    device_servos = find_all_servos(args)
+    if args['json']:
+        print(json.dumps(device_servos, indent=4, sort_keys=True))
+    else:
+        pp(device_servos)
+
+if __name__ == '__main__':
+    main()