Exemple #1
0
    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
Exemple #2
0
 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
Exemple #3
0
 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
Exemple #4
0
 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
Exemple #5
0
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.')
Exemple #6
0
 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
Exemple #7
0
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.')
Exemple #8
0
    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