def start_perception(self, req):
resp = TaskDataServiceResponse()
resp.taskdata = req.taskdata
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 = resp.taskdata.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',
resp.taskdata.initialization_time,
resp.taskdata.logging, 'Perception')
while not self.__perception_ready:
time.sleep(1)
rospy.loginfo('Sucessfully started Perception')
resp.result = 'success'
return resp
def start_perception(self, req):
resp = TaskDataServiceResponse()
resp.taskdata = req.taskdata
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 = resp.taskdata.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',
resp.taskdata.initialization_time,
resp.taskdata.logging, 'Perception')
while not self.__perception_ready:
time.sleep(1)
rospy.loginfo('Sucessfully started Perception')
resp.result = 'success'
return resp
def start_manipulation(self, req):
resp = TaskDataServiceResponse()
resp.taskdata = req.taskdata
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', resp.taskdata.initialization_time,
resp.taskdata.logging, 'Manipulation')
while not self.__move_group_status and not self.__manipulation_nodes_ready:
time.sleep(1)
task_type = resp.taskdata.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',
resp.taskdata.initialization_time, resp.taskdata.logging, 'Conveyor frames')
manipulation_conveyor_frames_process = manipulation_conveyor_frames_process
else:
manipulation_conveyor_frames_process = None
rospy.loginfo('Sucessfully started Manipulation')
resp.result = 'success'
return resp
def start_classifier(self, req):
resp = TaskDataServiceResponse()
resp.taskdata = req.taskdata
rospy.loginfo('Executing state StartClassifier')
global classifier_process
classifier_process, classifier_logger_process =\
utils.start_node('rosrun suturo_perception_classifier classifier.py', req.taskdata.initialization_time,
req.taskdata.logging, 'Classifier')
resp.result = 'success'
rospy.loginfo('Sucessfully started Classifier')
return resp
def start_classifier(self, req):
resp = TaskDataServiceResponse()
resp.taskdata = req.taskdata
rospy.loginfo('Executing state StartClassifier')
global classifier_process
classifier_process, classifier_logger_process =\
utils.start_node('rosrun suturo_perception_classifier classifier.py', req.taskdata.initialization_time,
req.taskdata.logging, 'Classifier')
resp.result = 'success'
rospy.loginfo('Sucessfully started Classifier')
return resp
def stop_simulation(self, req):
resp = TaskDataServiceResponse()
resp.taskdata = req.taskdata
rospy.loginfo('Executing state StopSimulation')
if not executed_test_node_check:
check_node(resp.taskdata.initialization_time, resp.taskdata.logging)
if self.savelog:
save_task()
stop_task()
time.sleep(3)
rospy.loginfo('Finished state StopSimulation')
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
def stop_simulation(self, req):
resp = TaskDataServiceResponse()
resp.taskdata = req.taskdata
rospy.loginfo('Executing state StopSimulation')
if not executed_test_node_check:
check_node(resp.taskdata.initialization_time,
resp.taskdata.logging)
if self.savelog:
save_task()
stop_task()
time.sleep(3)
rospy.loginfo('Finished state StopSimulation')
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
def stop_nodes(self, req):
resp = TaskDataServiceResponse()
resp.taskdata = req.taskdata
rospy.loginfo('Executing state StopNodes.')
if perception_process is not None:
exterminate(perception_process.pid, signal.SIGKILL, r=True)
if manipulation_process is not None:
exterminate(manipulation_process.pid, signal.SIGKILL, r=True)
if manipulation_conveyor_frames_process is not None:
exterminate(manipulation_conveyor_frames_process.pid, signal.SIGKILL, r=True)
if classifier_process is not None:
exterminate(classifier_process.pid, signal.SIGKILL, r=True)
rospy.signal_shutdown('Finished plan. Shutting down Node.')
time.sleep(3)
rospy.loginfo('Finished state StopNodes.')
resp.result = 'success'
return resp
def stop_nodes(self, req):
resp = TaskDataServiceResponse()
resp.taskdata = req.taskdata
rospy.loginfo('Executing state StopNodes.')
if perception_process is not None:
exterminate(perception_process.pid, signal.SIGKILL, r=True)
if manipulation_process is not None:
exterminate(manipulation_process.pid, signal.SIGKILL, r=True)
if manipulation_conveyor_frames_process is not None:
exterminate(manipulation_conveyor_frames_process.pid,
signal.SIGKILL,
r=True)
if classifier_process is not None:
exterminate(classifier_process.pid, signal.SIGKILL, r=True)
rospy.signal_shutdown('Finished plan. Shutting down Node.')
time.sleep(3)
rospy.loginfo('Finished state StopNodes.')
resp.result = 'success'
return resp
def determine_task_type(self, req):
resp = TaskDataServiceResponse()
resp.taskdata = req.taskdata
rospy.loginfo('Executing state DetermineTaskType')
task_type = resp.taskdata.yaml.task_type
if task_type == Task.TASK_1:
ret = 'task1'
elif task_type == Task.TASK_2:
ret = 'task2'
elif task_type == Task.TASK_3:
ret = 'task3'
elif task_type == Task.TASK_4:
ret = 'task4'
elif task_type == Task.TASK_5:
ret = 'task5'
elif task_type == Task.TASK_6:
ret = 'task6'
else:
ret = 'fail'
resp.result = ret
rospy.loginfo('Executing task is from type ' + str(ret) + '.')
return resp
def determine_task_type(self, req):
resp = TaskDataServiceResponse()
resp.taskdata = req.taskdata
rospy.loginfo('Executing state DetermineTaskType')
task_type = resp.taskdata.yaml.task_type
if task_type == Task.TASK_1:
ret = 'task1'
elif task_type == Task.TASK_2:
ret = 'task2'
elif task_type == Task.TASK_3:
ret = 'task3'
elif task_type == Task.TASK_4:
ret = 'task4'
elif task_type == Task.TASK_5:
ret = 'task5'
elif task_type == Task.TASK_6:
ret = 'task6'
else:
ret = 'fail'
resp.result = ret
rospy.loginfo('Executing task is from type ' + str(ret) + '.')
return resp
def start_manipulation(self, req):
resp = TaskDataServiceResponse()
resp.taskdata = req.taskdata
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', resp.taskdata.initialization_time,
resp.taskdata.logging, 'Manipulation')
while not self.__move_group_status and not self.__manipulation_nodes_ready:
time.sleep(1)
task_type = resp.taskdata.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',
resp.taskdata.initialization_time, resp.taskdata.logging, 'Conveyor frames')
manipulation_conveyor_frames_process = manipulation_conveyor_frames_process
else:
manipulation_conveyor_frames_process = None
rospy.loginfo('Sucessfully started Manipulation')
resp.result = 'success'
return resp
def __handle_call(self, req):
response = TaskDataServiceResponse()
taskdata = req.taskdata
response.result = self.__scan_obstacles(taskdata)
response.taskdata = taskdata
return response
def init(self, req):
resp = TaskDataServiceResponse()
resp.taskdata = req.taskdata
self.create_manipulation()
resp.result = 'success'
return resp
def __handle_call(self, req):
response = TaskDataServiceResponse()
taskdata = req.taskdata
response.result = self.__scan_obstacles(taskdata)
response.taskdata = taskdata
return response
def __handle_call(self, req):
resp = TaskDataServiceResponse()
resp.result = self.__classify(req.taskdata)
resp.taskdata = req.taskdata
return resp
def init(self, req):
resp = TaskDataServiceResponse()
resp.taskdata = req.taskdata
self.create_manipulation()
resp.result = 'success'
return resp
