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)