def test_client_srs_ui_pro_echo_server(json_input):
    client = actionlib.SimpleActionClient('srs_ui_pro/echo_server', echo_server_msg.dm_serverAction)
    client.wait_for_server()
    
    goal = echo_server_msg.dm_serverGoal()
    goal.json_input = json_input
    print "###type of goal is", type(goal)
    print "###goal is", goal
    print "###goal.json_input is", goal.json_input 
    
    #server_feedback = echo_server_msg.dm_serverFeedback()
    #server_result = echo_server_msg.dm_serverResult()
    server_feedback = echo_server_msg.dm_serverFeedback()
    server_result = echo_server_msg.dm_serverResult()
    try:
        client.send_goal(goal, result_callback, None, feedback_callback)
    except rospy.ServiceException, e:
        print "Service did not process request: %s"%str(e)
def test_client_srs_ui_pro_echo_server(json_input):
    client = actionlib.SimpleActionClient('srs_ui_pro/echo_server',
                                          echo_server_msg.dm_serverAction)
    client.wait_for_server()

    goal = echo_server_msg.dm_serverGoal()
    goal.json_input = json_input
    print "###type of goal is", type(goal)
    print "###goal is", goal
    print "###goal.json_input is", goal.json_input

    #server_feedback = echo_server_msg.dm_serverFeedback()
    #server_result = echo_server_msg.dm_serverResult()
    server_feedback = echo_server_msg.dm_serverFeedback()
    server_result = echo_server_msg.dm_serverResult()
    try:
        client.send_goal(goal, result_callback, None, feedback_callback)
    except rospy.ServiceException, e:
        print "Service did not process request: %s" % str(e)
예제 #3
0
 def execute(self,userdata):
     
     print ('CHECKING IF USER INTERVENTION IS REQUIRED')
     # check if user intervention is required. 
     # give_up if the task is in fully autonomous mode
     if userdata.semi_autonomous_mode == False:
         return 'give_up'
     
     global current_task_info
     #name of the overall task
     #the_task_name = current_task_info.task_feedback.task_name 
     
     #if not the_task_name:
         #return 'give_up'
     
     the_task_feedback = current_task_info.task_feedback
     
     if not the_task_feedback:
         return 'give_up'
     
     #parameter of the overall task
     the_task_parameter = current_task_info.task_feedback.task_parameter
     
     step_id = len (current_task_info.last_step_info)         
     
     print "### user intervention is going on..."
     
     if step_id > 0:
         #name of the current step 
         the_action_name = current_task_info.last_step_info[step_id-1].step_name 
         
         #out come of the last action
         the_action_outcome = current_task_info.last_step_info[step_id-1].outcome
     
         #object of the action
         the_action_object = current_task_info.task_feedback.action_object
     
         #parent of the object
         the_action_object_parent = current_task_info.task_feedback.action_object_parent
         
         try:
             print "### action client of echo server is working..."
             client = actionlib.SimpleActionClient('srs_ui_pro/echo_server', echo_server_msg.dm_serverAction)
             
             if client.wait_for_server(timeout=rospy.Duration(5)) is False:
                 rospy.loginfo ("there is no response from srs_ui_pro, this intervention action cannot be executed now...")
                 return self.give_up(the_action_name)
             else:
                 global sss      
                 rospy.sleep(6)            
                 sss.say(["I can not finish the task"])
                 sss.say(["Remote Operators are Online Should we ask them for help"])
                 
                 rospy.wait_for_service('answer_yes_no')
                 try:
                     # call ui_pri_topic_yes_no
                     # if the answer is "no", then return 'give_up'
                     answer_yes_no = rospy.ServiceProxy('answer_yes_no', xsrv.answer_yes_no)
                     resp = answer_yes_no()
                     if resp.answer == "No":
                         rospy.loginfo ("the use refused to get any remote assistance...")
                         # in this stage, the anser_yes_no is not used
                         # to use it, just make sure the following line is available
                         #return 'give_up'
                 except rospy.ServiceException, e:
                     print "Service call failed: %s"%e
             
             goal = echo_server_msg.dm_serverGoal()
             
             # see the json_parser file
             try:
                 json_decoded = json.loads(current_task_info.json_parameters)
             except Exception:
                 rospy.loginfo ("current_task_info.json_parameters is invalid...")
                 return self.give_up(the_action_name)
                 
             current_tasks = json_decoded['tasks']
             
             # value for "exception_id" in goal
             exception_id = 1
             
             time_schedule = 'null'
             current_task = 'null'
             deliver_destination = 'null'
             
             
             # value for "time_schedule" in goal
             try:
                 time_schedule = current_tasks[0]['time_schedule']
             except Exception:
                 rospy.loginfo ("no time schedule in this task set null")
             
             
             # value for "task" in goal
             try:
                 current_task = current_tasks[0]['task']
             except Exception:
                 rospy.loginfo ("no name in this task set null")
             
             # value for "deliver_destination" in goal
             try:
                 deliver_destination = current_tasks[0]['deliver_destination']
             except Exception:
                 rospy.loginfo ("no destination in this task set null")    
             
             # value for "additional_information" in goal
             additional_information = "this is a test message"
             
             current_goal = {"exception_id": exception_id, "tasks": [{"time_schedule": time_schedule, "task": current_task, "deliver_destination": deliver_destination}], "additional_information": additional_information}
             
              #convert current goal to json object
             json_input = json.dumps(current_goal)
            
             # construct a goal
             goal.json_input = json_input
             
             server_feedback = echo_server_msg.dm_serverFeedback()
             server_result = echo_server_msg.dm_serverResult()
             
             # send the goal to echo server
             client.send_goal(goal, self.result_callback, self.active_callback, self.feedback_callback)
             #rospy.sleep(25)
             
             timeout = 6000
             while(self.flag != True and timeout > 0):
                 rospy.sleep(1)
                 timeout = timeout - 1
             
             if self.server_json_result == "" :
                 rospy.loginfo ("*******")
                 rospy.loginfo ("there is no response from srs_ui_pro, the current intervention action has been given up...")
                 rospy.loginfo ("*******")
                 rospy.sleep(3)
                 return self.give_up(the_action_name)
             
             _feedback = xmsg.ExecutionFeedback()
             _feedback.current_state =  self.server_current_status + ": started"
             _feedback.solution_required = False
             _feedback.exceptional_case_id = exception_id
             _feedback.json_feedback = self.server_json_feedback
             current_task_info._srs_as._as.publish_feedback(_feedback)
         
             json_decoded = json.loads(self.server_json_result)
             result = json_decoded['result']
             # result should be succeeded
             if result == "succeeded":           
                 sss.say(["With the help of remote Operators, The task has been completed "])
                 return 'completed'
             elif result == "failed":
                 sss.say(["This task is impossible, I have to give up"])
                 return "failed"
             else:
                 return "give_up"
         except rospy.ROSInterruptException:
             print "error before completion"
             return "failed"
예제 #4
0
    def execute(self, userdata):

        print('CHECKING IF USER INTERVENTION IS REQUIRED')
        # check if user intervention is required.
        # give_up if the task is in fully autonomous mode
        if userdata.semi_autonomous_mode == False:
            return 'give_up'

        global current_task_info
        #name of the overall task
        #the_task_name = current_task_info.task_feedback.task_name

        #if not the_task_name:
        #return 'give_up'

        the_task_feedback = current_task_info.task_feedback

        if not the_task_feedback:
            return 'give_up'

        #parameter of the overall task
        the_task_parameter = current_task_info.task_feedback.task_parameter

        step_id = len(current_task_info.last_step_info)

        print "### user intervention is going on..."

        if step_id > 0:
            #name of the current step
            the_action_name = current_task_info.last_step_info[step_id -
                                                               1].step_name

            #out come of the last action
            the_action_outcome = current_task_info.last_step_info[step_id -
                                                                  1].outcome

            #object of the action
            the_action_object = current_task_info.task_feedback.action_object

            #parent of the object
            the_action_object_parent = current_task_info.task_feedback.action_object_parent

            try:
                print "### action client of echo server is working..."
                client = actionlib.SimpleActionClient(
                    'srs_ui_pro/echo_server', echo_server_msg.dm_serverAction)

                if client.wait_for_server(timeout=rospy.Duration(5)) is False:
                    rospy.loginfo(
                        "there is no response from srs_ui_pro, this intervention action cannot be executed now..."
                    )
                    return self.give_up(the_action_name)
                else:
                    global sss
                    rospy.sleep(6)
                    sss.say(["I can not finish the task"])
                    sss.say([
                        "Remote Operators are Online Should we ask them for help"
                    ])

                    rospy.wait_for_service('answer_yes_no')
                    try:
                        # call ui_pri_topic_yes_no
                        # if the answer is "no", then return 'give_up'
                        answer_yes_no = rospy.ServiceProxy(
                            'answer_yes_no', xsrv.answer_yes_no)
                        resp = answer_yes_no()
                        if resp.answer == "No":
                            rospy.loginfo(
                                "the use refused to get any remote assistance..."
                            )
                            # in this stage, the anser_yes_no is not used
                            # to use it, just make sure the following line is available
                            #return 'give_up'
                    except rospy.ServiceException, e:
                        print "Service call failed: %s" % e

                goal = echo_server_msg.dm_serverGoal()

                # see the json_parser file
                try:
                    json_decoded = json.loads(
                        current_task_info.json_parameters)
                except Exception:
                    rospy.loginfo(
                        "current_task_info.json_parameters is invalid...")
                    return self.give_up(the_action_name)

                current_tasks = json_decoded['tasks']

                # value for "exception_id" in goal
                exception_id = 1

                time_schedule = 'null'
                current_task = 'null'
                deliver_destination = 'null'

                # value for "time_schedule" in goal
                try:
                    time_schedule = current_tasks[0]['time_schedule']
                except Exception:
                    rospy.loginfo("no time schedule in this task set null")

                # value for "task" in goal
                try:
                    current_task = current_tasks[0]['task']
                except Exception:
                    rospy.loginfo("no name in this task set null")

                # value for "deliver_destination" in goal
                try:
                    deliver_destination = current_tasks[0][
                        'deliver_destination']
                except Exception:
                    rospy.loginfo("no destination in this task set null")

                # value for "additional_information" in goal
                additional_information = "this is a test message"

                current_goal = {
                    "exception_id":
                    exception_id,
                    "tasks": [{
                        "time_schedule": time_schedule,
                        "task": current_task,
                        "deliver_destination": deliver_destination
                    }],
                    "additional_information":
                    additional_information
                }

                #convert current goal to json object
                json_input = json.dumps(current_goal)

                # construct a goal
                goal.json_input = json_input

                server_feedback = echo_server_msg.dm_serverFeedback()
                server_result = echo_server_msg.dm_serverResult()

                # send the goal to echo server
                client.send_goal(goal, self.result_callback,
                                 self.active_callback, self.feedback_callback)
                #rospy.sleep(25)

                timeout = 6000
                while (self.flag != True and timeout > 0):
                    rospy.sleep(1)
                    timeout = timeout - 1

                if self.server_json_result == "":
                    rospy.loginfo("*******")
                    rospy.loginfo(
                        "there is no response from srs_ui_pro, the current intervention action has been given up..."
                    )
                    rospy.loginfo("*******")
                    rospy.sleep(3)
                    return self.give_up(the_action_name)

                _feedback = xmsg.ExecutionFeedback()
                _feedback.current_state = self.server_current_status + ": started"
                _feedback.solution_required = False
                _feedback.exceptional_case_id = exception_id
                _feedback.json_feedback = self.server_json_feedback
                current_task_info._srs_as._as.publish_feedback(_feedback)

                json_decoded = json.loads(self.server_json_result)
                result = json_decoded['result']
                # result should be succeeded
                if result == "succeeded":
                    sss.say([
                        "With the help of remote Operators, The task has been completed "
                    ])
                    return 'completed'
                elif result == "failed":
                    sss.say(["This task is impossible, I have to give up"])
                    return "failed"
                else:
                    return "give_up"
            except rospy.ROSInterruptException:
                print "error before completion"
                return "failed"