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