def setUp(self): self.tfl = tf.TransformListener() rospy.sleep(2.0) self.world_frame = "marker" self.object_id = "21" api = ArtApiHelper() api.wait_for_db_api() api.store_object_type(obj_type("fake_object_type", 0.046, 0.046, 0.154))
class ArtSimpleTracker: def __init__(self, target_frame="marker"): self.target_frame = target_frame self.tfl = tf.TransformListener() self.lock = threading.Lock() self.detection_enabled = True self.use_forearm_cams = False self.table_size = array_from_param("/art/conf/table/size", float, 2, wait=True) self.ground_objects_on_table = rospy.get_param("~ground_objects_on_table", False) self.yaw_only_on_table = rospy.get_param("~yaw_only_on_table", False) self.ground_bb_axis = rospy.get_param("~ground_bb_axis", SolidPrimitive.BOX_Z) if self.ground_objects_on_table: rospy.loginfo("Objects on table will be grounded.") self.api = ArtApiHelper() self.api.wait_for_db_api() self.meas_max_age = rospy.Duration(5.0) self.prune_timer = rospy.Timer(rospy.Duration(1.0), self.prune_timer_cb) self.objects = {} self.br = tf.TransformBroadcaster() self.sub = rospy.Subscriber( "/art/object_detector/object", InstancesArray, self.cb, queue_size=1) self.pub = rospy.Publisher( "/art/object_detector/object_filtered", InstancesArray, queue_size=1, latch=True) self.timer = rospy.Timer(rospy.Duration(0.1), self.timer_cb) self.srv_set_flag = rospy.Service('/art/object_detector/flag/set', ObjectFlagSet, self.srv_set_flag_cb) self.srv_clear_flag = rospy.Service('/art/object_detector/flag/clear', ObjectFlagClear, self.srv_clear_flag_cb) self.srv_clear_all_flags = rospy.Service('/art/object_detector/flag/clear_all', Empty, self.srv_clear_all_flags_cb) self.forearm_cams = ("/l_forearm_cam_optical_frame", "/r_forearm_cam_optical_frame") self.srv_enable_forearm = rospy.Service('/art/object_detector/forearm/enable', Empty, self.srv_enable_forearm_cb) self.srv_disable_forearm = rospy.Service('/art/object_detector/forearm/disable', Empty, self.srv_disable_forearm_cb) self.srv_enable_detection = rospy.Service('/art/object_detector/all/enable', Empty, self.srv_enable_detection_cb) self.srv_disable_detection = rospy.Service('/art/object_detector/all/disable', Empty, self.srv_disable_detection_cb) def srv_enable_forearm_cb(self, req): rospy.loginfo("Enabling forearm cameras.") self.use_forearm_cams = True return EmptyResponse() def srv_disable_forearm_cb(self, req): rospy.loginfo("Disabling forearm cameras.") self.use_forearm_cams = False with self.lock: for object_id, obj in self.objects.iteritems(): for cf in self.forearm_cams: try: del obj.meas[cf] except KeyError: pass return EmptyResponse() def srv_enable_detection_cb(self, req): rospy.loginfo("Enabling object detection.") self.detection_enabled = True return EmptyResponse() def srv_disable_detection_cb(self, req): rospy.loginfo("Disabling object detection.") self.detection_enabled = False with self.lock: for object_id, obj in self.objects.iteritems(): for cf in self.forearm_cams: try: del obj.meas[cf] except KeyError: pass return EmptyResponse() def srv_clear_all_flags_cb(self, req): with self.lock: for k, v in self.objects.iteritems(): v.flags = {} return EmptyResponse() def srv_clear_flag_cb(self, req): with self.lock: resp = ObjectFlagClearResponse() if req.object_id not in self.objects: resp.success = False resp.error = "Unknown object" return resp if req.key not in self.objects[req.object_id].flags: resp.success = False resp.error = "Unknown key" return resp del self.objects[req.object_id].flags[req.key] resp.success = True return resp def srv_set_flag_cb(self, req): with self.lock: # TODO should flag be remembered even if object is lost and then detected again? resp = ObjectFlagSetResponse() if req.object_id not in self.objects: resp.success = False resp.error = "Unknown object" return resp self.objects[req.object_id].flags[req.flag.key] = req.flag.value resp.success = True return resp def prune_timer_cb(self, event): with self.lock: now = rospy.Time.now() for k, v in self.objects.iteritems(): v.prune_meas(now, self.meas_max_age) def timer_cb(self, event): with self.lock: ia = InstancesArray() ia.header.frame_id = self.target_frame ia.header.stamp = rospy.Time.now() objects_to_delete = [] for k, v in self.objects.iteritems(): inst = v.inst(self.table_size, self.ground_objects_on_table, self.ground_bb_axis, self.yaw_only_on_table) if inst is None: # new object might not have enough measurements yet # TODO fix it: this would keep objects which were detected only few times if not v.new and not v.lost: # object is no longer detected v.lost = True objects_to_delete.append(k) ia.lost_objects.append(k) continue continue if v.new: v.new = False ia.new_objects.append(k) ia.instances.append(inst) self.br.sendTransform((inst.pose.position.x, inst.pose.position.y, inst.pose.position.z), q2a(inst.pose.orientation), ia.header.stamp, "object_id_" + inst.object_id, self.target_frame) # commented out in order to keep object flags even if object is lost for some time # for obj_id in objects_to_delete: # del self.objects[obj_id] self.pub.publish(ia) def cb(self, msg): if not self.detection_enabled: return if not self.use_forearm_cams and msg.header.frame_id in self.forearm_cams: return if msg.header.frame_id == self.target_frame: rospy.logwarn_throttle(1.0, "Some detections are already in target frame!") return with self.lock: for inst in msg.instances: if inst.object_id in self.objects: rospy.logdebug("Updating object: " + inst.object_id) else: object_type = self.api.get_object_type(inst.object_type) if not object_type: rospy.logerr("Unknown object type: " + inst.object_type) continue rospy.loginfo("Adding new object: " + inst.object_id) self.objects[inst.object_id] = TrackedObject(self.target_frame, self.tfl, inst.object_id, object_type) ps = PoseStamped() ps.header = msg.header ps.pose = inst.pose self.objects[inst.object_id].add_meas(ps)
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!")