def main(task, with_plan, init_sim, savelog, no_taskselector,
initialization_time, logging, parent_pid):
print('start_task: main')
if init_sim and not no_taskselector:
#Taskselector
print "Starting task_selector"
global _pro_task_selector
_pro_task_selector = subprocess.Popen(
'rosrun euroc_launch TaskSelector',
stdout=subprocess.PIPE,
shell=True,
preexec_fn=os.setsid)
rospy.sleep(5)
#If plans should be started start the state machine
if with_plan:
rospy.init_node('suturo_planning_execution', log_level=rospy.DEBUG)
rospy.loginfo('Started plan')
toplevel_plan(init_sim,
task,
savelog,
initialization_time,
logging,
parent_pid=parent_pid)
else:
rospy.init_node('suturo_planning_start_task', log_level=rospy.DEBUG)
#Start tasks
start_task(task)
print 'Waiting for ctrl-c'
rospy.spin()
print('start_task: exiting main')
def execute(self, userdata):
rospy.loginfo('Executing state StartSimulation')
userdata.yaml = suturo_planning_task_selector.start_task(self.task_name)
rospy.loginfo('Got YAML description')
userdata.objects_found = []
rospy.loginfo('Waiting for clock.')
self.clock = rospy.Subscriber('clock', Clock, self.wait_for_clock)
while not self.new_clock:
time.sleep(1)
return 'success'
def execute(self, userdata):
rospy.loginfo('Executing state StartSimulation')
userdata.yaml = suturo_planning_task_selector.start_task(
self.task_name)
rospy.loginfo('Got YAML description')
userdata.objects_found = []
rospy.loginfo('Waiting for clock.')
self.clock = rospy.Subscriber('clock', Clock, self.wait_for_clock)
while not self.new_clock:
time.sleep(1)
return 'success'
def main(task, with_plan, init_sim, savelog, no_taskselector, initialization_time, logging, parent_pid):
print('start_task: main')
if init_sim and not no_taskselector:
#Taskselector
print "Starting task_selector"
global _pro_task_selector
_pro_task_selector = subprocess.Popen('rosrun euroc_launch TaskSelector', stdout=subprocess.PIPE,
shell=True, preexec_fn=os.setsid)
rospy.sleep(5)
#If plans should be started start the state machine
if with_plan:
rospy.init_node('suturo_planning_execution', log_level=rospy.DEBUG)
rospy.loginfo('Started plan')
toplevel_plan(init_sim, task, savelog, initialization_time, logging, parent_pid=parent_pid)
else:
rospy.init_node('suturo_planning_start_task', log_level=rospy.DEBUG)
#Start tasks
start_task(task)
print 'Waiting for ctrl-c'
rospy.spin()
print('start_task: exiting main')
def start_simulation(self, req):
resp = TaskDataServiceResponse()
resp.taskdata = req.taskdata
rospy.loginfo('Executing state StartSimulation')
resp.taskdata.yaml = suturo_planning_task_selector.start_task(resp.taskdata.name)
rospy.loginfo('Got YAML description')
resp.taskdata.objects_found = []
rospy.loginfo('Waiting for clock.')
self.clock = rospy.Subscriber('clock', Clock, self.wait_for_clock)
while not self.new_clock:
time.sleep(1)
rospy.loginfo('Sucessfully started Simulation')
resp.result = 'success'
return resp
def start_simulation(self, req):
resp = TaskDataServiceResponse()
resp.taskdata = req.taskdata
rospy.loginfo('Executing state StartSimulation')
resp.taskdata.yaml = suturo_planning_task_selector.start_task(
resp.taskdata.name)
rospy.loginfo('Got YAML description')
resp.taskdata.objects_found = []
rospy.loginfo('Waiting for clock.')
self.clock = rospy.Subscriber('clock', Clock, self.wait_for_clock)
while not self.new_clock:
time.sleep(1)
rospy.loginfo('Sucessfully started Simulation')
resp.result = 'success'
return resp
