class Robot:
def __init__(self):
self.name = None
self.__protocol = Protocol()
self.__shutdown = False
self.__initialized = False
self.__configured = False
self.__waiting_for = None
def configure(self, name, port, baudrate):
self.name = name
try:
config_file = 'config/'+self.name+'.yaml'
with open(config_file) as f:
conf = yaml.safe_load(f)
f.close()
self.__port = port
self.__baudrate = baudrate
self.__num_joints = len(conf['Joints'])
except:
print("[Error] Configuration failed. \n ***** Please make sure " + self.name + ".yaml exists.")
return False
self.__configured = True
return True
def initialize(self):
if not self.__configured:
print('[Error] Robot has not been configured.')
return False
try:
self.__transport = Transport(self.__port, self.__baudrate)
except:
print("[Error] Serial initialization failed. \n ***** Check serial settings.")
return False
self.__proc = threading.Thread(name='robot process', target=self.__run)
self.__proc.setDaemon(True)
self.robot_status = RobotStatus(self.__num_joints)
self.__initialized = True
return True
def start(self):
if not self.__initialized:
print("Error: Robot has not been initialized.")
return
self.__proc.start()
def shutdown(self):
self.__shutdown = True # set shutdown flag
def get_joint_angles(self):
self.__set_wait(MsgId.RET_JOINT_POSITIONS)
self.__transport.write(self.__protocol.encode(MsgId.GET_JOINT_POSITIONS, Empty()))
if self.__wait():
return self.robot_status.current_joint_positions.angles
else:
return False
def set_joint_angles(self, angles):
print("setting joint angles at ", angles)
msg = JointPos().resize(self.__num_joints)
for i in range(self.__num_joints):
msg.angles[i] = angles[i]
self.__transport.write(self.__protocol.encode(MsgId.SET_JOINT_POSITIONS, msg))
def __run(self):
while not self.__shutdown:
c = self.__transport.read()
# print(c)
if self.__protocol.parse(c):
self.__process_message(self.__protocol.get_message())
def __process_message(self, msg_id):
if msg_id == MsgId.MOVE_DONE:
pass
elif msg_id == MsgId.RET_JOINT_POSITIONS:
self.__protocol.decode(msg_id, self.robot_status.current_joint_positions)
if self.__waiting_for == MsgId.RET_JOINT_POSITIONS:
self.__waiting_for = None
def __set_wait(self, msg_id):
self.__waiting_for = msg_id
def __wait(self, timeout=0.5):
_timeout = now()+timeout
while now() < _timeout:
if self.__waiting_for is None:
return True
return False
class Robot:
def __init__(self):
self.name = None
self.__protocol = Protocol()
self.__shutdown = False
self.__initialized = False
self.__configured = False
self.__waiting_for = None
def configure(self, name, port, baudrate):
self.name = name
home_path = os.getenv('HOME')
config_file = home_path+'/openarm_ros/ws_moveit/src/bringups/victor5_bringup/scripts/config/'+self.name+'.yaml'
with open(config_file) as f:
conf = yaml.safe_load(f)
f.close()
self.__port = port
self.__baudrate = baudrate
self.__num_joints = len(conf['Joints'])
self.__configured = True
def initialize(self):
if not self.__configured:
raise RuntimeError('robot has not been configured.')
try:
self.__transport = Transport(self.__port, self.__baudrate)
except:
print("serial initialization failed")
raise RuntimeError("Robot init failed. Check configuration.")
self.__proc = threading.Thread(name='robot process', target=self.__run)
self.__proc.setDaemon(True)
self.robot_status = RobotStatus(self.__num_joints)
self.__initialized = True
def start(self):
if not self.__initialized:
raise RuntimeError('robot has not been initialized.')
self.__proc.start()
def shutdown(self):
self.__shutdown = True # set shutdown flag
def get_joint_angles(self):
self.__set_wait(MsgId.RET_JOINT_POSITIONS)
self.__transport.write(self.__protocol.encode(MsgId.GET_JOINT_POSITIONS, Empty()))
if self.__wait():
return self.robot_status.current_joint_positions.angles
else:
return False
def set_joint_angles(self, angles):
print("setting joint angles at ", angles)
msg = JointPos().resize(self.__num_joints)
for i in range(self.__num_joints):
msg.angles[i] = angles[i]
self.__transport.write(self.__protocol.encode(MsgId.SET_JOINT_POSITIONS, msg))
def __run(self):
while not self.__shutdown:
c = self.__transport.read()
# print(c)
if self.__protocol.parse(c):
self.__process_message(self.__protocol.get_message())
def __process_message(self, msg_id):
if msg_id == MsgId.MOVE_DONE:
pass
elif msg_id == MsgId.RET_JOINT_POSITIONS:
self.__protocol.decode(msg_id, self.robot_status.current_joint_positions)
if self.__waiting_for == MsgId.RET_JOINT_POSITIONS:
self.__waiting_for = None
def __set_wait(self, msg_id):
self.__waiting_for = msg_id
def __wait(self, timeout=0.5):
_timeout = now()+timeout
while now() < _timeout:
if self.__waiting_for is None:
return True
return False
class Robot:
def __init__(self):
self.name = None
self.__protocol = Protocol()
self.__shutdown = False
self.__initialized = False
self.__configured = False
self.__waiting_for = None
self.__config = None
def configure(self, name, port, baudrate):
self.name = name
pkg_path = os.popen('rospack find robot_arm').read()[:-1]
try:
config_file = pkg_path + '/scripts/config/' + self.name + '.yaml'
with open(config_file) as f:
conf = yaml.safe_load(f)
f.close()
self.__config = conf
self.__port = port
self.__baudrate = baudrate
self.__num_joints = len(conf['Joints'])
except:
print("Error occurred during configuration. \n *** Make sure " +
config_file + " exists.")
return False
self.__configured = True
return True
def initialize(self):
if not self.__configured:
print('robot has not been configured.')
return False
try:
self.__transport = Transport(self.__port, self.__baudrate)
except:
print(
"Serial initialization failed. \n *** Check serial settings.")
return False
self.__proc = threading.Thread(name='robot process', target=self.__run)
self.__proc.setDaemon(True)
self.robot_status = RobotStatus(self.__num_joints)
self.__initialized = True
return True
def start(self):
if not self.__initialized:
print("Error: Robot has not been initialized.")
return False
self.__proc.start()
return True
def shutdown(self):
self.__shutdown = True # set shutdown flag
def get_joint_angles(self):
self.__set_wait(MsgId.RET_JOINT_POSITIONS)
self.__transport.write(
self.__protocol.encode(MsgId.GET_JOINT_POSITIONS, Empty()))
if self.__wait():
return self.robot_status.current_joint_positions.angles
else:
return False
def set_joint_angles(self, angles):
print("setting joint angles at ", angles, "\r\n")
msg = JointPos().resize(self.__num_joints)
for i in range(self.__num_joints):
msg.angles[i] = angles[i]
self.__transport.write(
self.__protocol.encode(MsgId.SET_JOINT_POSITIONS, msg))
def set_end_effector_state(self, state):
if state == EndEffectorStateList.IDLE:
return
elif state == EndEffectorStateList.GRIP:
print("End_effector ----> GRIP \r\n")
elif state == EndEffectorStateList.RELEASE:
print("End_effector ----> RELEASE \r\n")
msg = EndEffectorState()
msg.state = state
self.__transport.write(
self.__protocol.encode(MsgId.SET_END_EFFECTOR_STATE, msg))
def robot_config(self):
return self.__config
def __run(self):
while not self.__shutdown:
c = self.__transport.read()
# print(c)
if self.__protocol.parse(c):
self.__process_message(self.__protocol.get_message())
def __process_message(self, msg_id):
if msg_id == MsgId.MOVE_DONE:
pass
elif msg_id == MsgId.RET_JOINT_POSITIONS:
self.__protocol.decode(msg_id,
self.robot_status.current_joint_positions)
if self.__waiting_for == MsgId.RET_JOINT_POSITIONS:
self.__waiting_for = None
def __set_wait(self, msg_id):
self.__waiting_for = msg_id
def __wait(self, timeout=0.5):
_timeout = now() + timeout
while now() < _timeout:
if self.__waiting_for is None:
return True
return False
