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