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!")