Пример #1
0
    def execute(self, userdata):
        rospy.loginfo('Executing TestNode init.')
        subprocess.Popen('rosrun euroc_launch TestNode --init', shell=True)
        rospy.loginfo('Executing state StartManipulation')
        rospy.loginfo('Subscribing to /suturo/manipulation_node_status.')
        self.__nodes_subscriber = rospy.Subscriber('/suturo/manipulation_node_status', ManipulationNodeStatus,
                                             self.wait_for_manipulation)
        rospy.loginfo('Subscribign to /move_group/status topic.')
        rospy.sleep(1)
        self.__move_group_status_subscriber = rospy.Subscriber('/move_group/status', GoalStatusArray,
                                                               self.wait_for_move_group_status)
        global manipulation_process
        manipulation_process, manipulation_logger_process =\
            utils.start_node('roslaunch euroc_launch manipulation.launch', userdata.initialization_time,
                             userdata.logging, 'Manipulation')
        userdata.manipulation_process = manipulation_process

        while not self.__move_group_status and not self.__manipulation_nodes_ready:
            time.sleep(1)

        task_type = userdata.yaml.task_type
        if task_type == Task.TASK_6:
            global manipulation_conveyor_frames_process
            rospy.loginfo('Starting publish_conveyor_frames.')
            manipulation_conveyor_frames_process, manipulation_conveyor_frames_logger_process =\
                utils.start_node('rosrun suturo_planning_manipulation publish_conveyor_frames.py',
                                 userdata.initialization_time, userdata.logging, 'Conveyor frames')
            userdata.manipulation_conveyor_frames_process = manipulation_conveyor_frames_process
        else:
            userdata.manipulation_conveyor_frames_process = None
        return 'success'
Пример #2
0
 def execute(self, userdata):
     rospy.loginfo('Executing state StartClassifier')
     global classifier_process
     classifier_process, classifier_logger_process =\
         utils.start_node('rosrun suturo_perception_classifier classifier.py', userdata.initialization_time,
                          userdata.logging, 'Classifier')
     userdata.classifier_process = classifier_process
     return 'success'
Пример #3
0
def check_node(initialization_time, logging):
    global executed_test_node_check
    rospy.loginfo('Executing TestNode check.')
    rospy.loginfo('rospy.is_shutdown(): ' + str(rospy.is_shutdown()))
    test_node, test_node_logger = utils.start_node('rosrun euroc_launch TestNode --check', initialization_time,
                                                   logging, 'TestNode check')
    if not utils.wait_for_process(test_node, 15):
        rospy.loginfo('Killing TestNode check.')
        exterminate(test_node.pid, signal.SIGKILL)
    executed_test_node_check = True
    rospy.loginfo('Finished TestNode check.')
Пример #4
0
 def execute(self, userdata):
     rospy.loginfo('Executing state StartPerception')
     rospy.loginfo('Subscribing to /suturo/perception_node_status.')
     self.__subscriber = rospy.Subscriber('/suturo/perception_node_status', PerceptionNodeStatus,
                                          self.wait_for_perception)
     rospy.sleep(1)
     task_type = userdata.yaml.task_type
     if task_type == Task.TASK_4:
         task_num = '4'
     elif task_type == Task.TASK_6:
         task_num = '6'
     else:
         task_num = '1'
     global perception_process
     perception_process, perception_logger_process =\
         utils.start_node('roslaunch euroc_launch perception_task' + task_num + '.launch', userdata.initialization_time,
                          userdata.logging, 'Perception')
     userdata.perception_process = perception_process
     while not self.__perception_ready:
         time.sleep(1)
     return 'success'
Пример #5
0
def start_demo(wait, tasks, logging):
    global __quit
    global __clock_subscriber
    global __time_started_task
    global __current_task
    global subproc
    global __kill_count
    global __remaining_time
    global __time_limit
    global __task_aborted_by_exception
    global __current_intent
    global __clock_time_started_task
    global __last_received_clock
    global __after_task
    rospy.init_node('start_complete_demo')

    rospy.loginfo('Setting use_sim_time to true')
    rospy.set_param('/use_sim_time', True)

    #atexit.register(exit_handler)
    signal.signal(signal.SIGTERM, exit_handler)
    signal.signal(signal.SIGINT, exit_handler)
    signal.signal(signal.SIGQUIT, exit_handler)
    signal.signal(signal.SIGUSR1, usr1_handler)
    signal.signal(signal.SIGUSR2, usr2_handler)
    print_it('Getting available task names.')
    tasks_to_execute = []
    task_names = get_available_task_names()
    print_it('Available task names: ' + str(task_names))
    #start_task.main(task_names[:2], True, True, True, True)
    if re.search('^\d*:\d*$', tasks):
        tasks_to_execute = eval('task_names[' + tasks + ']')
    elif re.search('^\d+$', tasks):
        tasks_to_execute = [task_names[int(tasks)]]
    elif re.search('^(\d+,?)+$', tasks):
        for num in tasks.split(','):
            tasks_to_execute.append(task_names[int(num)])
            tasks_to_execute.sort()
    else:
        for task in tasks.split(','):
            if task in task_names:
                tasks_to_execute.append(task)
            tasks_to_execute.sort()
    print_it('Going to execute the following tasks: ' + str(tasks_to_execute))
    if logging in [2]:
        dont_print = False
    else:
        dont_print = True
    i = 0
    __clock_time_started_task = 0
    while i < len(tasks_to_execute):
        __kill_count = 0
        __task_aborted_by_exception = False
        rospy.loginfo('Setting use_sim_time to true')
        rospy.set_param('/use_sim_time', True)
        if __quit:
            print_it('Demo has been aborted. Exiting (1)')
            return
        __current_task = tasks_to_execute[i]
        init_time = __initialization_time + '-' + __current_task + '-' + str(__current_intent)
        if wait:
            raw_input('Starting task ' + str(__current_task) + '. Press ENTER.')
        print_it('################################################################################')
        print_it('Starting task : ' + str(__current_task))
        print_it('Intent        : ' + str(__current_intent))
        print_it('Started at    : ' + str(__clock_time_started_task))
        print_it('Finished tasks: ' + str(tasks_to_execute[0:tasks_to_execute.index(__current_task)]))
        print_it('Tasks to go   : ' + str(tasks_to_execute[tasks_to_execute.index(__current_task)+1:]))
        print_it('################################################################################')
        subproc, logger_process = utils.start_node('rosrun suturo_planning_startup start_task.py ' + __current_task +
                                                   ' --plan --init --save --no-ts --inittime="' + init_time + '"' +
                                                   ' --logging="' + str(logging) + '"' + ' --parent=' + str(os.getpid())
                                                   , init_time, logging, 'Complete',
                                                   dont_print=dont_print, print_prefix_to_stdout=False)
        __time_started_task = int(time.time())
        print_it('Subscribing to clock.')
        __clock_subscriber = rospy.Subscriber('clock', Clock, handle_clock, queue_size=1,
                                              callback_args={'task': __current_task,
                                                             'intent': __current_intent})
        print_it('Waiting for task ' + __current_task + ' to terminate.')
        __after_task = False
        subproc.wait()
        __after_task = True
        print_it('Task ' + __current_task + ' terminated')
        print_it('Unsubscribing clock subscriber.')
        __clock_subscriber.unregister()
        if __quit:
            print_it('Demo has been aborted. Exiting (2)')
            return
        else:
            if __task_aborted_by_exception:
                print_it('Task has been interrupted by exception.')
                if __remaining_time > __time_limit / 2:
                    print_it('Restarting task ' + __current_task)
                    __current_intent += 1
                    __clock_time_started_task += __last_received_clock
                else:
                    print_it('Remaining time (' + str(__remaining_time) + 's) is too low to restart task.')
                    print_it('Finished task ' + str(__current_task))
                    i += 1
                    __current_intent = 1
                    __clock_time_started_task = 0
            else:
                print_it('Task terminated as expected.')
                print_it('Finished task ' + str(__current_task))
                i += 1
                __current_intent = 1
                __clock_time_started_task = 0
                if i != len(tasks_to_execute):
                    time.sleep(5)
            # __remaining_time = __time_limit
    print_it('Finished complete demo.')
    rospy.signal_shutdown('Finished complete demo.')