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'
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'
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'
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'
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.')
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.')
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'
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'
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.')
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.')