def call_db(): rospy.wait_for_service('/objects_database_node/database_grasp_planning') try: grasp = rospy.ServiceProxy( '/objects_database_node/database_grasp_planning', GraspPlanning) req = GraspPlanningRequest() req.arm_name = 'right_arm' req.target.reference_frame_id = 'head_mount_kinect_rgb_optical_frame' model = DatabaseModelPose() model.model_id = 18699 pose = PoseStamped() pose.header.frame_id = 'head_mount_kinect_rgb_optical_frame' pose.pose.position.x = 0.320944428444 pose.pose.position.y = 0.0485505461693 pose.pose.position.z = 0.959076166153 pose.pose.orientation.x = 0.992253601551 pose.pose.orientation.y = 0.000377745833248 pose.pose.orientation.z = 0.0030180788599 pose.pose.orientation.w = 0.124191477895 model.pose = pose req.target.potential_models.append(model) resp = grasp(req) for i in range(resp.grasps.__len__()): print "resp: ", resp.grasps[i].grasp_pose return resp except rospy.ServiceException, e: print "Service call failed: %s" % e
def place_click(self): if (self.pickup_result): grasp = self.pickup_result.grasp self.pickupservice.pre_grasp_exec(grasp, 10.0) else: self.pickupservice.grasp_release_exec(10.0) initial_pose = PoseStamped() initial_pose.header.stamp = rospy.get_rostime() initial_pose.header.frame_id = "/world" #"/fixed" #fake hand to world pose initial_pose.pose.position.x = 0.4 #self.box_pose.pose.position.x+0.0 initial_pose.pose.position.y = 0.091 # self.box_pose.pose.position.y-0.2 initial_pose.pose.position.z = 1.25 #self.box_pose.pose.position.z+0.05 q = transformations.quaternion_about_axis(-0.05, (0, 0, 1)) initial_pose.pose.orientation.x = 0.41 #self.box_pose.pose.orientation.x#q[0] initial_pose.pose.orientation.y = 0.695 #self.box_pose.pose.orientation.y#q[1] initial_pose.pose.orientation.z = 0.52 #self.box_pose.pose.orientation.z#q[2] initial_pose.pose.orientation.w = 0.284 #self.box_pose.pose.orientation.w#q[3] executed_grasp = Grasp() executed_grasp.grasp_pose = copy.deepcopy(initial_pose.pose) #fake obj world origin pose initial_pose.pose.position.x = 0.43 initial_pose.pose.position.y = 0.149 initial_pose.pose.position.z = 1.088 initial_pose.pose.orientation.x = 0.0 #self.box_pose.pose.orientation.x#q[0] initial_pose.pose.orientation.y = 0.0 #self.box_pose.pose.orientation.y#q[1] initial_pose.pose.orientation.z = 0.567 #self.box_pose.pose.orientation.z#q[2] initial_pose.pose.orientation.w = 0.824 #self.box_pose.pose.orientation.w#q[3] #fake graspable object graspable_object = GraspableObject() graspable_object.reference_frame_id = "/world" mypotentialmodels = DatabaseModelPose() mypotentialmodels.model_id = 18744 mypotentialmodels.confidence = 1.0 mypotentialmodels.pose = copy.deepcopy(initial_pose) graspable_object.potential_models.append(mypotentialmodels) #fake obj world destination pose initial_pose.pose.position.x = 0.43 initial_pose.pose.position.y = 0.0 initial_pose.pose.position.z = 1.088 initial_pose.pose.orientation.x = 0.0 #self.box_pose.pose.orientation.x#q[0] initial_pose.pose.orientation.y = 0.0 #self.box_pose.pose.orientation.y#q[1] initial_pose.pose.orientation.z = 0.567 #self.box_pose.pose.orientation.z#q[2] initial_pose.pose.orientation.w = 0.824 #self.box_pose.pose.orientation.w#q[3] list_of_poses = self.compute_list_of_poses(initial_pose, graspable_object, executed_grasp)
def segment_objects(self, req): segmentation_result = self.tabletop_segmentation_srv() if segmentation_result.result != TabletopSegmentationResponse.SUCCESS or not segmentation_result.clusters: rospy.logerr('TabletopSegmentation did not find any clusters') return None rospy.loginfo('TabletopSegmentation found %d clusters' % len(segmentation_result.clusters)) tdr = TabletopDetectionResult() tdr.table = segmentation_result.table tdr.clusters = segmentation_result.clusters tdr.result = segmentation_result.result req = GetClusterLabelsRequest() req.clusters = segmentation_result.clusters res = self.classify_srv(req) labels = res.labels index_to_label = { 0: 'Lego', 1: 'Box', 2: 'Toy', 3: 'Ball', 4: 'Cup', } label_to_index = { 'Lego': 0, 'Box': 1, 'Toy': 2, 'Ball': 3, 'Cup': 4, } for idx, cluster in enumerate(segmentation_result.clusters): bbox_response = self.find_bounding_box_srv(cluster) dmp = DatabaseModelPose() dmp.model_id = label_to_index[labels[idx]] dmp.pose = bbox_response.pose dmpl = DatabaseModelPoseList() dmpl.model_list = [dmp] tdr.models.append(dmpl) res = TabletopDetectionResponse() res.detection = tdr rospy.loginfo(res) return res
def segment_objects(self, req): segmentation_result = self.tabletop_segmentation_srv() if segmentation_result.result != TabletopSegmentationResponse.SUCCESS or not segmentation_result.clusters: rospy.logerr('TabletopSegmentation did not find any clusters') return None rospy.loginfo('TabletopSegmentation found %d clusters' % len(segmentation_result.clusters)) tdr = TabletopDetectionResult() tdr.table = segmentation_result.table tdr.clusters = segmentation_result.clusters tdr.result = segmentation_result.result req = GetClusterLabelsRequest() req.clusters = segmentation_result.clusters res = self.classify_srv(req) labels = res.labels index_to_label = { 0: 'Lego', 1: 'Box', 2: 'Toy', 3: 'Ball', 4: 'Cup', } label_to_index = { 'Lego': 0, 'Box': 1, 'Toy': 2, 'Ball': 3, 'Cup': 4, } for idx,cluster in enumerate(segmentation_result.clusters): bbox_response = self.find_bounding_box_srv(cluster) dmp = DatabaseModelPose() dmp.model_id = label_to_index[labels[idx]] dmp.pose = bbox_response.pose dmpl = DatabaseModelPoseList() dmpl.model_list = [dmp] tdr.models.append(dmpl) res = TabletopDetectionResponse() res.detection = tdr rospy.loginfo(res) return res
def get_grasps(self, model_id, model_pose): ''' Get the set of grasps for a model, given a particular model pose **Args:** **model_id:** The model id corresponding to the database being used *model_pose:* The pose of the model ''' request = GraspPlanningRequest() request.arm_name = 'right_arm' db_pose = DatabaseModelPose() db_pose.pose = model_pose db_pose.model_id = model_id db_pose.confidence = 0.5 request.target.potential_models.append(db_pose) request.target.reference_frame_id = model_pose.header.frame_id response = self.database_grasp_planner(request) rospy.loginfo('Found = ' + str(len(response.grasps)) + ' grasps') self._visualize_grasps(response.grasps, model_pose.header.frame_id) return response.grasps
def get_grasps(self, model_id, model_pose): ''' Get the set of grasps for a model, given a particular model pose **Args:** **model_id:** The model id corresponding to the database being used *model_pose:* The pose of the model ''' request = GraspPlanningRequest() request.arm_name = 'right_arm' db_pose = DatabaseModelPose() db_pose.pose = model_pose db_pose.model_id = model_id db_pose.confidence = 0.5 request.target.potential_models.append(db_pose) request.target.reference_frame_id = model_pose.header.frame_id response = self.database_grasp_planner(request) rospy.loginfo('Found = '+str(len(response.grasps))+' grasps') self._visualize_grasps(response.grasps,model_pose.header.frame_id) return response.grasps