def __init__(self, current_state=-1, opcode=-1, enable_algorithm_node=False, current_command=Button.NONE):
self.edo_current_state = current_state
self._edo_opcode_previous = opcode
self._edo_current_state_previous = current_state
self.edo_opcode = opcode
self._edo_jog_speed = 0.5
self.gripper_position = 60
self.send_first_step_bool = False # select 6-axis configuration
self.send_second_step_bool = False # disconnect the brakes
self.send_third_step_bool = False # calibration process will start
self.current_joint = 0
self.current_joint_states = None
self._sent_next_movement_command_bool = False
self.reselect_joints_bool = False
self.disengage_brakes_timer = None
# self.disengage_brakes_bool = False
self.disengage_brakes_bool = True
self.read_input_bool = False
self.msg_jca = JointControlArray()
self.msg_mc = MovementCommand()
# Joint Command topics
self.jog_command_pub = None
self.joint_control_pub = None
rospy.Subscriber("/machine_state", MachineState, self.callback)
rospy.Subscriber("usb_jnt_state", JointStateArray, self.js_callback)
rospy.Subscriber("open_gripper", Bool, self.callback_gripper)
self._joint_init_command_pub = rospy.Publisher('/bridge_init', JointInit, queue_size=10, latch=True)
self._joint_reset_command_pub = rospy.Publisher('/bridge_jnt_reset', JointReset, queue_size=10, latch=True)
self.jog_command_pub = rospy.Publisher('/bridge_jog', MovementCommand, queue_size=10, latch=True)
self.joint_control_pub = rospy.Publisher('/algo_jnt_ctrl', JointControlArray, queue_size=1)
self.movement_command_pub = rospy.Publisher('/bridge_move', MovementCommand, queue_size=10, latch=True)
self._joint_calibration_command_pub = rospy.Publisher('/bridge_jnt_calib', JointCalibration, queue_size=10,
latch=True)
self._emergency_stop_pub = rospy.Publisher('/emergency_stop', Bool, queue_size=10, latch=True)
self._emergency_stop_pub.publish(Bool(False))
rospy.Subscriber('/machine_movement_ack', MovementFeedback, self.move_ack_callback)
# collision disabled to prevent robot from falling during movement
# TODO alternative solution possible ?
self.disable_collision()
self.switch_algo_service(enable_algorithm_node)
self.current_command = current_command
float(parsed[1]),
float(parsed[2]),
float(parsed[3]),
float(parsed[4]),
float(parsed[5]),
float(parsed[6])
]
else :
joints = [
float(parsed[0]),
float(parsed[1]),
float(parsed[2]),
float(parsed[3]),
float(parsed[4]),
float(parsed[5])
]
#print(joints)
ctrlMsg = JointControlArray()
ctrlMsg.size = len(joints)
ctrlMsg.joints = [JointControl(i, 0, 0, 0, 0, 0) for i in joints]
algo_jnt_ctrl.publish(ctrlMsg)
rate.sleep()
s.send("<CONNECTION_CLOSE>")
s.close()
control_switch(2)
waitme = 0
print("exit slave mode")
while not rospy.is_shutdown() and startc5g == 1:
s.send("<GET_ARM_JNT;1>")
# print(" inviato: <GET_ARM_JNT;1>")
data = s.recv(1024)
# print("ricevuto: " + data)
parsed = parse("<GET_ARM_JNT;1;{};{};{};{};{};{};{};{};{};{}>", data).fixed
# print(parsed)
# print(parse("<GET_ARM_JNT;{};{};{};{};{};{};{};{};{};{}>", data).fixed)
joints = [
float(parsed[0]),
float(parsed[1]),
float(parsed[2]),
float(parsed[3]),
float(parsed[4]),
float(parsed[5])
]
#joints = [1, 2, 3, 4, 5, 6]
#print(joints)
ctrlMsg = JointControlArray()
ctrlMsg.size = len(joints)
ctrlMsg.joints = [JointControl(i, 0, 0, 0, 0) for i in joints]
algo_jnt_ctrl.publish(ctrlMsg)
rate.sleep()
s.send("<CONNECTION_CLOSE>")
s.close()
control_switch(2)
waitme = 0
print("exit slave mode")