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