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