"""Explicit read-only position queries. Default invocation does not open CAN.""" import argparse import json import time from pathlib import Path from a7_can_sdk import MotorBus from a7_can_sdk.configuration import load from a7_can_sdk.transport import SocketCan def main(): parser = argparse.ArgumentParser(description=__doc__) parser.add_argument('--config', type=Path) parser.add_argument('--execute', action='store_true', help='Open CAN and send read-only queries') parser.add_argument('--samples', type=int, default=10) args = parser.parse_args() if not args.execute: print('Plan only: identify configured motors and query positions; no hardware connection.') return if args.config is None or args.samples < 1: parser.error('--execute requires --config and --samples >= 1') cfg = load(args.config) bus = MotorBus(SocketCan(cfg['interface'], cfg['bitrate']), cfg['motors'], allow_control=False) try: bus.start() for motor in cfg['motors']: if bus.identify(motor['motor_id']) != motor['uid']: raise RuntimeError('Motor identity mismatch') for _ in range(args.samples): rows = [{'motor_id': m['motor_id'], 'position_rad': bus.read_parameter(m['motor_id'], 'position')} for m in cfg['motors']] print(json.dumps({'host_time_ns': time.time_ns(), 'joints': rows})) time.sleep(.1) finally: bus.close() if __name__ == '__main__': main()