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 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.bbox = v["bbox"]
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 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)
