def __init__(self):
self.object_publisher = rospy.Publisher(
'/art/object_detector/object_filtered',
InstancesArray,
queue_size=10,
latch=True)
self.objects = []
obj = ObjInstance()
obj.object_id = "profile_20_60"
obj.object_type = "profile_20_60"
obj.pose = Pose()
obj.pose.position.x = 0.3
obj.pose.position.y = 0.3
obj.pose.position.z = 0
obj.pose.orientation.x = 0
obj.pose.orientation.y = 0
obj.pose.orientation.z = 0
obj.pose.orientation.w = 1
self.objects.append(obj)
obj = ObjInstance()
obj.object_id = "profile_21_60"
obj.object_type = "profile_20_60"
obj.pose = Pose()
obj.pose.position.x = 0.75
obj.pose.position.y = 0.58
obj.pose.position.z = 0
obj.pose.orientation.x = 0
obj.pose.orientation.y = 0
obj.pose.orientation.z = 0
obj.pose.orientation.w = 1
self.objects.append(obj)
def __init__(self):
self.object_publisher = rospy.Publisher(
'/art/object_detector/object_filtered',
InstancesArray,
queue_size=10,
latch=True)
self.srv_set_flag = rospy.Service('/art/object_detector/flag/set',
ObjectFlagSet, self.set_cb)
self.srv_clear_flag = rospy.Service('/art/object_detector/flag/clear',
ObjectFlagClear, self.clear_cb)
self.srv_clear_all_flags = rospy.Service(
'/art/object_detector/flag/clear_all', Empty, self.empty_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.empty_cb)
self.srv_disable_forearm = rospy.Service(
'/art/object_detector/forearm/disable', Empty, self.empty_cb)
self.srv_enable_detection = rospy.Service(
'/art/object_detector/all/enable', Empty, self.empty_cb)
self.srv_disable_detection = rospy.Service(
'/art/object_detector/all/disable', Empty, self.empty_cb)
self.objects = []
obj = ObjInstance()
obj.object_id = "profile_20_60"
obj.object_type = "profile_20_60"
obj.pose = Pose()
obj.pose.position.x = 0.3
obj.pose.position.y = 0.3
obj.pose.position.z = 0
obj.pose.orientation.x = 0
obj.pose.orientation.y = 0
obj.pose.orientation.z = 0
obj.pose.orientation.w = 1
self.objects.append(obj)
obj = ObjInstance()
obj.object_id = "profile_21_60"
obj.object_type = "profile_20_60"
obj.pose = Pose()
obj.pose.position.x = 0.75
obj.pose.position.y = 0.58
obj.pose.position.z = 0
obj.pose.orientation.x = 0
obj.pose.orientation.y = 0
obj.pose.orientation.z = 0
obj.pose.orientation.w = 1
self.objects.append(obj)
def get_pick_obj_from_feeder(obj_type):
assert isinstance(obj_type, str)
obj = ObjInstance()
obj.object_id = None
obj.object_type = obj_type
obj.pose = Pose()
return obj
def ar_code_cb(self, data):
rospy.logdebug("New arcodes arrived:")
instances = InstancesArray()
id = 0
for arcode in data.markers:
aid = int(arcode.id)
# list of allowed object ids
# TODO load from param
if aid not in [4, 5, 3, 21, 26, 31]:
continue
if aid not in self.objects_cache:
# TODO AR code ID / object type assignment should be done somewhere...
# type "profile_20_60" is just for example (used in art_db/test_db.py)
if aid == 21:
object_type = self.art.get_object_type(
"profile_20_60_longer")
else:
object_type = self.art.get_object_type("profile_20_60")
if object_type is None:
# error or unknown object - let's ignore it
self.objects_cache[aid] = None
continue
self.objects_cache[aid] = {
'type': object_type.name,
'bb': object_type.bbox
}
# skip unknown objects
if self.objects_cache[aid] is None:
continue
obj_in = ObjInstance()
obj_in.object_id = str(aid)
obj_in.object_type = self.objects_cache[aid]['type']
obj_in.pose = arcode.pose.pose
self.show_rviz_bb(arcode.header.frame_id, obj_in, arcode.id,
self.objects_cache[aid]['bb'])
instances.header.stamp = arcode.header.stamp
instances.header.frame_id = arcode.header.frame_id
instances.instances.append(obj_in)
++id
if len(data.markers) == 0:
rospy.logdebug("Empty")
else:
# print instances
self.detected_objects_pub.publish(instances)
def ar_code_cb(self, data):
rospy.logdebug("New arcodes arrived:")
instances = InstancesArray()
id = 0
for arcode in data.markers:
aid = int(arcode.id)
if aid not in self.objects_cache:
try:
resp = self.get_object_srv(obj_id=aid)
except rospy.ServiceException, e:
# error or unknown object - let's ignore it
self.objects_cache[aid] = None
continue
self.objects_cache[aid] = {'name': resp.name, 'model_url': resp.model_url, 'type': resp.type, 'bb': resp.bbox}
if self.objects_cache[aid] is None: continue
obj_in = ObjInstance()
obj_in.object_id = self.objects_cache[aid]['name']
obj_in.object_type = self.objects_cache[aid]['type']
obj_in.pose = arcode.pose.pose
angles = transformations.euler_from_quaternion([obj_in.pose.orientation.x,
obj_in.pose.orientation.y,
obj_in.pose.orientation.z,
obj_in.pose.orientation.w])
q = transformations.quaternion_from_euler(0, 0, angles[2])
obj_in.pose.orientation.x = q[0]
obj_in.pose.orientation.y = q[1]
obj_in.pose.orientation.z = q[2]
obj_in.pose.orientation.w = q[3]
obj_in.bbox = self.objects_cache[aid]['bb']
obj_in.pose.position.z = float(obj_in.bbox.dimensions[2]/2)
self.show_rviz_bb(obj_in, arcode.id)
instances.header.stamp = arcode.header.stamp
instances.header.frame_id = arcode.header.frame_id
instances.instances.append(obj_in)
++id
def timer_cb(self, event):
ia = InstancesArray()
ia.header.frame_id = self.target_frame
ia.header.stamp = rospy.Time.now()
objects_to_prune = []
now = rospy.Time.now()
for k, v in self.objects.iteritems():
if (now - v["pose"].header.stamp) > self.max_age:
objects_to_prune.append(k)
ia.lost_objects.append(k)
continue
if v["cnt"] < self.min_cnt:
continue
obj = ObjInstance()
obj.pose = v["pose"].pose
obj.pose.orientation = self.normalize(obj.pose.orientation)
obj.object_id = k
obj.object_type = v["object_type"]
ia.instances.append(obj)
if v["cnt"] == self.min_cnt:
ia.new_objects.append(k)
# TODO also publish TF for each object???
self.pub.publish(ia)
for k in objects_to_prune:
rospy.loginfo("Object " + k + " no longer visible")
del self.objects[k]
if len(objects_to_prune) > 0:
rospy.loginfo("Pruned " + str(len(objects_to_prune)) + " objects")
def get_pick_obj_from_feeder(instruction):
obj = ObjInstance()
obj.object_id = None
obj.object_type = instruction.object[0]
obj.pose = Pose()
return obj
def ar_code_cb(self, data):
rospy.logdebug("New arcodes arrived:")
instances = InstancesArray()
id = 0
for arcode in data.markers:
aid = int(arcode.id)
# list of allowed object ids
# TODO load from param
if aid not in [
50, 51, 52, 53, 54, 55, 56, 57, 60, 61, 62, 63, 1001, 1002,
1003, 1004, 1005, 1006, 1007, 1008, 1009, 1010, 1011, 1012,
1013, 1014, 1015, 1016, 1017, 1018, 1019, 1020, 1021, 1022,
1023, 1024, 1025, 2001, 2002, 2003, 2004, 2005, 2006, 2007,
2008, 3001, 3002, 3003, 3004
]:
continue
if aid not in self.objects_cache:
# TODO AR code ID / object type assignment should be done somewhere...
# type "profile_20_60" is just for example (used in art_db/test_db.py)
object_type = None
if aid in [
50, 51, 52, 53, 54, 55, 56, 57, 1001, 1002, 1003, 1004,
1005, 1006, 1007, 1008, 1009, 1010, 1011, 1012, 1013,
1014, 1015, 1016, 1017, 1018, 1019, 1020, 1021, 1022,
1023, 1024, 1025
]:
object_type = self.art.get_object_type("Spojka")
elif aid in [
60, 61, 62, 63, 2001, 2002, 2003, 2004, 2005, 2006,
2007, 2008
]:
object_type = self.art.get_object_type("Kratka_noha")
elif aid in [60, 61, 62, 63, 3001, 3002, 3003, 3004]:
object_type = self.art.get_object_type("Dlouha_noha")
if object_type is None:
# error or unknown object - let's ignore it
self.objects_cache[aid] = None
continue
self.objects_cache[aid] = {
'type': object_type.name,
'bb': object_type.bbox
}
# skip unknown objects
if self.objects_cache[aid] is None:
continue
obj_in = ObjInstance()
obj_in.object_id = str(aid)
obj_in.object_type = self.objects_cache[aid]['type']
obj_in.pose = arcode.pose.pose
self.show_rviz_bb(arcode.header.frame_id, obj_in, arcode.id,
self.objects_cache[aid]['bb'])
instances.header.stamp = arcode.header.stamp
instances.header.frame_id = arcode.header.frame_id
instances.instances.append(obj_in)
++id
if len(data.markers) == 0:
rospy.logdebug("Empty")
else:
# print instances
self.detected_objects_pub.publish(instances)
