def __init__(self):
self.detected_objects_pub = rospy.Publisher("/art_object_detector/object", InstancesArray, queue_size=10)
while not rospy.is_shutdown():
instances = InstancesArray()
obj_in = ObjInstance()
obj_in.object_id = str('box')
obj_in.pose.position.x = 0.5
obj_in.pose.position.y = 0.5
obj_in.pose.position.z = 0
obj_in.pose.orientation.x = 0
obj_in.pose.orientation.y = 0
obj_in.pose.orientation.z = 0
obj_in.pose.orientation.w = 1.0
obj_in.bbox.dimensions = [0.05, 0.05, 0.1]
obj_in.bbox.type = SolidPrimitive.BOX
instances.header.frame_id = "/marker"
instances.instances.append(obj_in)
self.detected_objects_pub.publish(instances)
time.sleep(1)
pass
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 getRandomObject():
tmp = ObjInstance()
tmp.object_id = "object_" + str(random.randint(1,10000))
tmp.pose.position.x = random.uniform(0.5, 0.9)
tmp.pose.position.y = random.uniform(-0.8, 0.8)
tmp.pose.position.z = 0.74 + 0.1 # vyska stolu + pulka kosticky
tmp.pose.orientation.x = 0.0
tmp.pose.orientation.y = 0.0
tmp.pose.orientation.z = 0.0
tmp.pose.orientation.w = 1.0
tmp.bbox = SolidPrimitive()
tmp.bbox.type = SolidPrimitive.BOX
tmp.bbox.dimensions.append(0.05)
tmp.bbox.dimensions.append(0.05)
tmp.bbox.dimensions.append(0.2)
return tmp
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 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)
def inst(self):
inst = ObjInstance()
inst.object_id = self.object_id
inst.object_type = self.object_type
w = []
px = []
py = []
pz = []
r = []
p = []
y = []
for frame_id, val in self.meas.iteritems():
if len(val) < 2:
continue
for idx in range(0, len(val)):
v = val[idx]
# distance normalized to 0, 1
d = (v[0] - self.min_dist) / (self.max_dist - self.min_dist)
# weight based on distance from object to sensor
w_dist = (1.0 - d) ** 2 # (0, 1)
# newer detections are more interesting
w_age = (float(idx) / (len(val) - 1)) / 2 + 0.5 # (0.5, 1)
w.append(w_dist * w_age)
px.append(v[1].pose.position.x)
py.append(v[1].pose.position.y)
pz.append(v[1].pose.position.z)
rpy = transformations.euler_from_quaternion(q2a(v[1].pose.orientation))
r.append([cos(rpy[0]), sin(rpy[0])])
p.append([cos(rpy[1]), sin(rpy[1])])
y.append([cos(rpy[2]), sin(rpy[2])])
if len(w) < self.min_meas_cnt:
return None
inst.pose.position.x = np.average(px, weights=w)
inst.pose.position.y = np.average(py, weights=w)
inst.pose.position.z = np.average(pz, weights=w)
ar = np.average(r, axis=0, weights=w)
ap = np.average(p, axis=0, weights=w)
ay = np.average(y, axis=0, weights=w)
fr = atan2(ar[1], ar[0])
fp = atan2(ap[1], ap[0])
fy = atan2(ay[1], ay[0])
cur_rpy = [fr, fp, fy]
# TODO hysteresis!
# TODO is there a smarter way how to do it?
# "fix" roll and pitch so they are only 0, 90, 180 or 270 degrees
# yaw (in table coordinates) may stay as it is
for i in range(0, 2):
if cur_rpy[i] < 0.0:
cur_rpy[i] = 2 * pi + cur_rpy[i]
if cur_rpy[i] >= 7 * pi / 4 or cur_rpy[i] < pi / 4:
cur_rpy[i] = 0.0
elif cur_rpy[i] >= pi / 4 and cur_rpy[i] < 3 * pi / 4:
cur_rpy[i] = pi / 2
elif cur_rpy[i] >= 3 * pi / 4 and cur_rpy[i] < 5 * pi / 4:
cur_rpy[i] = pi
elif cur_rpy[i] >= 5 * pi / 4 and cur_rpy[i] < 7 * pi / 4:
cur_rpy[i] = 3 * pi / 2
a2q(inst.pose.orientation, transformations.quaternion_from_euler(*cur_rpy))
return inst
def main():
pub = rospy.Publisher("/art_object_detector/object_filtered",
InstancesArray)
client = actionlib.SimpleActionClient('/pr2_pick_place_left/pp',
art_msgs.msg.pickplaceAction)
client.wait_for_server()
arr = InstancesArray()
arr.header.frame_id = "base_footprint"
arr.header.stamp = rospy.Time.now()
obj = ObjInstance()
obj.object_id = "my_object"
obj.pose.position.x = random.uniform(0.4, 0.7)
obj.pose.position.y = random.uniform(-0.2, 0.5)
obj.pose.position.z = 0.74 + 0.1 # vyska stolu + pulka kosticky
obj.pose.orientation.x = 0.0
obj.pose.orientation.y = 0.0
obj.pose.orientation.z = 0.0
obj.pose.orientation.w = 1.0
obj.bbox = SolidPrimitive()
obj.bbox.type = SolidPrimitive.BOX
obj.bbox.dimensions.append(0.05)
obj.bbox.dimensions.append(0.05)
obj.bbox.dimensions.append(0.2)
arr.instances.append(obj)
arr.instances.append(getRandomObject())
arr.instances.append(getRandomObject())
pub.publish(arr)
rospy.sleep(2.0)
goal = art_msgs.msg.pickplaceGoal()
goal.id = "my_object"
goal.operation = goal.PICK_AND_PLACE
goal.z_axis_angle_increment = (2 * 3.14) / 360 * 90
goal.keep_orientation = False
goal.place_pose = PoseStamped()
goal.place_pose.header.frame_id = "base_footprint"
goal.place_pose.header.stamp = rospy.Time.now()
goal.place_pose.pose.position.x = random.uniform(0.4, 0.7)
goal.place_pose.pose.position.y = random.uniform(-0.2, 0.5)
goal.place_pose.pose.position.z = 0.74 + 0.1
goal.place_pose.pose.orientation.x = 0.0
goal.place_pose.pose.orientation.y = 0.0
goal.place_pose.pose.orientation.z = 0.0
goal.place_pose.pose.orientation.w = 1.0
rospy.loginfo('sending goal')
client.send_goal(goal)
client.wait_for_result()
rospy.loginfo('got result')
print client.get_result()
print "status: " + client.get_goal_status_text()
print "state: " + str(client.get_state())
def main():
pub = rospy.Publisher("/art_object_detector/object_filtered", InstancesArray)
client = actionlib.SimpleActionClient('/pr2_pick_place_left/pp', art_msgs.msg.pickplaceAction)
client.wait_for_server()
arr = InstancesArray()
arr.header.frame_id = "base_footprint"
arr.header.stamp = rospy.Time.now()
obj = ObjInstance()
obj.object_id = "my_object"
obj.pose.position.x = random.uniform(0.4, 0.7)
obj.pose.position.y = random.uniform(-0.2, 0.5)
obj.pose.position.z = 0.74 + 0.1 # vyska stolu + pulka kosticky
obj.pose.orientation.x = 0.0
obj.pose.orientation.y = 0.0
obj.pose.orientation.z = 0.0
obj.pose.orientation.w = 1.0
obj.bbox = SolidPrimitive()
obj.bbox.type = SolidPrimitive.BOX
obj.bbox.dimensions.append(0.05)
obj.bbox.dimensions.append(0.05)
obj.bbox.dimensions.append(0.2)
arr.instances.append(obj)
arr.instances.append(getRandomObject())
arr.instances.append(getRandomObject())
pub.publish(arr)
rospy.sleep(2.0)
goal = art_msgs.msg.pickplaceGoal()
goal.id = "my_object"
goal.operation = goal.PICK_AND_PLACE
goal.z_axis_angle_increment = (2*3.14)/360*90
goal.keep_orientation = False
goal.place_pose = PoseStamped()
goal.place_pose.header.frame_id = "base_footprint"
goal.place_pose.header.stamp = rospy.Time.now()
goal.place_pose.pose.position.x = random.uniform(0.4, 0.7)
goal.place_pose.pose.position.y = random.uniform(-0.2, 0.5)
goal.place_pose.pose.position.z = 0.74 + 0.1
goal.place_pose.pose.orientation.x = 0.0
goal.place_pose.pose.orientation.y = 0.0
goal.place_pose.pose.orientation.z = 0.0
goal.place_pose.pose.orientation.w = 1.0
rospy.loginfo('sending goal')
client.send_goal(goal)
client.wait_for_result()
rospy.loginfo('got result')
print client.get_result()
print "status: " + client.get_goal_status_text()
print "state: " + str(client.get_state())
def main():
pub = rospy.Publisher("/art/object_detector/object_filtered", InstancesArray, queue_size=10)
pub_point_left = rospy.Publisher("/art/user/pointing_left", PoseStamped, queue_size=10)
pub_point_right = rospy.Publisher("/art/user/pointing_right", PoseStamped, queue_size=10)
arr = InstancesArray()
arr.header.frame_id = "marker"
arr.header.stamp = rospy.Time.now()
obj = ObjInstance()
obj.object_id = "my_object"
obj.object_type = "object"
obj.pose.position.x = 0.5
obj.pose.position.y = 0.5
obj.pose.position.z = 0.0
obj.pose.orientation.x = 0.0
obj.pose.orientation.y = 0.0
obj.pose.orientation.z = 0.0
obj.pose.orientation.w = 1.0
obj.bbox = SolidPrimitive()
obj.bbox.type = SolidPrimitive.BOX
obj.bbox.dimensions.append(0.05)
obj.bbox.dimensions.append(0.05)
obj.bbox.dimensions.append(0.2)
arr.instances.append(obj)
obj2 = ObjInstance()
obj2.object_id = "another_object"
obj2.object_type = "object"
obj2.pose.position.x = 0.7
obj2.pose.position.y = 0.3
obj2.pose.position.z = 0.0
obj2.pose.orientation.x = 0.0
obj2.pose.orientation.y = 0.0
obj2.pose.orientation.z = 0.0
obj2.pose.orientation.w = 1.0
obj2.bbox = SolidPrimitive()
obj2.bbox.type = SolidPrimitive.BOX
obj2.bbox.dimensions.append(0.05)
obj2.bbox.dimensions.append(0.05)
obj2.bbox.dimensions.append(0.2)
arr.instances.append(obj2)
arr.new_objects.append(obj.object_id)
arr.new_objects.append(obj2.object_id)
ps = PoseStamped()
ps.header.stamp = rospy.Time.now()
ps.header.frame_id = "marker"
ps.pose.position.x = 0.0
ps.pose.position.y = 0.5
ps.pose.position.z = 0
ps.pose.orientation.x = 0.0
ps.pose.orientation.y = 0.0
ps.pose.orientation.z = 0.0
ps.pose.orientation.w = 1.0
psr = PoseStamped()
psr.header.stamp = rospy.Time.now()
psr.header.frame_id = "marker"
psr.pose.position.x = 0.0
psr.pose.position.y = 0.3
psr.pose.position.z = 0
psr.pose.orientation.x = 0.0
psr.pose.orientation.y = 0.0
psr.pose.orientation.z = 0.0
psr.pose.orientation.w = 1.0
noise = 0.0001
rospy.sleep(2.0)
pub.publish(arr)
rospy.sleep(1.0)
arr.new_objects = []
while(not rospy.is_shutdown()):
pub.publish(arr)
rospy.sleep(1.0)
while(not rospy.is_shutdown()):
if psr.pose.position.x < 0.8:
psr.pose.position.x += 0.003
else:
psr.pose.position.x = 0
psr.pose.position.y = 0.3
pub_point_right.publish(psr)
if ps.pose.position.x < 0.8:
ps.pose.position.x += 0.002
else:
ps.pose.position.x = 0
ps.pose.position.y = 0.5
if isclose(ps.pose.position.x, 0.5):
for i in range(0, 300):
pub.publish(arr)
pub_point_left.publish(ps)
rospy.sleep(0.01)
if isclose(ps.pose.position.x, 0.6):
for i in range(0, 300):
pub.publish(arr)
pub_point_left.publish(ps)
rospy.sleep(0.01)
pub.publish(arr)
pub_point_left.publish(ps)
rospy.sleep(0.01)
def inst(self):
inst = ObjInstance()
inst.object_id = self.object_id
inst.object_type = self.object_type
w = []
px = []
py = []
pz = []
r = []
p = []
y = []
for frame_id, val in self.meas.iteritems():
if len(val) < 2:
continue
for idx in range(0, len(val)):
v = val[idx]
# distance normalized to 0, 1
d = (v[0] - self.min_dist) / (self.max_dist - self.min_dist)
# weight based on distance from object to sensor
w_dist = (1.0 - d) ** 2 # (0, 1)
# newer detections are more interesting
w_age = (float(idx) / (len(val) - 1)) / 2 + 0.5 # (0.5, 1)
w.append(w_dist * w_age)
px.append(v[1].pose.position.x)
py.append(v[1].pose.position.y)
pz.append(v[1].pose.position.z)
rpy = transformations.euler_from_quaternion(q2a(v[1].pose.orientation))
r.append([cos(rpy[0]), sin(rpy[0])])
p.append([cos(rpy[1]), sin(rpy[1])])
y.append([cos(rpy[2]), sin(rpy[2])])
if len(w) < self.min_meas_cnt:
return None
inst.pose.position.x = np.average(px, weights=w)
inst.pose.position.y = np.average(py, weights=w)
inst.pose.position.z = np.average(pz, weights=w)
# TODO read table size from param
inst.on_table = 0 < inst.pose.position.x < 1.5 and 0 < inst.pose.position.x < 0.7
ar = np.average(r, axis=0, weights=w)
ap = np.average(p, axis=0, weights=w)
ay = np.average(y, axis=0, weights=w)
fr = atan2(ar[1], ar[0])
fp = atan2(ap[1], ap[0])
fy = atan2(ay[1], ay[0])
cur_rpy = [fr, fp, fy]
a2q(inst.pose.orientation, transformations.quaternion_from_euler(*cur_rpy))
# TODO "fix" roll and pitch so they are only 0, 90, 180 or 270 degrees
# yaw (in table coordinates) may stay as it is
for (key, value) in self.flags.iteritems():
kv = KeyValue()
kv.key = key
kv.value = value
inst.flags.append(kv)
return inst
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 main():
pub = rospy.Publisher("/art/object_detector/object_filtered",
InstancesArray,
queue_size=10)
pub_point_left = rospy.Publisher("/art/user/pointing_left",
PoseStamped,
queue_size=10)
pub_point_right = rospy.Publisher("/art/user/pointing_right",
PoseStamped,
queue_size=10)
br = tf.TransformBroadcaster()
arr = InstancesArray()
arr.header.frame_id = "marker"
obj = ObjInstance()
obj.object_id = "3"
obj.object_type = "profile_20_60"
obj.pose.position.x = 0.5
obj.pose.position.y = 0.5
obj.pose.position.z = 0.025
obj.pose.orientation = yaw2orientation(90, 0, 0)
arr.instances.append(obj)
obj2 = ObjInstance()
obj2.object_id = "4"
obj2.object_type = "profile_20_60"
obj2.pose.position.x = 0.7
obj2.pose.position.y = 0.2
obj2.pose.position.z = 0.08
obj2.pose.orientation = yaw2orientation(0, 0, 45 / 2)
arr.instances.append(obj2)
obj3 = ObjInstance()
obj3.object_id = "even_another_object"
obj3.object_type = "profile_20_60"
obj3.pose.position.x = 0.9
obj3.pose.position.y = 0.3
obj3.pose.position.z = 0.025
obj3.pose.orientation = yaw2orientation(90, 90, 45)
arr.instances.append(obj3)
arr.new_objects.append(obj.object_id)
arr.new_objects.append(obj2.object_id)
arr.new_objects.append(obj3.object_id)
rospy.sleep(1.0)
pub.publish(arr)
rospy.sleep(1.0)
arr.new_objects = []
while not rospy.is_shutdown():
arr.header.stamp = rospy.Time.now()
pub.publish(arr)
for obj in arr.instances:
br.sendTransform((obj.pose.position.x, obj.pose.position.y,
obj.pose.position.z),
(obj.pose.orientation.x, obj.pose.orientation.y,
obj.pose.orientation.z, obj.pose.orientation.w),
arr.header.stamp, "object_id_" + obj.object_id,
"marker")
rospy.sleep(0.25)
