class CollisionEnv(object): NS = "/art/collision_env/" def __init__(self, setup, world_frame): assert setup != "" self.ready = False self.setup = setup self.world_frame = world_frame self.tf_listener = TransformListener() self.api = ArtApiHelper() rospy.loginfo("Waiting for DB API") self.api.wait_for_db_api() self.ignored_prefixes = array_from_param("~ignored_prefixes") rospy.loginfo("Will ignore following prefixes: " + str(self.ignored_prefixes)) self.lock = RLock() self.ps = PlanningSceneInterface() self._paused = False self.oh = ObjectHelper(self.object_cb) self.artificial_objects = {} self.collision_objects_pub = rospy.Publisher(self.NS + "artificial", CollisionObjects, latch=True, queue_size=1) self.pub_artificial() self.timer = rospy.Timer(rospy.Duration(1.0), self.timer_cb) self.paused_pub = rospy.Publisher(self.NS + "paused", Bool, latch=True, queue_size=1) self.paused = False def start(self): self.ready = True rospy.loginfo("Ready") def load_from_db(self): for prim in self.api.get_collision_primitives(self.setup): rospy.loginfo("Loading object: " + prim.name) self.add_primitive(prim) def save_primitive(self, name): with self.lock: if name not in self.artificial_objects: raise CollisionEnvException("Unknown object name") p = self.artificial_objects[name] self.api.add_collision_primitive(p) def save_primitives(self): with self.lock: for name in self.artificial_objects.keys(): self.save_primitive(name) def set_primitive_pose(self, name, ps): with self.lock: p = self.artificial_objects[name] assert p.pose.header.frame_id == ps.header.frame_id p.pose.pose = ps.pose self.ps.add_box(p.name, p.pose, p.bbox.dimensions) self.pub_artificial() def add_primitive(self, p): with self.lock: self.artificial_objects[p.name] = p self.ps.add_box(p.name, p.pose, p.bbox.dimensions) self.pub_artificial() def pub_artificial(self): msg = CollisionObjects() for v in self.artificial_objects.values(): # transform all collision primitives into world frame (= marker) if v.pose.header.frame_id != self.world_frame: try: self.tf_listener.waitForTransform(v.pose.header.frame_id, self.world_frame, rospy.Time(0), rospy.Duration(1.0)) v.pose = self.tf_listener.transformPose( self.world_frame, v.pose) except tf.Exception as e: rospy.logwarn("Failed to transform artificial object: " + str(e)) continue msg.primitives.append(v) self.collision_objects_pub.publish(msg) @property def paused(self): return self._paused @paused.setter def paused(self, val): self._paused = val self.paused_pub.publish(val) def remove_name(self, name): with self.lock: if name not in self.artificial_objects: rospy.logwarn("Unknown artificial object: " + name) return False self.ps.remove_world_object(name) del self.artificial_objects[name] self.pub_artificial() if not self.api.clear_collision_primitives(self.setup, names=[name]): rospy.logwarn("Failed to remove from permanent storage") rospy.loginfo("Removed object: " + name) return True def clear_all(self, permanent=True): with self.lock: rospy.loginfo("Clearing " + str(len(self.artificial_objects)) + " artificial objects...") for k in self.artificial_objects.keys(): if self.is_ignored(k): continue self.ps.remove_world_object(k) self.artificial_objects = {} self.pub_artificial() try: if permanent and not self.api.clear_collision_primitives( self.setup): rospy.logwarn("Failed to remove from permanent storage") except ArtApiException as e: rospy.logerr(str(e)) def reload(self): self.clear_all(permanent=False) self.load_from_db() self.pub_artificial() def _generate_name(self): # as we use only part of uuid, there might be collisions... while True: name = str(uuid.uuid4())[:8] if name not in self.artificial_objects.keys(): break return name def set_det_pose(self, name, ps): object_type = self.api.get_object_type( self.oh.objects[name].object_type) if object_type is not None: self.add_detected(name, ps, object_type) def clear_det_on_table(self, inv=False, ignore=None): if ignore is None: ignore = [] ret = [] with self.lock: for v in self.oh.objects.values(): if v.object_id in ignore: continue if not inv and not v.on_table: continue if inv and v.on_table: continue ret.append(v.object_id) self.clear_detected(v.object_id) return ret def is_ignored(self, name): for ip in self.ignored_prefixes: if name.startswith(ip): return True return False def timer_cb(self, evt): if self.paused: return with self.lock: known_objects = self.ps.get_known_object_names() for name in known_objects: if name not in self.artificial_objects and name not in self.oh.objects and not self.is_ignored( name): rospy.loginfo("Removing outdated object: " + name) self.clear_detected(name) # restore artificial objects if they are lost somehow (e.g. by restart of MoveIt!) for k, v in self.artificial_objects.iteritems(): if k in known_objects: continue rospy.loginfo("Restoring artificial object: " + v.name) if v.bbox.type == SolidPrimitive.BOX: self.ps.add_box(v.name, v.pose, v.bbox.dimensions) else: # TODO other types pass def object_cb(self, evt, h, inst): if self.paused or not self.ready: return with self.lock: attached_objects = self.ps.get_attached_objects() if evt in (ObjectHelper.OBJECT_ADDED, ObjectHelper.OBJECT_UPDATED): if inst.object_id in attached_objects: return ps = PoseStamped() ps.header = h ps.pose = inst.pose object_type = self.api.get_object_type(inst.object_type) if object_type is not None: self.add_detected(inst.object_id, ps, object_type) elif evt == ObjectHelper.OBJECT_LOST: if inst.object_id not in attached_objects: self.clear_detected(inst.object_id) def add_detected(self, name, ps, object_type): with self.lock: self.ps.add_box(name, ps, object_type.bbox.dimensions) def clear_detected(self, name): with self.lock: self.ps.remove_world_object(name) def clear_all_det(self, ignore=None): if ignore is None: ignore = [] ret = [] with self.lock: for v in self.oh.objects.values(): name = v.object_id if name in ignore: continue self.clear_detected(name) ret.append(name) rospy.loginfo("Removed " + str(len(ret)) + " detected objects.") return ret def get_attached(self, transform_to_world=True): """ keep in mind - attached objects might not be detected """ ret = [] with self.lock: ao = self.ps.get_attached_objects() for k, v in ao.iteritems(): if len(v.object.primitives) != 1 or len( v.object.primitive_poses) != 1: rospy.logwarn("Unsupported type of attached object: " + k) continue if v.object.primitives[0].type != SolidPrimitive.BOX: rospy.logwarn( "Only box-like attached objects are supported so far.") continue ps = PoseStamped() ps.header = v.object.header ps.pose = v.object.primitive_poses[0] if transform_to_world and ps.header.frame_id != self.world_frame: try: self.tf_listener.waitForTransform(ps.header.frame_id, self.world_frame, ps.header.stamp, rospy.Duration(1.0)) ps = self.tf_listener.transformPose(self.world_frame, ps) except tf.Exception as e: rospy.logwarn("Failed to transform attached object: " + str(e)) continue ret.append((k, ps, v.object.primitives[0])) return ret
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): 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!")