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