79 lines
2.3 KiB
Python
79 lines
2.3 KiB
Python
import sys
|
|
import termios
|
|
import tty
|
|
|
|
from clients._path_setup import ensure_paths
|
|
|
|
ensure_paths()
|
|
|
|
from google.protobuf import timestamp_pb2
|
|
|
|
from clients.base_client import RobotClientBase
|
|
from cmvr.api import common_pb2
|
|
from cmvr.api import arm_command_pb2 as pb
|
|
from cmvr.api import arm_service_pb2_grpc as rpc
|
|
|
|
|
|
class TorqueClient(RobotClientBase):
|
|
"""Client to control torque: on/off"""
|
|
|
|
def torque_on(self, device_id="right_arm"):
|
|
req = common_pb2.CommandHeader.Request()
|
|
req.device_id = device_id
|
|
ts = timestamp_pb2.Timestamp()
|
|
ts.GetCurrentTime()
|
|
req.timestamp.CopyFrom(ts)
|
|
try:
|
|
resp = self.stub.torqueOn(req, timeout=self.timeout)
|
|
print("TorqueOn RPC call succeeded")
|
|
print(f"Success: {resp.success}")
|
|
print(f"Error message: {resp.error_message}")
|
|
print(f"Timestamp: {resp.timestamp.seconds}")
|
|
except Exception as e:
|
|
print("TorqueOn RPC call failed:", e)
|
|
|
|
def torque_off(self, device_id="right_arm"):
|
|
req = common_pb2.CommandHeader.Request()
|
|
req.device_id = device_id
|
|
ts = timestamp_pb2.Timestamp()
|
|
ts.GetCurrentTime()
|
|
req.timestamp.CopyFrom(ts)
|
|
try:
|
|
resp = self.stub.torqueOff(req, timeout=self.timeout)
|
|
print("TorqueOff RPC call succeeded")
|
|
print(f"Success: {resp.success}")
|
|
print(f"Error message: {resp.error_message}")
|
|
print(f"Timestamp: {resp.timestamp.seconds}")
|
|
except Exception as e:
|
|
print("TorqueOff RPC call failed:", e)
|
|
|
|
|
|
def get_single_key():
|
|
"""Read a single key from stdin without Enter"""
|
|
fd = sys.stdin.fileno()
|
|
old_settings = termios.tcgetattr(fd)
|
|
try:
|
|
tty.setraw(fd)
|
|
ch = sys.stdin.read(1)
|
|
finally:
|
|
termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
|
|
return ch
|
|
|
|
|
|
if __name__ == "__main__":
|
|
client = TorqueClient()
|
|
print("Press 'o' for TorqueOn, 'p' for TorqueOff, 'q' to quit.")
|
|
|
|
try:
|
|
while True:
|
|
key = get_single_key()
|
|
if key.lower() == 'o':
|
|
client.torque_on()
|
|
elif key.lower() == 'p':
|
|
client.torque_off()
|
|
elif key.lower() == 'q':
|
|
print("Exiting.")
|
|
break
|
|
finally:
|
|
client.close()
|