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_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_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 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 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 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 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
