def call_read_DI_command(self):
try:
client = self.call_read_di
val = client.call(self.call_read_di_req).digital_input
self.process_DI_btn(val)
except rospy.ServiceException, e:
resp = ElfinIODReadResponse()
resp.digital_input = 0x0000
def call_set_bool_common(self, event, client, request):
try:
resp = client.call(request)
wx.CallAfter(self.update_reply_show, resp)
except rospy.ServiceException, e:
resp = SetBoolResponse()
resp.success = False
resp.message = 'no such service in simulation'
wx.CallAfter(self.update_reply_show, resp)
def thread_bg(self, client, request):
wx.CallAfter(self.show_dialog)
self.action_stop()
rospy.sleep(1)
try:
resp = client.call(request)
wx.CallAfter(self.update_reply_show, resp)
except rospy.ServiceException, e:
resp = SetBoolResponse()
resp.success = False
resp.message = 'no such service in simulation'
wx.CallAfter(self.update_reply_show, resp)
def call_set_bool_common(self, event, client, request):
btn = event.GetEventObject()
check_list = ['Servo On', 'Servo Off', 'Clear Fault']
if btn.GetName() in check_list:
self.show_message_dialog(btn.GetName(), client, request)
else:
try:
resp = client.call(request)
wx.CallAfter(self.update_reply_show, resp)
except rospy.ServiceException, e:
resp = SetBoolResponse()
resp.success = False
resp.message = 'no such service in simulation'
wx.CallAfter(self.update_reply_show, resp)
def call_write_DO_command(self, event, marker, client):
self.justification_DO_btn(marker)
request = 0
try:
self.DO_btn_lock.acquire()
for i in range(0, 8):
request = request + self.DO_btn[i] * pow(2, i)
resp = client.call(request << 12)
self.DO_btn_lock.release()
except rospy.ServiceException, e:
self.DO_btn_lock.release()
resp = ElfinIODWriteResponse()
resp.success = False
self.justification_DO_btn(marker)
rp = SetBoolResponse()
rp.success = False
rp.message = 'no such service for DO control'
wx.CallAfter(self.update_reply_show, rp)
def call_set_bool_common(self, event, client, request):
btn = event.GetEventObject()
check_list = ['Servo On', 'Servo Off', 'Clear Fault']
# Check servo state
if btn.GetName() == 'Servo On':
servo_enabled = bool()
if self.servo_state_lock.acquire():
servo_enabled = self.servo_state
self.servo_state_lock.release()
if servo_enabled:
resp = SetBoolResponse()
resp.success = False
resp.message = 'Robot is already enabled'
wx.CallAfter(self.update_reply_show, resp)
event.Skip()
return
# Check fault state
if btn.GetName() == 'Clear Fault':
fault_flag = bool()
if self.fault_state_lock.acquire():
fault_flag = self.fault_state
self.fault_state_lock.release()
if not fault_flag:
resp = SetBoolResponse()
resp.success = False
resp.message = 'There is no fault now'
wx.CallAfter(self.update_reply_show, resp)
event.Skip()
return
# Check if the button is in check list
if btn.GetName() in check_list:
self.show_message_dialog(btn.GetName(), client, request)
else:
try:
resp = client.call(request)
wx.CallAfter(self.update_reply_show, resp)
except rospy.ServiceException, e:
resp = SetBoolResponse()
resp.success = False
resp.message = 'no such service in simulation'
wx.CallAfter(self.update_reply_show, resp)