def execute(self,userdata): if userdata.semi_autonomous_mode == True: # user intervention is possible while not (rospy.is_shutdown() or self.preempt_requested()) : global current_task_info _feedback=xmsg.ExecutionFeedback() _feedback.current_state = "need user intervention for base pose" _feedback.solution_required = True _feedback.exceptional_case_id = 1 current_task_info._srs_as._as.publish_feedback(_feedback) rospy.sleep(1) self.count += 1 rospy.loginfo("State : need user intervention for base pose") rospy.wait_for_service('message_errors') s = xsrv.errorsResponse() current_state= 'need user intervention for base pose' exceptional_case_id=1 # need user intervention for base pose try: message_errors = rospy.ServiceProxy('message_errors',xsrv.errors) s = message_errors(current_state, exceptional_case_id) print s.solution except rospy.ServiceException, e: print "Service call failed: %s"%e self.pub_fb.publish(False) return 'failed' print (s) if (s.giveup == 1): return 'no_more_retry' else: """import json tmppos = s.solution.__str__()#"home"#[1.0, 3.0, 0.0]" # (temp) -- commented by ze, simpler to use split(sep) directly #tmppos = tmppos.replace('[','') #tmppos = tmppos.replace(']','') #tmppos = tmppos.replace(',',' ') #tmppos = tmppos.replace('#','') #listtmp = tmppos.split() listtmp = tmppos.split('[]#, ') #end of comment# list_out = list() list_out.insert(0, float(listtmp[0])) list_out.insert(1, float(listtmp[1])) list_out.insert(2, float(listtmp[2])) userdata.intermediate_pose = list_out """ userdata.intermediate_pose = eval(s.solution.__str__()) #rospy.loginfo("New intermediate target is :%s", list_out) return 'retry' return 'failed'
def handle_message_errors(self,req): print "Returning [%s]"%(req.current_state) print req.exceptional_case_id rospy.loginfo("The user will be invited to enter a new position to solve problem :") self.solfromUser = xsrv.errorsResponse() satisfaction_flag = False self.time_out_max = 30 try: self.time_out_max = rospy.get_param("srs/common/max_time_out") except Exception, e: rospy.loginfo("Parameter Server not ready, use default value for max time_out")
def execute(self, userdata): if userdata.semi_autonomous_mode == True: # user intervention is possible while not (rospy.is_shutdown() or self.preempt_requested()): global current_task_info _feedback = xmsg.ExecutionFeedback() _feedback.current_state = "need user intervention for base pose" _feedback.solution_required = True _feedback.exceptional_case_id = 1 current_task_info._srs_as._as.publish_feedback(_feedback) rospy.sleep(1) self.count += 1 rospy.loginfo("State : need user intervention for base pose") rospy.wait_for_service('message_errors') s = xsrv.errorsResponse() current_state = 'need user intervention for base pose' exceptional_case_id = 1 # need user intervention for base pose try: message_errors = rospy.ServiceProxy( 'message_errors', xsrv.errors) s = message_errors(current_state, exceptional_case_id) print s.solution except rospy.ServiceException, e: print "Service call failed: %s" % e self.pub_fb.publish(False) return 'failed' print(s) if (s.giveup == 1): return 'no_more_retry' else: """ tmppos = s.solution.__str__()#"home"#[1.0, 3.0, 0.0]" tmppos = tmppos.replace('[','') tmppos = tmppos.replace(']','') tmppos = tmppos.replace(',',' ') tmppos = tmppos.replace('#','') listtmp = tmppos.split() list_out = list() list_out.insert(0, float(listtmp[0])) list_out.insert(1, float(listtmp[1])) list_out.insert(2, float(listtmp[2])) userdata.intermediate_pose = list_out """ userdata.intermediate_pose = eval(s.solution.__str__()) #rospy.loginfo("New intermediate target is :%s", list_out) return 'retry' return 'failed'
def handle_message_errors(self, req): print "Returning [%s]" % (req.current_state) print req.exceptional_case_id rospy.loginfo( "The user will be invited to enter a new position to solve problem :" ) self.solfromUser = xsrv.errorsResponse() satisfaction_flag = False self.time_out_max = 30 try: self.time_out_max = rospy.get_param("srs/common/max_time_out") except Exception, e: rospy.loginfo( "Parameter Server not ready, use default value for max time_out" )
class UI_PRI_TOPICS: #### Initialization and declaration of the variables #### def __init__(self): rospy.loginfo("Public topics for UI_PRI ...") self.pubUIerr = rospy.Publisher('DM_UI/interface_cmd', srs_msg.DM_UIcom) self.user_respsol = "" self.user_resppar = "" self.user_com_id = 0 rospy.Subscriber("DM_UI/interface_cmd_response", srs_msg.UI_DMresp, self.callbackUI) ### Declaration of callback function for the commands from the user interface def callbackUI(self, data): rospy.loginfo("I heard %s %s %i from the UI_PRI", data.solution, data.parameter, data.responseID) if (data.responseID == self.user_com_id): self.user_respsol = data.solution self.user_resppar = data.parameter rospy.loginfo( "Match between responseId and requestID. Now user_respsol is:%s and user_resppar is:%s", self.user_respsol, self.user_resppar) else: print("but the id:%s does not correspond to requestId:%s", self.user_com_id) def handle_message_errors(self, req): print "Returning [%s]" % (req.current_state) print req.exceptional_case_id rospy.loginfo( "The user will be invited to enter a new position to solve problem :" ) self.solfromUser = xsrv.errorsResponse() satisfaction_flag = False self.time_out_max = 30 try: self.time_out_max = rospy.get_param("srs/common/max_time_out") except Exception, e: rospy.loginfo( "Parameter Server not ready, use default value for max time_out" ) while (not satisfaction_flag): rospy.loginfo( "Message sent to user to ask if he wants to continue:") self.user_respsol = "" message1 = srs_msg.DM_UIcom( "continue?", "There is an obstacle on the robot's path. Do you want to try to move the robot manually? ", 5) self.user_com_id = 5 self.pubUIerr.publish(message1) timeout = 0 while (self.user_respsol == "" and timeout < self.time_out_max): print "waiting for response", timeout, " seconds from the user" timeout = timeout + 1 print "user_respsol is :", self.user_respsol rospy.sleep(1) print "user_respsol is.:", self.user_respsol if (self.user_respsol.strip() == "continue"): rospy.loginfo( "Just received continue from the user and will ask for a new position to solve the problem :" ) self.user_respsol = "" self.user_resppar = "" message1 = srs_msg.DM_UIcom( "position?", "Plese specify a new intermedeate position for the robot to try reaching the target from there", 6) rospy.loginfo( "Just sent a request to the user to give me a new position to solve the problem :" ) self.user_com_id = 6 self.pubUIerr.publish(message1) #t2= raw_input() timeout = 0 while (self.user_respsol == "" and timeout < 3 * self.time_out_max): print "waiting for response for", timeout, " seconds from the user" print "user_respsol is :", self.user_respsol timeout = timeout + 1 rospy.sleep(1) if (self.user_respsol.strip() == "move"): satisfaction_flag = True self.solfromUser.giveup = 0 self.solfromUser.solution = self.user_resppar.strip() print "We got a move response from the user" print(self.solfromUser) else: satisfaction_flag = False rospy.loginfo( "The user response doesn't make sense or timed out") self.solfromUser.giveup = 1 self.solfromUser.solution = "" else: rospy.loginfo( "The response from the user is: <<Do not to continue>>. Giving up" ) satisfaction_flag = True self.solfromUser.giveup = 1 self.solfromUser.solution = "" print(self.solfromUser) return xsrv.errorsResponse(self.solfromUser.solution, self.solfromUser.giveup)
def execute(self, userdata): if userdata.semi_autonomous_mode == True: # user intervention is possible while not (rospy.is_shutdown() or self.preempt_requested()): global current_task_info _feedback = xmsg.ExecutionFeedback() _feedback.current_state = "need user intervention for base pose" _feedback.solution_required = True _feedback.exceptional_case_id = 1 current_task_info._srs_as._as.publish_feedback(_feedback) rospy.sleep(1) self.count += 1 rospy.loginfo("State : need user intervention for base pose") rospy.wait_for_service('message_errors') s = xsrv.errorsResponse() current_state = 'need user intervention for base pose' exceptional_case_id = 1 # need user intervention for base pose try: message_errors = rospy.ServiceProxy( 'message_errors', xsrv.errors) s = message_errors(current_state, exceptional_case_id) print s.solution except rospy.ServiceException, e: print "Service call failed: %s" % e self.pub_fb.publish(False) return 'failed' print(s) if (s.giveup == 1): return 'no_more_retry' else: """import json tmppos = s.solution.__str__()#"home"#[1.0, 3.0, 0.0]" # (temp) -- commented by ze, simpler to use split(sep) directly #tmppos = tmppos.replace('[','') #tmppos = tmppos.replace(']','') #tmppos = tmppos.replace(',',' ') #tmppos = tmppos.replace('#','') #listtmp = tmppos.split() listtmp = tmppos.split('[]#, ') #end of comment# list_out = list() list_out.insert(0, float(listtmp[0])) list_out.insert(1, float(listtmp[1])) list_out.insert(2, float(listtmp[2])) userdata.intermediate_pose = list_out """ #userdata.intermediate_pose = eval(s.solution.__str__()) if s.solution.find("[") == -1: # not a list (the position is home order etc.) userdata.intermediate_pose = s.solution.__str__() else: # list [1, 2, 3] etc. try: userdata.intermediate_pose = eval( s.solution.__str__()) except Exception, e: rospy.INFO("%s", e) return 'failed' return 'retry'