def main(): rospy.init_node('simple_trolley_init_script', anonymous=True) art = ArtApiHelper() art.wait_for_api() art.store_object_type(obj_type("mala_desticka", 0.0125, 0.06, 0.06)) art.store_object_type( obj_type("Modry_kontejner", 0.14, 0.195, 0.075, container=True)) # delete all created programs ph = art.get_program_headers() if ph: for h in ph: art.program_clear_ro(h.id) art.delete_program(h.id) prog = Program() prog.header.id = 1 prog.header.name = "Pick & Place" pb = ProgramBlock() pb.id = 1 pb.name = "Pick & Place" pb.on_success = 1 pb.on_failure = 0 prog.blocks.append(pb) pb.items.append(polygon_item(1)) pb.items.append(place_item(2, ref_id=[1], on_success=3, on_failure=0)) pb.items.append(item(3, "GetReady", on_success=4, on_failure=0)) pb.items.append(polygon_item(4)) pb.items.append(place_item(5, ref_id=[4], on_success=6, on_failure=0)) pb.items.append(item(6, "GetReady", on_success=1, on_failure=0)) art.store_program(prog) art.program_set_ro(prog.header.id)
class ProgramStore(object): def __init__(self): self.art = ArtApiHelper() def store_programs(self): program_headers = self.art.get_program_headers() programs = [] file = open("JSONprograms_" + str(rospy.Time.now().to_nsec()), "w") bag = rosbag.Bag( 'BAGprograms_' + str(rospy.Time.now().to_nsec()) + '.bag', 'w') for header in program_headers: program = self.art.load_program(header.id) programs.append(program) to_print = json.loads( json_message_converter.convert_ros_message_to_json(program)) file.write(">>>>>>>>>>>>>>>>>>>>>>>>>> program id " + str(header.id) + " <<<<<<<<<<<<<<<<<<<<<<<<<<<<<\n") file.write(json.dumps(to_print, indent=2, sort_keys=False)) file.write("\n\n\n") bag.write("program", program) file.close() bag.close()
class UICoreRos(UICore): """The class builds on top of UICore and adds ROS-related stuff and application logic. Attributes: user_status (UserStatus): current user tracking status fsm (FSM): state machine maintaining current state of the interface and proper transitions between states state_manager (interface_state_manager): synchronization of interfaces within the ARTable system scene_pub (rospy.Publisher): publisher for scene images last_scene_update (rospy.Time): time when the last scene image was published scene_img_deq (Queue.Queue): thread-safe queue for scene images (which are published in separate thread) projectors (list): array of ProjectorHelper instances art (ArtApiHelper): easy access to ARTable services """ def __init__(self): origin = rospy.get_param("scene_origin") size = rospy.get_param("scene_size") rpm = rospy.get_param("rpm") port = rospy.get_param("scene_server_port") super(UICoreRos, self).__init__(origin[0], origin[1], size[0], size[1], rpm, port) QtCore.QObject.connect(self, QtCore.SIGNAL('objects'), self.object_cb_evt) QtCore.QObject.connect(self, QtCore.SIGNAL('user_status'), self.user_status_cb_evt) QtCore.QObject.connect(self, QtCore.SIGNAL('interface_state'), self.interface_state_evt) QtCore.QObject.connect(self, QtCore.SIGNAL('touch_calibration_points_evt'), self.touch_calibration_points_evt) QtCore.QObject.connect(self, QtCore.SIGNAL('touch_detected_evt'), self.touch_detected_evt) QtCore.QObject.connect(self, QtCore.SIGNAL('notify_user_evt'), self.notify_user_evt) QtCore.QObject.connect(self, QtCore.SIGNAL('learning_request_done_evt'), self.learning_request_done_evt) self.user_state = None self.program_list = None self.program_vis = None self.template = False # TODO this should be stored in program_vis? self.last_prog_pos = (0.2, self.height - 0.2) self.last_edited_prog_id = None self.ph = ProgramHelper() cursors = rospy.get_param("~cursors", []) for cur in cursors: PoseStampedCursorItem(self.scene, cur) TouchTableItem( self.scene, '/art/interface/touchtable/touch', list(self.get_scene_items_by_type(PoseStampedCursorItem))) self.stop_btn = ButtonItem(self.scene, 0, 0, "STOP", None, self.stop_btn_clicked, 2.0, QtCore.Qt.red) self.stop_btn.setPos( self.scene.width() - self.stop_btn.boundingRect().width() - 300, self.scene.height() - self.stop_btn.boundingRect().height() - 60) self.stop_btn.set_enabled(True) self.projectors = [] projs = rospy.get_param("~projectors", []) for proj in projs: self.projectors.append(ProjectorHelper(proj)) rospy.loginfo("Waiting for /art/brain/learning_request") self.learning_action_cl = actionlib.SimpleActionClient( '/art/brain/learning_request', LearningRequestAction) self.learning_action_cl.wait_for_server() self.art = ArtApiHelper() self.projectors_calibrated_pub = rospy.Publisher( "~projectors_calibrated", Bool, queue_size=1, latch=True) self.projectors_calibrated_pub.publish(False) self.start_learning_srv = rospy.ServiceProxy( '/art/brain/learning/start', startProgram) # TODO wait for service? where? self.stop_learning_srv = rospy.ServiceProxy( '/art/brain/learning/stop', Trigger) # TODO wait for service? where? self.program_pause_srv = rospy.ServiceProxy('/art/brain/program/pause', Trigger) self.program_resume_srv = rospy.ServiceProxy( '/art/brain/program/resume', Trigger) self.program_stop_srv = rospy.ServiceProxy('/art/brain/program/stop', Trigger) self.emergency_stop_srv = rospy.ServiceProxy( '/pr2_ethercat/halt_motors', EmptyService) # TODO wait for service? where? self.emergency_stop_reset_srv = rospy.ServiceProxy( '/pr2_ethercat/reset_motors', EmptyService) # TODO wait for service? where? self.program_error_resolve_srv = rospy.ServiceProxy( '/art/brain/program/error_response', ProgramErrorResolve) # TODO wait for service? where? self.program_error_dialog = None self.grasp_dialog = None self.emergency_stopped = False rospy.loginfo("Waiting for ART services...") self.art.wait_for_api() rospy.loginfo("Ready! Starting state machine.") # TODO move this to ArtApiHelper ?? self.obj_sub = rospy.Subscriber('/art/object_detector/object_filtered', InstancesArray, self.object_cb, queue_size=1) self.user_status_sub = rospy.Subscriber('/art/user/status', UserStatus, self.user_status_cb, queue_size=1) self.touch_points = None self.touch_calib_srv = rospy.Service( '/art/interface/projected_gui/touch_calibration', TouchCalibrationPoints, self.touch_calibration_points_cb) self.notify_user_srv = rospy.Service( '/art/interface/projected_gui/notify_user', NotifyUser, self.notify_user_srv_cb) proj_calib = True if len(self.projectors) > 0: rospy.loginfo("Waiting for projector nodes...") for proj in self.projectors: proj.wait_until_available() if not proj.is_calibrated(): proj_calib = False if proj_calib: rospy.loginfo('Projectors already calibrated.') self.projectors_calibrated_pub.publish(True) else: rospy.loginfo( 'Projectors not calibrated yet - waiting for command...') self.projector_calib_srv = rospy.Service( '/art/interface/projected_gui/calibrate_projectors', Trigger, self.calibrate_projectors_cb) self.state_manager = InterfaceStateManager("PROJECTED UI", cb=self.interface_state_cb) def touch_calibration_points_evt(self, pts): for it in self.scene.items(): if isinstance(it, LabelItem): continue it.setVisible(False) # TODO remember settings (how?) self.notif(translate( "UICoreRos", "Touch table calibration started. Please press the white point."), temp=False) self.touch_points = TouchPointsItem(self.scene, pts) def save_gripper_pose_cb(self, idx): topics = [ '/art/pr2/right_arm/gripper/pose', '/art/pr2/left_arm/gripper/pose' ] # wait for message, set pose try: ps = rospy.wait_for_message(topics[idx], PoseStamped, timeout=2) except (rospy.ROSException) as e: rospy.logerr(str(e)) self.notif(translate("UICoreRos", "Failed to store gripper pose."), temp=False) return self.notif(translate("UICoreRos", "Gripper pose stored."), temp=False) self.program_vis.set_pose(ps) def touch_calibration_points_cb(self, req): resp = TouchCalibrationPointsResponse() pts = [] for pt in req.points: pts.append((pt.point.x, pt.point.y)) self.emit(QtCore.SIGNAL('touch_calibration_points_evt'), pts) self.touched_sub = rospy.Subscriber( '/art/interface/touchtable/touch_detected', Empty, self.touch_detected_cb, queue_size=10) resp.success = True return resp def touch_detected_evt(self, msg): if self.touch_points is None: return if not self.touch_points.next(): self.notif(translate("UICoreRos", "Touch saved."), temp=True) for it in self.scene.items(): if isinstance(it, LabelItem): continue # TODO fix this - in makes visible even items that are invisible by purpose it.setVisible(True) self.notif(translate("UICoreRos", "Touch table calibration finished."), temp=False) self.scene.removeItem(self.touch_points) self.touch_points = None self.touched_sub.unregister() else: self.notif(translate("UICoreRos", "Touch saved."), temp=True) self.notif(translate("UICoreRos", "Please press the next point."), temp=False) def touch_detected_cb(self, msg): self.emit(QtCore.SIGNAL('touch_detected_evt'), msg) def calibrate_projectors_cb(self, req): resp = TriggerResponse() resp.success = True # call to start_projector_calibration is blocking self.proj_calib_timer = rospy.Timer(rospy.Duration(0.001), self.start_projector_calibration, oneshot=True) return resp def notify_user_srv_cb(self, req): self.emit(QtCore.SIGNAL('notify_user_evt'), req) return NotifyUserResponse() def notify_user_evt(self, req): # TODO message should be displayed until user closes it if req.duration == rospy.Duration(0): self.notif(req.message, message_type=req.type) else: self.notif(req.message, min_duration=req.duration.to_sec(), temp=True, message_type=req.type) def stop_btn_clicked(self, btn): try: if self.emergency_stopped: self.emergency_stop_reset_srv.call() self.emergency_stopped = False self.stop_btn.set_caption("STOP") self.stop_btn.set_background_color(QtCore.Qt.red) self.notif(translate("UICoreRos", "Resetting motors"), temp=True) else: self.emergency_stop_srv.call() self.emergency_stopped = True self.stop_btn.set_caption("RUN") self.stop_btn.set_background_color(QtCore.Qt.green) self.notif(translate("UICoreRos", "Emergency stop pressed"), temp=True) except rospy.service.ServiceException: self.notif(translate("UICoreRos", "Failed to stop/run robot."), temp=True) def program_error_dialog_cb(self, idx): req = ProgramErrorResolveRequest() req.user_response_type = idx + 1 resp = None try: resp = self.program_error_resolve_srv(req) except rospy.ServiceException as e: rospy.logerr("Service call failed: " + str(e)) if resp is None or not resp.success: self.notif(translate("UICoreRos", "System failure: failed to resolve error."), temp=True) self.scene.removeItem(self.program_error_dialog) self.program_error_dialog = None def interface_state_evt(self, old_state, state, flags): system_state_changed = old_state.system_state != state.system_state if system_state_changed: rospy.logdebug("New system state: " + str(state.system_state) + ", was: " + str(old_state.system_state)) self.clear_all(True) if state.error_severity == InterfaceState.NONE and self.program_error_dialog is not None: # hide dialog self.scene.removeItem(self.program_error_dialog) self.program_error_dialog = None # display info/warning/error if there is any - only once (on change) if state.error_severity != old_state.error_severity: if state.error_severity == InterfaceState.INFO: self.notif(translate("UICoreRos", "Error occurred: ") + error_strings.get_error_string(state.error_code), temp=True) elif state.error_severity == InterfaceState.WARNING: # TODO translate error number to error message self.program_error_dialog = DialogItem( self.scene, self.width / 2, 0.1, translate("UICoreRos", "Handle error: ") + error_strings.get_error_string(state.error_code), [ translate("UICoreRos", "Try again"), translate("UICoreRos", "Skip instruction"), translate("UICoreRos", "Fail instruction"), translate("UICoreRos", "End program") ], self.program_error_dialog_cb) # TODO what to do with SEVERE? if state.system_state == InterfaceState.STATE_PROGRAM_FINISHED: if system_state_changed: self.notif(translate("UICoreRos", "The program is done.")) elif state.system_state == InterfaceState.STATE_IDLE: if system_state_changed: self.show_program_list() elif state.system_state == InterfaceState.STATE_LEARNING: self.state_learning(old_state, state, flags, system_state_changed) elif state.system_state in [ InterfaceState.STATE_PROGRAM_RUNNING, InterfaceState.STATE_PROGRAM_STOPPED ]: self.state_running(old_state, state, flags, system_state_changed) def interface_state_cb(self, old_state, state, flags): # print state self.emit(QtCore.SIGNAL('interface_state'), old_state, state, flags) def state_running(self, old_state, state, flags, system_state_changed): if system_state_changed: if not self.ph.load(self.art.load_program(state.program_id)): self.notif( translate("UICoreRos", "Failed to load program from database.")) # TODO what to do? return stopped = state.system_state == InterfaceState.STATE_PROGRAM_STOPPED self.show_program_vis(readonly=True, stopped=stopped) if stopped: self.notif(translate("UICoreRos", "Program paused."), temp=True) if not stopped and old_state.system_state == InterfaceState.STATE_PROGRAM_STOPPED: self.notif(translate("UICoreRos", "Program resumed."), temp=True) # ignore not valid states if state.block_id == 0 or state.program_current_item.id == 0: rospy.logerr("Invalid state!") return # TODO if the item id is same - do rather update then clear + add everything? self.clear_all() self.program_vis.set_active(state.block_id, state.program_current_item.id) it = state.program_current_item if it.type == ProgIt.GET_READY: self.notif(translate("UICoreRos", "Robot is getting ready")) elif it.type == ProgIt.WAIT_FOR_USER: self.notif(translate("UICoreRos", "Waiting for user")) elif it.type == ProgIt.WAIT_UNTIL_USER_FINISHES: self.notif(translate("UICoreRos", "Waiting for user to finish")) elif it.type == ProgIt.PICK_FROM_POLYGON: obj_id = None try: obj_id = flags["SELECTED_OBJECT_ID"] except KeyError: rospy.logerr( "PICK_FROM_POLYGON: SELECTED_OBJECT_ID flag not set") if obj_id is not None: self.select_object(obj_id) obj = self.get_object(obj_id) # TODO notif - object type if obj is not None: self.notif( translate("UICoreRos", "Going to pick object ID ") + obj_id + translate("UICoreRos", " of type ") + obj.object_type.name + translate("UICoreRos", " from polygon.")) self.add_polygon( translate("UICoreRos", "PICK POLYGON"), poly_points=conversions.get_pick_polygon_points(it), fixed=True) elif it.type == ProgIt.PICK_FROM_FEEDER: # TODO PICK_FROM_FEEDER pass elif it.type == ProgIt.PICK_OBJECT_ID: self.notif( translate("UICoreRos", "Picking object with ID=") + it.object[0]) self.select_object(it.object[0]) elif it.type == ProgIt.PLACE_TO_POSE: try: obj_id = flags["SELECTED_OBJECT_ID"] except KeyError: rospy.logerr("PLACE_TO_POSE: SELECTED_OBJECT_ID flag not set") return obj = self.get_object(obj_id) if obj is not None: self.add_place(translate("UICoreRos", "OBJECT PLACE POSE"), it.pose[0], obj.object_type, obj_id, fixed=True) elif it.type == ProgIt.PLACE_TO_GRID: ref_msg = self.program_vis.get_ref_item( it.ref_id) # obtaining reference instruction if ref_msg.type == ProgIt.PICK_OBJECT_ID: obj = self.get_object(ref_msg.object[0]) object_type = obj.object_type self.program_vis.get_current_item().object[0] = obj.object_id else: object_type = self.art.get_object_type(ref_msg.object[0]) self.program_vis.get_current_item( ).object[0] = ref_msg.object[0] self.notif( translate("UICoreRos", "Going to place object into grid")) self.add_square( translate("UICoreRos", "PLACE SQUARE GRID"), self.width / 2, self.height / 2, 0.1, 0.075, object_type, it.pose, grid_points=conversions.get_pick_polygon_points(it), square_changed=self.square_changed, fixed=True) def show_program_vis(self, readonly=False, stopped=False): rospy.logdebug("Showing ProgramItem with readonly=" + str(readonly) + ", stopped=" + str(stopped)) self.program_vis = ProgramItem( self.scene, self.last_prog_pos[0], self.last_prog_pos[1], self.ph, done_cb=self.learning_done_cb, item_switched_cb=self.active_item_switched, learning_request_cb=self.learning_request_cb, stopped=stopped, pause_cb=self.pause_cb, cancel_cb=self.cancel_cb) self.program_vis.set_readonly(readonly) def pause_cb(self): if self.state_manager.state.system_state == InterfaceState.STATE_PROGRAM_STOPPED: # TODO call trigger service method try: resp = self.program_resume_srv() except rospy.ServiceException: pass if resp is not None and resp.success: return True else: self.notif(translate("UICoreRos", "Failed to resume program."), temp=True) return False elif self.state_manager.state.system_state == InterfaceState.STATE_PROGRAM_RUNNING: try: resp = self.program_pause_srv() except rospy.ServiceException: pass if resp is not None and resp.success: self.notif(translate("UICoreRos", "Program paused."), temp=True) return True else: self.notif(translate("UICoreRos", "Failed to pause program."), temp=True) return True else: rospy.logdebug( "Attempt to pause/resume program in strange state: " + str(self.state_manager.state.system_state)) return False def cancel_cb(self): if self.state_manager.state.system_state in [ InterfaceState.STATE_PROGRAM_RUNNING, InterfaceState.STATE_PROGRAM_STOPPED ]: try: resp = self.program_stop_srv() except rospy.ServiceException: pass if resp is not None and resp.success: self.notif(translate("UICoreRos", "Program stopped."), temp=True) return True else: self.notif(translate("UICoreRos", "Failed to stop program."), temp=True) return True else: rospy.logdebug("Attempt to stop program in strange state: " + str(self.state_manager.state.system_state)) return False def clear_all(self, include_dialogs=False): rospy.logdebug("Clear all") super(UICoreRos, self).clear_all() if include_dialogs: for it in [self.program_list, self.program_vis]: if it is None: continue try: self.last_prog_pos = it.get_pos() except AttributeError: pass break for it in [ self.program_error_dialog, self.grasp_dialog, self.program_vis, self.program_list ]: if it is None: continue self.remove_scene_items_by_type(type(it)) it = None def state_learning(self, old_state, state, flags, system_state_changed): if system_state_changed: self.last_edited_prog_id = state.program_id if not self.ph.load(self.art.load_program(state.program_id)): self.notif( translate("UICoreRos", "Failed to load program from database.")) # TODO what to do? return if state.block_id != 0 and state.program_current_item.id != 0: # there may be unsaved changes - let's use ProgramItem from brain self.ph.set_item_msg(state.block_id, state.program_current_item) self.show_program_vis() if state.block_id == 0 or state.program_current_item.id == 0: rospy.logerr("Invalid state!") return if old_state.block_id != state.block_id or old_state.program_current_item.id != state.program_current_item.id: self.clear_all() # TODO overit funkcnost - pokud ma state novejsi timestamp nez nas - ulozit ProgramItem if old_state.timestamp == rospy.Time( 0 ) or old_state.timestamp - state.timestamp > rospy.Duration(0): rospy.logdebug('Got state with newer timestamp!') item = self.ph.get_item_msg(state.block_id, state.program_current_item.id) item = state.program_current_item self.clear_all() self.learning_vis(state.block_id, state.program_current_item.id, not state.edit_enabled) def learning_vis(self, block_id, item_id, read_only): if not self.ph.item_requires_learning(block_id, item_id): self.notif(translate("UICoreRos", "Item has no parameters.")) return self.program_vis.editing_item = not read_only # TODO Edit/Done button not visible when there is work in progress! if block_id != self.program_vis.block_id or item_id != self.program_vis.item_id: self.program_vis.set_active(block_id, item_id) msg = self.ph.get_item_msg(block_id, item_id) if self.ph.item_learned(block_id, item_id): self.notif( translate("UICoreRos", "This program item seems to be done")) else: if msg.type in [ ProgIt.PICK_FROM_POLYGON, ProgIt.PICK_FROM_FEEDER, ProgIt.PICK_OBJECT_ID, ProgIt.PLACE_TO_POSE ]: self.notif( translate("UICoreRos", "Program current manipulation task")) if msg.type == ProgIt.PICK_FROM_POLYGON: if not self.ph.is_object_set(block_id, item_id): self.notif(translate("UICoreRos", "Select object type to be picked up"), temp=True) else: self.select_object_type(msg.object[0]) if self.ph.is_polygon_set(block_id, item_id): self.add_polygon( translate("UICoreRos", "PICK POLYGON"), poly_points=conversions.get_pick_polygon_points(msg), polygon_changed=self.polygon_changed, fixed=read_only) elif msg.type == ProgIt.PICK_FROM_FEEDER: if self.state_manager.state.edit_enabled and self.grasp_dialog is None: self.grasp_dialog = DialogItem(self.scene, self.width / 2, 0.1, "Save gripper pose", ["Right arm", "Left arm"], self.save_gripper_pose_cb) if self.ph.is_object_set(block_id, item_id): self.select_object_type(msg.object[0]) else: self.notif(translate("UICoreRos", "Select object type to be picked up"), temp=True) # TODO show pick pose somehow (arrow??) elif msg.type == ProgIt.PICK_OBJECT_ID: if self.ph.is_object_set(block_id, item_id): self.select_object(msg.object[0]) else: self.notif(translate("UICoreRos", "Select object to be picked up"), temp=True) elif msg.type == ProgIt.PLACE_TO_POSE: if not self.ph.is_object_set(block_id, msg.ref_id[0]): self.notif( translate("UICoreRos", "Select object to be picked up in ID=") + str(msg.ref_id[0])) else: # TODO what to do with more than 1 reference? ref_msg = self.ph.get_item_msg(block_id, msg.ref_id[0]) if ref_msg.type == ProgIt.PICK_OBJECT_ID: obj = self.get_object(ref_msg.object[0]) object_type = obj.object_type object_id = obj.object_id self.select_object(ref_msg.object[0]) else: object_type = self.art.get_object_type(ref_msg.object[0]) object_id = None self.select_object_type(ref_msg.object[0]) if self.ph.is_object_set(block_id, msg.ref_id[0]): if self.ph.is_pose_set(block_id, item_id): if object_type is not None: self.add_place(translate("UICoreRos", "OBJECT PLACE POSE"), msg.pose[0], object_type, object_id, place_cb=self.place_pose_changed, fixed=read_only) else: self.notif( translate("UICoreRos", "Set where to place picked object")) self.add_place(translate("UICoreRos", "OBJECT PLACE POSE"), self.get_def_pose(), object_type, object_id, place_cb=self.place_pose_changed, fixed=read_only) elif msg.type == ProgIt.PLACE_TO_GRID: ref_msg = self.program_vis.get_ref_item( msg.ref_id) # obtaining reference instruction if ref_msg.type == ProgIt.PICK_OBJECT_ID: obj = self.get_object(ref_msg.object[0]) object_type = obj.object_type self.program_vis.get_current_item().object[0] = obj.object_id else: object_type = self.art.get_object_type(ref_msg.object[0]) self.program_vis.get_current_item( ).object[0] = ref_msg.object[0] self.notif(translate("UICoreRos", "Place grid")) self.add_square( translate("UICoreRos", "PLACE SQUARE GRID"), self.width / 2, self.height / 2, 0.1, 0.075, object_type, msg.pose, grid_points=conversions.get_pick_polygon_points(msg), square_changed=self.square_changed, fixed=read_only) def active_item_switched(self, block_id, item_id, read_only=True): rospy.logdebug("Program ID:" + str(self.ph.get_program_id()) + ", active item ID: " + str((block_id, item_id))) self.clear_all() if item_id is None: # TODO hlaska return self.learning_vis(block_id, item_id, read_only) self.state_manager.update_program_item( self.ph.get_program_id(), block_id, self.ph.get_item_msg(block_id, item_id)) def get_def_pose(self): ps = PoseStamped() ps.pose.position.x = self.width / 2 ps.pose.position.y = self.height / 2 ps.pose.orientation.w = 1.0 return ps def place_pose_changed(self, place): if self.program_vis.editing_item: self.program_vis.set_place_pose(place) self.state_manager.update_program_item( self.ph.get_program_id(), self.program_vis.block_id, self.program_vis.get_current_item()) def calib_done_cb(self, proj): if proj.is_calibrated(): self.calib_proj_cnt += 1 while self.calib_proj_cnt < len(self.projectors): if self.projectors[self.calib_proj_cnt].is_calibrated(): self.calib_proj_cnt += 1 continue self.projectors[self.calib_proj_cnt].calibrate( self.calib_done_cb) return rospy.loginfo('Projectors calibrated.') self.projectors_calibrated_pub.publish(True) else: # calibration failed - let's try again rospy.logerr('Calibration failed for projector: ' + proj.proj_id) proj.calibrate(self.calib_done_cb) def start_projector_calibration(self, evt): if len(self.projectors) == 0: rospy.loginfo('No projectors to calibrate.') self.projectors_calibrated_pub.publish(True) else: self.projectors_calibrated_pub.publish(False) rospy.loginfo('Starting calibration of ' + str(len(self.projectors)) + ' projector(s)') self.calib_proj_cnt = 0 for proj in self.projectors: if proj.is_calibrated(): self.calib_proj_cnt += 1 continue else: if not proj.calibrate(self.calib_done_cb): # TODO what to do? rospy.logerr("Failed to start projector calibration") return rospy.loginfo('Projectors calibrated.') self.projectors_calibrated_pub.publish(True) def is_template(self): return self.template def learning_done_cb(self): prog = self.ph.get_program() # if it is template - save it with new id if self.is_template(): self.template = False headers = self.art.get_program_headers() ids = [] for h in headers: ids.append(h.id) # is there a better way how to find not used ID for program? for i in range(0, 2**16 - 1): if i not in ids: prog.header.id = i break else: rospy.logerr("Failed to find available program ID") if not self.art.store_program(prog): self.notif(translate("UICoreRos", "Failed to store program"), temp=True) # TODO what to do? self.notif(translate("UICoreRos", "Program stored with ID=") + str(prog.header.id), temp=True) self.last_edited_prog_id = prog.header.id resp = None try: resp = self.stop_learning_srv() except rospy.ServiceException as e: print "Service call failed: %s" % e if resp is None or not resp.success: rospy.logwarn("Failed to stop learning mode.") return def program_selected_cb(self, prog_id, run=False, template=False): self.template = template if run: (started, error) = self.art.start_program(prog_id) if not started: self.notif(translate("UICoreRos", "Failed to start program.")) rospy.logerr("Brain refused to start program: " + error) return self.notif(translate("UICoreRos", "Starting program ID=" + str(prog_id)), temp=True) self.program_list.set_enabled(False) else: if not self.ph.load(self.art.load_program(prog_id), template): self.notif( translate("UICoreRos", "Failed to load program from database.")) # TODO what to do? return req = startProgramRequest() req.program_id = prog_id resp = None try: resp = self.start_learning_srv(req) except rospy.ServiceException as e: print "Service call failed: %s" % e if resp is None or not resp.success: self.notif(translate("UICoreRos", "Failed to start edit mode.")) # else: # self.clear_all() # self.show_program_vis() def learning_request_cb(self, req): if req == LearningRequestGoal.GET_READY: self.notif( translate("UICoreRos", "Robot is getting ready for learning")) elif req == LearningRequestGoal.DONE: self.notif( translate("UICoreRos", "Robot is getting into default state")) if self.grasp_dialog is not None: self.scene.removeItem(self.grasp_dialog) self.grasp_dialog = None elif req == LearningRequestGoal.EXECUTE_ITEM: self.notif( translate("UICoreRos", "Robot is executing current program instruction")) g = LearningRequestGoal() g.request = req self.learning_action_cl.send_goal( g, done_cb=self.learning_request_done_cb, feedback_cb=self.learning_request_feedback_cb) def learning_request_feedback_cb(self, fb): rospy.logdebug('learning request progress: ' + str(fb.progress)) def learning_request_done_evt(self, status, result): self.program_vis.learning_request_result(result.success) def learning_request_done_cb(self, status, result): self.emit(QtCore.SIGNAL('learning_request_done_evt'), status, result) def show_program_list(self): self.notif(translate("UICoreRos", "Please select a program")) headers = self.art.get_program_headers() d = {} headers_to_show = [] for header in headers: ph = ProgramHelper() d[header.id] = None if ph.load(self.art.load_program(header.id)): headers_to_show.append(header) d[header.id] = ph.program_learned() self.program_list = ProgramListItem(self.scene, self.last_prog_pos[0], self.last_prog_pos[1], headers_to_show, d, self.last_edited_prog_id, self.program_selected_cb) def object_cb(self, msg): self.emit(QtCore.SIGNAL('objects'), msg) def object_cb_evt(self, msg): for obj_id in msg.lost_objects: self.remove_object(obj_id) self.notif(translate("UICoreRos", "Object") + " ID=" + str(obj_id) + " " + translate("UICoreRos", "disappeared"), temp=True) for inst in msg.instances: obj = self.get_object(inst.object_id) if obj: obj.set_pos(inst.pose.position.x, inst.pose.position.y, inst.pose.position.z) obj.set_orientation(conversions.q2a(inst.pose.orientation)) else: obj_type = self.art.get_object_type(inst.object_type) self.add_object(inst.object_id, obj_type, inst.pose.position.x, inst.pose.position.y, inst.pose.position.z, conversions.q2a(inst.pose.orientation), self.object_selected) self.notif(translate("UICoreRos", "New object") + " ID=" + str(inst.object_id), temp=True) def polygon_changed(self, pts): if self.program_vis.editing_item: self.program_vis.set_polygon(pts) self.state_manager.update_program_item( self.ph.get_program_id(), self.program_vis.block_id, self.program_vis.get_current_item()) ''' Method which saves grid points and place poses of all objects in grid. ''' def square_changed(self, pts, poses=None): self.program_vis.set_place_grid( pts) # saving grid points into the ProgramItem message self.program_vis.set_place_poses( poses) # saving place poses into the ProgramItem message self.state_manager.update_program_item( self.ph.get_program_id(), self.program_vis.block_id, self.program_vis.get_current_item()) def object_selected(self, id, selected): if self.program_vis is None or not self.program_vis.editing_item: rospy.logdebug("not in edit mode") return False rospy.logdebug("attempt to select object id: " + id) obj = self.get_object(id) msg = self.program_vis.get_current_item() if msg is None: return False if msg.type in [ProgIt.PICK_FROM_FEEDER, ProgIt.PICK_FROM_POLYGON]: # this type of object is already set if len(msg.object) > 0 and obj.object_type.name == msg.object[0]: rospy.logdebug("object type " + obj.object_type.name + " already selected") return else: # TODO remove previously inserted polygon, do not insert new # place rospy.logdebug("selecting new object type: " + obj.object_type.name) pass if msg.type == ProgIt.PICK_FROM_FEEDER: self.program_vis.set_object(obj.object_type.name) self.select_object_type(obj.object_type.name) elif msg.type == ProgIt.PICK_OBJECT_ID: self.program_vis.set_object(obj.object_id) self.select_object(obj.object_id) elif msg.type == ProgIt.PICK_FROM_POLYGON: if obj.object_type.name not in self.selected_object_types: self.remove_scene_items_by_type(PolygonItem) poly_points = [] self.program_vis.set_object(obj.object_type.name) self.select_object_type(obj.object_type.name) for ob in self.get_scene_items_by_type(ObjectItem): if ob.object_type.name != obj.object_type.name: continue poly_points.append(ob.get_pos()) self.add_polygon(translate("UICoreRos", "PICK POLYGON"), poly_points, polygon_changed=self.polygon_changed) self.notif(translate("UICoreRos", "Check and adjust pick polygon"), temp=True) self.state_manager.update_program_item( self.ph.get_program_id(), self.program_vis.block_id, self.program_vis.get_current_item()) return True def user_status_cb(self, msg): self.emit(QtCore.SIGNAL('user_status'), msg) def user_status_cb_evt(self, msg): if msg.user_state != self.user_state: if msg.user_state == UserStatus.USER_NOT_CALIBRATED: self.notif( translate("UICoreRos", "Please do a calibration pose")) elif msg.user_state == UserStatus.USER_CALIBRATED: self.notif(translate("UICoreRos", "Successfully calibrated")) elif msg.user_state == UserStatus.NO_USER: self.notif(translate("UICoreRos", "Waiting for user...")) self.user_state = msg
def main(args): global setup rospy.init_node('setup_init_script', anonymous=True) try: setup = os.environ["ARTABLE_SETUP"] except KeyError: rospy.logerr("ARTABLE_SETUP has to be set!") return api = ArtApiHelper() api.wait_for_db_api() rospy.loginfo("Refreshing collision environment...") api.clear_collision_primitives(setup) api.store_object_type(obj_type("desticka", 0.08, 0.08, 0.0125)) api.store_object_type( obj_type("Modry_kontejner", 0.11, 0.165, 0.075, container=True)) # delete all created programs ph = api.get_program_headers() if ph: for h in ph: api.program_clear_ro(h.id) api.delete_program(h.id) # TODO add parameters prog = Program() prog.header.id = 1 prog.header.name = "MSV DEMO" pb = ProgramBlock() pb.id = 1 pb.name = "Pick and place" pb.on_success = 1 pb.on_failure = 0 prog.blocks.append(pb) pi = ProgramItem() pi.id = 1 pi.on_success = 2 pi.on_failure = 0 pi.type = "PickFromPolygon" pi.object.append("") pi.polygon.append(PolygonStamped()) pb.items.append(pi) pi = ProgramItem() pi.id = 2 pi.on_success = 1 pi.on_failure = 0 pi.type = "PlaceToContainer" pi.object.append("") pi.polygon.append(PolygonStamped()) pi.ref_id.append(1) pb.items.append(pi) api.store_program(prog) rospy.loginfo("Done!")
def main(args): rospy.init_node('simple_trolley_init_script', anonymous=True) art = ArtApiHelper() # art.wait_for_api() # delete all created programs ph = art.get_program_headers() if ph: for h in ph: art.program_clear_ro(h.id) art.delete_program(h.id) # ------------------------------------------------------------------------------------------- # Training program 1 # ------------------------------------------------------------------------------------------- prog = Program() prog.header.id = 1 prog.header.name = "Trenink - oblast" pb = ProgramBlock() pb.id = 1 pb.name = "Zvedni ze stolu a poloz" pb.on_success = 1 pb.on_failure = 0 prog.blocks.append(pb) pb.items.append(polygon_item(1)) pb.items.append(place_item(2, ref_id=[1], on_success=3, on_failure=0)) pb.items.append(item(3, "GetReady", on_success=4, on_failure=0)) pb.items.append(wait_item(4, ref_id=[2], on_success=1, on_failure=0)) art.store_program(prog) art.program_set_ro(prog.header.id) # ------------------------------------------------------------------------------------------- # Training program 2 # ------------------------------------------------------------------------------------------- prog = Program() prog.header.id = 2 prog.header.name = "Trenink - podavac" pb = ProgramBlock() pb.id = 1 pb.name = "Zvedni z podavace a poloz" pb.on_success = 1 pb.on_failure = 0 prog.blocks.append(pb) pb.items.append(feeder_item(1)) pb.items.append(place_item(2, ref_id=[1], on_success=3, on_failure=0)) pb.items.append(feeder_item(3, ref_id=[1])) pb.items.append(place_item(4, ref_id=[3], on_success=5, on_failure=0)) pb.items.append(item(5, "GetReady", on_success=6, on_failure=0)) pb.items.append(wait_item(6, ref_id=[4], on_success=1, on_failure=0)) art.store_program(prog) art.program_set_ro(prog.header.id) # ------------------------------------------------------------------------------------------- # Training program 3 # ------------------------------------------------------------------------------------------- prog = Program() prog.header.id = 3 prog.header.name = "Trenink - lepidlo" pb = ProgramBlock() pb.id = 1 pb.name = "Aplikace lepidla" pb.on_success = 0 pb.on_failure = 0 prog.blocks.append(pb) pb.items.append(drill_item(1, on_success=1, on_failure=2, obj_type=[""])) pb.items.append(item(2, "GetReady", on_success=3, on_failure=0)) pb.items.append(drill_item(3, on_success=3, on_failure=4, obj_type=[""])) pb.items.append(item(4, "GetReady", on_success=0, on_failure=0)) art.store_program(prog) art.program_set_ro(prog.header.id) # ------------------------------------------------------------------------------------------- # Training program 4 # ------------------------------------------------------------------------------------------- prog = Program() prog.header.id = 4 prog.header.name = "Trenink - inspekce" pb = ProgramBlock() pb.id = 1 pb.name = "Vizualni inspekce" pb.on_success = 0 pb.on_failure = 0 prog.blocks.append(pb) pb.items.append(polygon_item(1)) pb.items.append( visual_inspection_item(2, ref_id=[1], on_success=3, on_failure=5)) pb.items.append( visual_inspection_item(3, ref_id=[1], on_success=4, on_failure=5)) pb.items.append( place_item(4, ref_id=[1], on_success=1, on_failure=0, name="OK parts")) pb.items.append( place_item(5, ref_id=[1], on_success=1, on_failure=0, name="NOK parts")) art.store_program(prog) art.program_set_ro(prog.header.id) # ------------------------------------------------------------------------------------------- # Training program 5 # ------------------------------------------------------------------------------------------- prog = Program() prog.header.id = 5 prog.header.name = "Trenink - krabice" pb = ProgramBlock() pb.id = 1 pb.name = "Polozeni do krabice" pb.on_success = 0 pb.on_failure = 0 prog.blocks.append(pb) pb.items.append(polygon_item(1)) p = ProgramItem() p.id = 2 p.type = "PlaceToContainer" p.polygon.append(PolygonStamped()) p.object.append("") p.on_success = 1 p.on_failure = 0 p.ref_id.append(1) pb.items.append(p) art.store_program(prog) art.program_set_ro(prog.header.id) # ------------------------------------------------------------------------------------------- # Simplified trolley assembly: object types # ------------------------------------------------------------------------------------------- art.store_object_type(obj_type("Spojka", 0.046, 0.046, 0.154)) art.store_object_type( obj_type("Kratka_noha", 0.046, 0.046, 0.298, container=True)) art.store_object_type(obj_type("Dlouha_noha", 0.046, 0.046, 0.398)) art.store_object_type( obj_type("Modry_kontejner", 0.1, 0.14, 0.08, container=True)) # ------------------------------------------------------------------------------------------- # Simplified trolley assembly: program # ------------------------------------------------------------------------------------------- prog = Program() prog.header.id = 20 prog.header.name = "Montaz stolicky" # --- left side of the trolley ------------------------------------------------------ pb = ProgramBlock() pb.id = 1 pb.name = "Bocnice 1" pb.on_success = 2 pb.on_failure = 0 prog.blocks.append(pb) pb.items.append(wait_item(2, on_failure=2)) # each side consists of four profiles (of two types) pb.items.append(feeder_item(3, obj_type="")) pb.items.append(place_item(4, ref_id=[3], on_failure=4)) pb.items.append(feeder_item(5, ref_id=[3])) pb.items.append(place_item(6, ref_id=[5], on_failure=6)) pb.items.append(feeder_item(7, obj_type="")) pb.items.append(place_item(8, ref_id=[7], on_failure=8)) pb.items.append(feeder_item(9, ref_id=[7])) pb.items.append(place_item(10, ref_id=[9], on_failure=10)) # after p&p, let's drill holes pb.items.append(drill_item(11, on_success=11, on_failure=13, obj_type=[""])) pb.items.append(item(13, "GetReady", on_success=0, on_failure=13)) # --- right side of the trolley ------------------------------------------------------ pb = deepcopy(pb) pb.id = 2 pb.name = "Bocnice 2" pb.on_success = 3 pb.on_failure = 0 prog.blocks.append(pb) # --- connecting profiles ------------------------------------------------------------ pb = ProgramBlock() pb.id = 3 pb.name = "Spojovaci dily" pb.on_success = 1 pb.on_failure = 0 prog.blocks.append(pb) pb.items.append(wait_item(1, on_success=10, on_failure=1)) pb.items.append(feeder_item(10, obj_type="")) pb.items.append(place_item(11, ref_id=[10], on_failure=11)) pb.items.append(feeder_item(12, ref_id=[10])) pb.items.append(place_item(13, ref_id=[12], on_failure=13)) pb.items.append(feeder_item(14, ref_id=[10])) pb.items.append(place_item(15, ref_id=[14], on_failure=15)) pb.items.append(feeder_item(16, ref_id=[10])) pb.items.append(place_item(17, ref_id=[16], on_failure=17)) pb.items.append(item(18, "GetReady", on_success=0, on_failure=18)) art.store_program(prog) art.program_set_ro(prog.header.id)
def main(args): rospy.init_node('simple_trolley_init_script', anonymous=True) art = ArtApiHelper() # art.wait_for_api() # delete all created programs ph = art.get_program_headers() if ph: for h in ph: art.program_clear_ro(h.id) art.delete_program(h.id) # ------------------------------------------------------------------------------------------- # Training program 1 # ------------------------------------------------------------------------------------------- prog = Program() prog.header.id = 1 prog.header.name = "Training - polygon" pb = ProgramBlock() pb.id = 1 pb.name = "Pick from table and place" pb.on_success = 1 pb.on_failure = 0 prog.blocks.append(pb) pb.items.append(polygon_item(1)) pb.items.append(place_item(2, ref_id=[1], on_success=3, on_failure=0)) pb.items.append(item(3, "GetReady", on_success=4, on_failure=0)) pb.items.append(wait_item(4, ref_id=[2], on_success=1, on_failure=0)) art.store_program(prog) art.program_set_ro(prog.header.id) # ------------------------------------------------------------------------------------------- # Training program 2 # ------------------------------------------------------------------------------------------- prog = Program() prog.header.id = 2 prog.header.name = "Training - feeder" pb = ProgramBlock() pb.id = 1 pb.name = "Pick from feeder and place" pb.on_success = 1 pb.on_failure = 0 prog.blocks.append(pb) pb.items.append(feeder_item(1)) pb.items.append(place_item(2, ref_id=[1], on_success=3, on_failure=0)) pb.items.append(feeder_item(3, ref_id=[1])) pb.items.append(place_item(4, ref_id=[3], on_success=5, on_failure=0)) pb.items.append(item(5, "GetReady", on_success=6, on_failure=0)) pb.items.append(wait_item(6, ref_id=[4], on_success=1, on_failure=0)) art.store_program(prog) art.program_set_ro(prog.header.id) # ------------------------------------------------------------------------------------------- # Training program 3 # ------------------------------------------------------------------------------------------- prog = Program() prog.header.id = 3 prog.header.name = "Training - glue" pb = ProgramBlock() pb.id = 1 pb.name = "Glue application" pb.on_success = 0 pb.on_failure = 0 prog.blocks.append(pb) pb.items.append(drill_item(1, on_success=1, on_failure=2, obj_type=[""])) pb.items.append(item(2, "GetReady", on_success=3, on_failure=0)) pb.items.append(drill_item(3, on_success=3, on_failure=4, obj_type=[""])) pb.items.append(item(4, "GetReady", on_success=0, on_failure=0)) art.store_program(prog) art.program_set_ro(prog.header.id) # ------------------------------------------------------------------------------------------- # Simplified trolley assembly: object types # ------------------------------------------------------------------------------------------- art.store_object_type(obj_type("Stretcher", 0.046, 0.046, 0.154)) art.store_object_type(obj_type("ShortLeg", 0.046, 0.046, 0.298)) art.store_object_type(obj_type("LongLeg", 0.046, 0.046, 0.398)) # ------------------------------------------------------------------------------------------- # Simplified trolley assembly: program # ------------------------------------------------------------------------------------------- prog = Program() prog.header.id = 4 prog.header.name = "Stool assembly" # --- left side of the trolley ------------------------------------------------------ pb = ProgramBlock() pb.id = 1 pb.name = "Side 1" pb.on_success = 2 pb.on_failure = 0 prog.blocks.append(pb) pb.items.append(wait_item(2, on_failure=2)) # each side consists of four profiles (of two types) pb.items.append(feeder_item(3, obj_type="")) pb.items.append(place_item(4, ref_id=[3], on_failure=4)) pb.items.append(feeder_item(5, ref_id=[3])) pb.items.append(place_item(6, ref_id=[5], on_failure=6)) pb.items.append(feeder_item(7, obj_type="")) pb.items.append(place_item(8, ref_id=[7], on_failure=8)) pb.items.append(feeder_item(9, ref_id=[7])) pb.items.append(place_item(10, ref_id=[9], on_failure=10)) # after p&p, let's drill holes pb.items.append(drill_item(11, on_success=11, on_failure=13, obj_type=[""])) pb.items.append(item(13, "GetReady", on_success=0, on_failure=13)) # --- right side of the trolley ------------------------------------------------------ pb = deepcopy(pb) pb.id = 2 pb.name = "Side 2" pb.on_success = 3 pb.on_failure = 0 prog.blocks.append(pb) # --- connecting profiles ------------------------------------------------------------ pb = ProgramBlock() pb.id = 3 pb.name = "Stretchers" pb.on_success = 1 pb.on_failure = 0 prog.blocks.append(pb) pb.items.append(wait_item(1, on_success=10, on_failure=1)) pb.items.append(feeder_item(10, obj_type="")) pb.items.append(place_item(11, ref_id=[10], on_failure=11)) pb.items.append(feeder_item(12, ref_id=[10])) pb.items.append(place_item(13, ref_id=[12], on_failure=13)) pb.items.append(feeder_item(14, ref_id=[10])) pb.items.append(place_item(15, ref_id=[14], on_failure=15)) pb.items.append(feeder_item(16, ref_id=[10])) pb.items.append(place_item(17, ref_id=[16], on_failure=17)) pb.items.append(item(18, "GetReady", on_success=0, on_failure=18)) art.store_program(prog) art.program_set_ro(prog.header.id)
def main(args): global setup rospy.init_node('setup_init_script', anonymous=True) try: setup = os.environ["ARTABLE_SETUP"] except KeyError: rospy.logerr("ARTABLE_SETUP has to be set!") return api = ArtApiHelper() api.wait_for_db_api() rospy.loginfo("Refreshing collision environment...") api.clear_collision_primitives(setup) table_width = 1.5 table_depth = 0.7 api.add_collision_primitive( makePrimitive("table", [table_width, table_depth, 0.78], z=-0.78 / 2)) feeder_depth = 0.35 feeder_thickness = 0.001 feeder_front_to_table = 0.15 # left feeder (1) api.add_collision_primitive( makePrimitive("lf-front", [feeder_depth, feeder_thickness, 0.175], x=-feeder_depth / 2 - feeder_front_to_table, y=table_depth - 0.495)) api.add_collision_primitive( makePrimitive("lf-middle", [feeder_depth, feeder_thickness, 0.35], x=-feeder_depth / 2 - feeder_front_to_table, y=table_depth - 0.18)) api.add_collision_primitive( makePrimitive("lf-rear", [feeder_depth, feeder_thickness, 0.35], x=-feeder_depth / 2 - feeder_front_to_table, y=table_depth)) # right feeder (2) api.add_collision_primitive( makePrimitive("rf-front", [feeder_depth, feeder_thickness, 0.175], x=table_width + feeder_depth / 2 + feeder_front_to_table, y=table_depth - 0.495)) api.add_collision_primitive( makePrimitive("rf-middle", [feeder_depth, feeder_thickness, 0.35], x=table_width + feeder_depth / 2 + feeder_front_to_table, y=table_depth - 0.18)) api.add_collision_primitive( makePrimitive("rf-rear", [feeder_depth, feeder_thickness, 0.35], x=table_width + feeder_depth / 2 + feeder_front_to_table, y=table_depth)) api.add_collision_primitive( makePrimitive("kinect-n1", [0.3, 0.3, 0.3], x=0, y=0, z=0, frame_id="n1_kinect2_link")) api.add_collision_primitive( makePrimitive("kinect-n2", [0.3, 0.3, 0.3], x=0, y=0, z=0, frame_id="n2_kinect2_link")) api.store_object_type(obj_type("Stretcher", 0.046, 0.046, 0.154)) api.store_object_type(obj_type("ShortLeg", 0.046, 0.046, 0.298)) api.store_object_type(obj_type("LongLeg", 0.046, 0.046, 0.398)) api.store_object_type(obj_type("Spojka", 0.046, 0.046, 0.154)) api.store_object_type(obj_type("Kratka_noha", 0.046, 0.046, 0.298)) api.store_object_type(obj_type("Dlouha_noha", 0.046, 0.046, 0.398)) api.store_object_type( obj_type("Modry_kontejner", 0.11, 0.165, 0.075, container=True)) api.store_object_type( obj_type("Bily_kontejner_velky", 0.245, 0.37, 0.15, container=True)) api.store_object_type( obj_type("Bily_kontejner_maly", 0.18, 0.245, 0.15, container=True)) # delete all created programs ph = api.get_program_headers() if ph: for h in ph: api.program_clear_ro(h.id) api.delete_program(h.id) # TODO add parameters prog = Program() prog.header.id = 1 prog.header.name = "MSV DEMO" pb = ProgramBlock() pb.id = 1 pb.name = "Pick and place" pb.on_success = 2 pb.on_failure = 0 prog.blocks.append(pb) pi = ProgramItem() pi.id = 1 pi.on_success = 2 pi.on_failure = 0 pi.type = "PickFromFeeder" pi.object.append("") pi.pose.append(PoseStamped()) pb.items.append(pi) pi = ProgramItem() pi.id = 2 pi.on_success = 3 pi.on_failure = 0 pi.type = "PlaceToPose" pi.pose.append(PoseStamped()) pi.ref_id.append(1) pb.items.append(pi) pi = ProgramItem() pi.id = 3 pi.on_success = 0 pi.on_failure = 0 pi.type = "GetReady" pb.items.append(pi) pb = ProgramBlock() pb.id = 2 pb.name = "Inspect and sort" pb.on_success = 1 pb.on_failure = 0 prog.blocks.append(pb) pi = ProgramItem() pi.id = 1 pi.on_success = 2 pi.on_failure = 0 pi.type = "PickFromPolygon" pi.object.append("") pi.polygon.append(PolygonStamped()) pb.items.append(pi) pi = ProgramItem() pi.id = 2 pi.on_success = 3 pi.on_failure = 4 pi.type = "VisualInspection" pi.pose.append(PoseStamped()) pi.ref_id.append(1) pb.items.append(pi) pi = ProgramItem() pi.id = 3 pi.on_success = 5 pi.on_failure = 0 pi.type = "PlaceToContainer" pi.name = "OK" pi.object.append("") pi.polygon.append(PolygonStamped()) pi.ref_id.append(1) pb.items.append(pi) pi = ProgramItem() pi.id = 4 pi.on_success = 5 pi.on_failure = 0 pi.type = "PlaceToContainer" pi.name = "NOK" pi.object.append("") pi.polygon.append(PolygonStamped()) pi.ref_id.append(1) pb.items.append(pi) pi = ProgramItem() pi.id = 5 pi.on_success = 0 pi.on_failure = 0 pi.type = "GetReady" pb.items.append(pi) api.store_program(prog) rospy.loginfo("Done!")