def updateScene(self, remove_collision = False): if remove_collision: for name in self.scene.getKnownCollisionObjects(): self.scene.removeCollisionObject(name, False) for name in self.scene.getKnownAttachedObjects(): self.scene.removeAttachedObject(name, False) self.scene.waitForSync() return # find objects self.cubes = [] goal = FindGraspableObjectsGoal() goal.plan_grasps = True self.find_client.send_goal(goal) self.find_client.wait_for_result(rospy.Duration(15.0)) find_result = self.find_client.get_result() # rospy.loginfo(find_result) # remove previous objects for name in self.scene.getKnownCollisionObjects(): self.scene.removeCollisionObject(name, False) for name in self.scene.getKnownAttachedObjects(): self.scene.removeAttachedObject(name, False) self.scene.waitForSync() # insert objects to scene idx = -1 print(find_result.objects)
def updateScene(self):
# find objects
goal = FindGraspableObjectsGoal()
goal.plan_grasps = True
self.find_client.send_goal(goal)
self.find_client.wait_for_result(rospy.Duration(5.0))
find_result = self.find_client.get_result()
# remove previous objects
for name in self.scene.getKnownCollisionObjects():
self.scene.removeCollisionObject(name, False)
for name in self.scene.getKnownAttachedObjects():
self.scene.removeAttachedObject(name, False)
self.scene.waitForSync()
# insert objects to scene
objects = list()
idx = -1
for obj in find_result.objects:
idx += 1
obj.object.name = "object%d" % idx
# self.scene.addSolidPrimitive(obj.object.name,
# obj.object.primitives[0],
# obj.object.primitive_poses[0],
# wait = False)
self.scene.addSolidPrimitive(obj.object.name,
obj.object.primitives[0],
obj.object.primitive_poses[0])
if obj.object.primitive_poses[0].position.x < 0.85:
objects.append([obj, obj.object.primitive_poses[0].position.z])
for obj in find_result.support_surfaces:
# extend surface to floor, and make wider since we have narrow field of view
height = obj.primitive_poses[0].position.z
obj.primitives[0].dimensions = [
obj.primitives[0].dimensions[0],
1.5, # wider
obj.primitives[0].dimensions[2] + height
]
obj.primitive_poses[0].position.z += -height / 2.0
# add to scene
# self.scene.addSolidPrimitive(obj.name,
# obj.primitives[0],
# obj.primitive_poses[0],
# wait = False)
self.scene.addSolidPrimitive(obj.name, obj.primitives[0],
obj.primitive_poses[0])
self.scene.waitForSync()
# store for grasping
#self.objects = find_result.objects
self.surfaces = find_result.support_surfaces
# store graspable objects by Z
objects.sort(key=lambda object: object[1])
objects.reverse()
self.objects = [object[0] for object in objects]
def updateScene(self, remove_collision=False):
if remove_collision:
for name in self.scene.getKnownCollisionObjects():
self.scene.removeCollisionObject(name, False)
for name in self.scene.getKnownAttachedObjects():
self.scene.removeAttachedObject(name, False)
self.scene.waitForSync()
return
# find objects
self.cubes = []
goal = FindGraspableObjectsGoal()
goal.plan_grasps = True
self.find_client.send_goal(goal)
self.find_client.wait_for_result(rospy.Duration(5.0))
find_result = self.find_client.get_result()
# remove previous objects
for name in self.scene.getKnownCollisionObjects():
self.scene.removeCollisionObject(name, False)
for name in self.scene.getKnownAttachedObjects():
self.scene.removeAttachedObject(name, False)
self.scene.waitForSync()
# insert objects to scene
idx = -1
for obj in find_result.objects:
print(idx)
idx += 1
obj.object.name = "object%d" % idx
self.scene.addSolidPrimitive(obj.object.name,
obj.object.primitives[0],
obj.object.primitive_poses[0],
wait=False)
self.cubes.append(obj.object.primitive_poses[0])
for obj in find_result.support_surfaces:
# extend surface to floor, and make wider since we have narrow field of view
height = obj.primitive_poses[0].position.z
obj.primitives[0].dimensions = [
obj.primitives[0].dimensions[0],
1.5, # wider
obj.primitives[0].dimensions[2] + height
]
obj.primitive_poses[0].position.z += -height / 2.0
# add to scene
self.scene.addSolidPrimitive(obj.name,
obj.primitives[0],
obj.primitive_poses[0],
wait=False)
self.scene.waitForSync()
# store for grasping
self.objects = find_result.objects
self.surfaces = find_result.support_surfaces
def updateScene(self):
# find objects
rospy.loginfo("get new grasps")
goal = FindGraspableObjectsGoal()
goal.plan_grasps = True
self.find_client.send_goal(goal)
self.find_client.wait_for_result(rospy.Duration(5.0))
find_result = self.find_client.get_result()
# remove previous objects
for name in self.scene.getKnownCollisionObjects():
self.scene.removeCollisionObject(name, False)
for name in self.scene.getKnownAttachedObjects():
self.scene.removeAttachedObject(name, False)
self.scene.removeCollisionObject("my_front_ground")
self.scene.removeCollisionObject("my_back_ground")
self.scene.removeCollisionObject("my_right_ground")
self.scene.removeCollisionObject("my_left_ground")
self.scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
self.scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
self.scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
self.scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
# TODO: ADD BOX
self.scene.waitForSync()
# insert objects to scene
idx = -1
for obj in find_result.objects:
idx += 1
obj.object.name = "object%d" % idx
self.scene.addSolidPrimitive(obj.object.name,
obj.object.primitives[0],
obj.object.primitive_poses[0],
wait=False)
for obj in find_result.support_surfaces:
# extend surface to floor, and make wider since we have narrow field of view
height = obj.primitive_poses[0].position.z
# added + .1
obj.primitives[0].dimensions = [
obj.primitives[0].dimensions[0],
1.5, # wider
obj.primitives[0].dimensions[2] + height
]
obj.primitive_poses[0].position.z += -height / 2.0
# add to scene
self.scene.addSolidPrimitive(obj.name,
obj.primitives[0],
obj.primitive_poses[0],
wait=False)
self.scene.waitForSync()
# store for grasping
self.objects = find_result.objects
self.surfaces = find_result.support_surfaces
def updateScene(self):
# find objects
rospy.loginfo("get new grasps")
goal = FindGraspableObjectsGoal()
goal.plan_grasps = True
self.find_client.send_goal(goal)
self.find_client.wait_for_result(rospy.Duration(5.0))
find_result = self.find_client.get_result()
# remove previous objects
for name in self.scene.getKnownCollisionObjects():
self.scene.removeCollisionObject(name, False)
for name in self.scene.getKnownAttachedObjects():
self.scene.removeAttachedObject(name, False)
self.scene.removeCollisionObject("my_front_ground")
self.scene.removeCollisionObject("my_back_ground")
self.scene.removeCollisionObject("my_right_ground")
self.scene.removeCollisionObject("my_left_ground")
self.scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
self.scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
self.scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
self.scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
# TODO: ADD BOX
self.scene.waitForSync()
# insert objects to scene
idx = -1
for obj in find_result.objects:
idx += 1
obj.object.name = "object%d"%idx
self.scene.addSolidPrimitive(obj.object.name,
obj.object.primitives[0],
obj.object.primitive_poses[0],
wait = False)
for obj in find_result.support_surfaces:
# extend surface to floor, and make wider since we have narrow field of view
height = obj.primitive_poses[0].position.z
# added + .1
obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0],
1.5, # wider
obj.primitives[0].dimensions[2] + height]
obj.primitive_poses[0].position.z += -height/2.0
# add to scene
self.scene.addSolidPrimitive(obj.name,
obj.primitives[0],
obj.primitive_poses[0],
wait = False)
self.scene.waitForSync()
# store for grasping
self.objects = find_result.objects
self.surfaces = find_result.support_surfaces
def updateScene(self):
# find objects
goal = FindGraspableObjectsGoal()
goal.plan_grasps = True
print " find goal"
self.find_client.send_goal(goal)
self.find_client.wait_for_result(rospy.Duration(5.0))
print " find client"
find_result = self.find_client.get_result()
# remove previous objects
for name in self.scene.getKnownCollisionObjects():
print "the object %s should be removed" % (name)
self.scene.removeCollisionObject(name, False)
for name in self.scene.getKnownAttachedObjects():
print "the object %s should be removed" % (name)
self.scene.removeAttachedObject(name, False)
self.scene.waitForSync()
# insert objects to scene
idx = -1
for obj in find_result.objects:
idx += 1
obj.object.name = "object%d" % idx
print "-----------------------"
print obj.object.primitive_poses[0]
print " x: %d" % (obj.object.primitive_poses[0].position.x)
obj.object.name = "object%d" % idx
self.scene.addSolidPrimitive(obj.object.name,
obj.object.primitives[0],
obj.object.primitive_poses[0],
use_service=True)
for obj in find_result.support_surfaces:
# extend surface to floor, and make wider since we have narrow field of view
height = obj.primitive_poses[0].position.z
obj.primitives[0].dimensions = [
obj.primitives[0].dimensions[0],
1.5, # wider
obj.primitives[0].dimensions[2] + height
]
obj.primitive_poses[0].position.z += -height / 2.0
# add to scene
self.scene.addSolidPrimitive(obj.name,
obj.primitives[0],
obj.primitive_poses[0],
use_service=True)
self.scene.waitForSync()
# store for grasping
self.objects = find_result.objects
self.surfaces = find_result.support_surfaces
def updateScene(self):
# find objects
goal = FindGraspableObjectsGoal()
goal.plan_grasps = True
self.find_client.send_goal(goal)
self.find_client.wait_for_result(rospy.Duration(5.0))
find_result = self.find_client.get_result()
rospy.loginfo("Found %d objects" % len(find_result.objects))
# remove previous objects
#for name in self.scene.getKnownCollisionObjects():
# self.scene.removeCollisionObject(name, False)
#for name in self.scene.getKnownAttachedObjects():
# self.scene.removeAttachedObject(name, False)
self.scene.waitForSync()
# insert objects to scene
objects = list()
idx = -1
for obj in find_result.objects:
idx += 1
#obj.object.name = find_result.objects[idx].name -> 'GraspableObject' object has no attribute 'name
obj.object.name = "obj%d" % idx
self.scene.addSolidPrimitive(obj.object.name,
obj.object.primitives[0],
obj.object.primitive_poses[0],
use_service=False)
if obj.object.primitive_poses[0].position.x < 1.3: #< 0.85:
objects.append([obj, obj.object.primitive_poses[0].position.y
]) ##should be it's goal position,,,
#objects.append([obj, obj.object.primitive_poses[0].position.z])
#for obj in find_result.support_surfaces:
# # extend surface to floor, and make wider since we have narrow field of view
# height = obj.primitive_poses[0].position.z
# obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0], 1.5, # wider
# obj.primitives[0].dimensions[2] + height]
# obj.primitive_poses[0].position.z += -height/2.0
# # add to scene
# self.scene.addSolidPrimitive(obj.name, obj.primitives[0], obj.primitive_poses[0])
self.scene.waitForSync()
# store for grasping
self.surfaces = find_result.support_surfaces
# store graspable objects by Z -> y
objects.sort(key=lambda object: object[1])
#objects.reverse()
self.objects = [object[0] for object in objects]
def updateScene(self):
# find objects
goal = FindGraspableObjectsGoal()
goal.plan_grasps = True
self.find_client.send_goal(goal)
self.find_client.wait_for_result(rospy.Duration(5.0))
find_result = self.find_client.get_result()
# remove previous objects
for name in self.scene.getKnownCollisionObjects():
self.scene.removeCollisionObject(name, False)
for name in self.scene.getKnownAttachedObjects():
self.scene.removeAttachedObject(name, False)
self.scene.waitForSync()
# insert objects to scene
idx = -1
for obj in find_result.objects:
idx += 1
obj.object.name = "object%d" % idx
self.scene.addSolidPrimitive(
obj.object.name, obj.object.primitives[0], obj.object.primitive_poses[0], wait=False
)
# Try mesh representation
# idx = -1
# for obj in find_result.objects:
# idx += 1
# obj.object.name = "object%d"%idx
# self.scene.addMesh(obj.object.name,
# obj.object.mesh_poses[0],
# obj.object.meshes[0],
# wait = False)
for obj in find_result.support_surfaces:
# extend surface to floor, and make wider since we have narrow field of view
height = obj.primitive_poses[0].position.z
obj.primitives[0].dimensions = [
obj.primitives[0].dimensions[0] + 0.1,
2.5, # wider
obj.primitives[0].dimensions[2] + height,
]
obj.primitive_poses[0].position.z += -height / 2.0
# add to scene
self.scene.addSolidPrimitive(obj.name, obj.primitives[0], obj.primitive_poses[0], wait=False)
self.scene.waitForSync()
# store for grasping
self.objects = find_result.objects
self.surfaces = find_result.support_surfaces
def updateScene(self):
# find objects
goal = FindGraspableObjectsGoal()
goal.plan_grasps = True
self.find_client.send_goal(goal)
self.find_client.wait_for_result(rospy.Duration(5.0))
find_result = self.find_client.get_result()
# remove previous objects
for name in self.scene.getKnownCollisionObjects():
self.scene.removeCollisionObject(name, wait=False)
for name in self.scene.getKnownAttachedObjects():
self.scene.removeAttachedObject(name, wait=False)
rospy.sleep(0.5) # Gets rid of annoying error messages stemming from race condition.
self.scene.waitForSync()
# insert objects to scene
idx = -1
for obj in find_result.objects:
idx += 1
obj.object.name = "object%d"%idx
self.scene.addSolidPrimitive(obj.object.name,
obj.object.primitives[0],
obj.object.primitive_poses[0],
wait = False)
for obj in find_result.support_surfaces:
# extend surface to floor, and make wider since we have narrow field of view
height = obj.primitive_poses[0].position.z
obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0] + 0.1,
1.5, # wider
obj.primitives[0].dimensions[2] + height]
obj.primitive_poses[0].position.z += -height/2.0
# add to scene
self.scene.addSolidPrimitive(obj.name,
obj.primitives[0],
obj.primitive_poses[0],
wait = False)
self.scene.waitForSync()
# store for grasping
self.objects = find_result.objects
self.surfaces = find_result.support_surfaces
def updateScene(self):
# find objects
goal = FindGraspableObjectsGoal()
goal.plan_grasps = True
print " find goal"
self.find_client.send_goal(goal)
self.find_client.wait_for_result(rospy.Duration(5.0))
print " find client"
find_result = self.find_client.get_result()
# remove previous objects
for name in self.scene.getKnownCollisionObjects():
print "the object %s should be removed" %(name)
self.scene.removeCollisionObject(name, False)
for name in self.scene.getKnownAttachedObjects():
print "the object %s should be removed" %(name)
self.scene.removeAttachedObject(name, False)
self.scene.waitForSync()
# insert objects to scene
objects = list()
idx = -1
for obj in find_result.objects:
idx += 1
obj.object.name = "object%d"%idx
print "-----------------------"
print obj.object.primitive_poses[0]
print " x: %d" %(obj.object.primitive_poses[0].position.x)
# if obj.object.primitive_poses[0].position.y > 0.0
self.scene.addSolidPrimitive(obj.object.name,
obj.object.primitives[0],
obj.object.primitive_poses[0],
use_service = False)
# def addSolidPrimitive (self, name, solid, pose, use_service=True)
print "1, %d" %(idx)
if obj.object.primitive_poses[0].position.x < 1.25:
objects.append([obj, obj.object.primitive_poses[0].position.z])
for obj in find_result.support_surfaces:
# extend surface to floor, and make wider since we have narrow field of view
print "2,"
height = obj.primitive_poses[0].position.z
obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0],
1.5, # wider
obj.primitives[0].dimensions[2] + height]
obj.primitive_poses[0].position.z += -height/2.0
# add to scene
self.scene.addSolidPrimitive(obj.name,
obj.primitives[0],
obj.primitive_poses[0],
use_service = True
)
self.scene.waitForSync()
# store for grasping
#self.objects = find_result.objects
self.surfaces = find_result.support_surfaces
# store graspable objects by Z
objects.sort(key=lambda object: object[1])
objects.reverse()
self.objects = [object[0] for object in objects]
rospy.loginfo("!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!")
rospy.loginfo("number of objects...:::::::" + str(len(objects)))
def updateScene(self):
# detect objects
goal = FindGraspableObjectsGoal()
goal.plan_grasps = False
self.find_client.send_goal(goal)
self.find_client.wait_for_result(rospy.Duration(5.0))
find_result = self.find_client.get_result()
#remove original objects
self.clear_scene()
# all the objects in scene
self.objects = list()
# height is used for grasping selection
self.height = -1
# object number
object_num = -1
# add support surface to scene
for obj in find_result.support_surfaces:
# extend surface to floor
h = obj.primitive_poses[0].position.z
if (h + obj.primitives[0].dimensions[2] / 2.0) > self.height:
self.height = h + obj.primitives[0].dimensions[2] / 2.0
self.table = obj
obj.primitives[0].dimensions = [
obj.primitives[0].dimensions[0] + 0.02,
obj.primitives[0].dimensions[1] + 0.02,
obj.primitives[0].dimensions[2] + h - 0.02
]
obj.primitive_poses[0].position.z += -h / 2.0
# add to scene
self.scene.addSolidPrimitive(obj.name,
obj.primitives[0],
obj.primitive_poses[0],
wait=False)
# add objects Solid to scene
for obj in find_result.objects:
object_num += 1
obj.object.name = "object%d" % object_num
self.scene.addSolidPrimitive(obj.object.name,
obj.object.primitives[0],
obj.object.primitive_poses[0],
wait=False)
if obj.object.primitive_poses[0].position.z > self.height:
self.objects.append([
obj,
numpy.array([
obj.object.primitive_poses[0].position.x,
obj.object.primitive_poses[0].position.y,
obj.object.primitive_poses[0].position.z
]), obj.object.primitive_poses[0].position.x
])
# localize basket
if obj.object.primitive_poses[0].position.y > self.table.primitive_poses[0].position.y + self.table.primitives[0].dimensions[1] / 2.0 + 0.1 or \
obj.object.primitive_poses[0].position.y < self.table.primitive_poses[0].position.y - self.table.primitives[0].dimensions[1] / 2.0 - 0.1:
if obj.object.primitives[0].dimensions[
1] > 0.20 and obj.object.primitive_poses[
0].position.z > 0.3 and not self.basket_found:
if self.basket_search_t == 1:
self.basket_pos_x = obj.object.primitive_poses[
0].position.x
self.basket_pos_y = obj.object.primitive_poses[
0].position.y
self.basket_search_t += 1
elif self.basket_search_t == 2:
if self.basket_pos_x - obj.object.primitive_poses[0].position.x < 0.04 and \
self.basket_pos_y - obj.object.primitive_poses[0].position.y < 0.04:
self.basket_found = True
if obj.object.primitive_poses[0].position.y < 0:
self.basket_pos = 'right'
self.basket = obj
self.marker_pub2.publish(self.basket_marker())
rospy.loginfo("Basket is found at %s..." %
self.basket_pos)
else:
self.basket_search_t -= 1
if self.basket_search_t == 0:
self.basket_search_t += 1
self.scene.waitForSync()
def updateScene(self):
# find objects
# here i am declaring goal as an object. https://github.com/mikeferguson/grasping_msgs/blob/master/action/FindGraspableObjects.action
goal = FindGraspableObjectsGoal()
goal.plan_grasps = True
# passing the object to find_client
# now find_client is wer magic happens
# on demo.launch file i am runnning basic_grasping_perception. (below <!-- Start Perception -->)
# this keeps running on background and i use actionlib (initalize on init) to get a hook to it.
# find_client is connected to basic_grasping_perception
self.find_client.send_goal(goal)
self.find_client.wait_for_result(rospy.Duration(5.0))
# here we get all the objects
find_result = self.find_client.get_result()
# remove previous objects
for name in self.scene.getKnownCollisionObjects():
self.scene.removeCollisionObject(name, False)
for name in self.scene.getKnownAttachedObjects():
self.scene.removeAttachedObject(name, False)
self.scene.waitForSync()
rospy.loginfo("updating scene")
idx = 0
# insert objects to the planning scene
#TODO so these two for loops yo can hardcode the values. try printing all the params and u will understand
for obj in find_result.objects:
rospy.loginfo("object number -> %d" %idx)
obj.object.name = "object%d"%idx
self.scene.addSolidPrimitive(obj.object.name,
obj.object.primitives[0],
obj.object.primitive_poses[0],
wait = False)
idx += 1
# for grp in obj.grasps:
# grp.grasp_pose.pose.position.z = 0.37
# this is just minor adjustments i did mess up with this code. just follwed simple gasp thingy
for obj in find_result.support_surfaces:
# extend surface to floor, and make wider since we have narrow field of view
height = obj.primitive_poses[0].position.z
rospy.loginfo("height before => %f" % height)
obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0],
1.5, # wider
obj.primitives[0].dimensions[2] + height]
obj.primitive_poses[0].position.z += -height/2.0
rospy.loginfo("height after => %f" % obj.primitive_poses[0].position.z)
# add to scene
self.scene.addSolidPrimitive(obj.name,
obj.primitives[0],
obj.primitive_poses[0],
wait = False)
self.scene.waitForSync()
# store for grasping
self.objects = find_result.objects
self.surfaces = find_result.support_surfaces
