def __init__(self, whicharm = "left_arm", #octomap_filters_service="/create_filter", planner = None, padding = 0.1): """ whicharm: a string, right_arm or left_arm. padding: a float """ self.whicharm = whicharm self.padding = padding #Don't use the object recognition-detection, otherwise it will populate #the collision map self.detector = GenericDetector(detector_service = None, collision_processing = None ) if planner is None: self.planner = PR2MoveArm() else: self.planner = planner self.planner.update_planning_scene() #rospy.loginfo("Waiting for %s", octomap_filters_service) #rospy.wait_for_service(octomap_filters_service) #self.filter_srv = rospy.ServiceProxy(octomap_filters_service,FilterDefine) self.manager = ControllerManagerClient() if self.whicharm.startswith("right"): self.manager.switch_controllers(["r_arm_controller"], ["r_arm_cart_imped_controller"], ) else: self.manager.switch_controllers(["l_arm_controller"], ["l_arm_cart_imped_controller"], ) haptics_service_name = "start_haptic_exploration" rospy.loginfo("Waiting for service %s", haptics_service_name) self.haptics_service = rospy.ServiceProxy(haptics_service_name, Empty) self.move_base = PR2BaseMover(listener = self.planner.tf_listener, use_safety_dist=True) rospy.loginfo("%s is ready", self.__class__.__name__)
def main(): rospy.init_node("show_boxes", anonymous=True) detector = GenericDetector(tabletop_segmentation="find_table") res = detector.segment_only().detection if res is None: rospy.logerr("No object found!") return rospy.loginfo("I've found %d boxes", len(res.clusters)) for i, cluster in enumerate(res.clusters): box = detector.detect_bounding_box(cluster) if box is None: rospy.logerr("Error while plotting a box") continue r = random.random() g = random.random() b = random.random() detector.draw_bounding_box(i, box, (0.7, r,g,b)) rospy.loginfo("Done")
def search_object(tf_listener): """ Searches for objects using the detector. Returns: a list of (pos, dims, names) where pos is a PoseStamped(converted in the self.frame fame with the object pose if the object is found), dims is a Vector3 specifing the x,y,z dimensions of the box and names are the names of the objects. None if no object is found """ detector = GenericDetector() res = detector.detect() if not res: return None coll_res = detector.call_collision_map_processing(res) isinstance(coll_res, TabletopCollisionMapProcessingResponse) rospy.loginfo("%d objects found, changing their poses into frame %s", len(coll_res.graspable_objects),frame) poses = [] dims = [] for graspable in coll_res.graspable_objects: cluster = graspable.cluster box_pose = detector.detect_bounding_box(cluster).pose tf_listener.waitForTransform(frame, box_pose.header.frame_id, rospy.Time(0), rospy.Duration(1) ) object_pose = tf_listener.transformPose(frame, box_pose) poses.append(object_pose) dims.append(detector.last_box_msg.box_dims) return poses, dims, coll_res.collision_object_names
class MoveToHaptics(object): """Class to detect an object and move the arm so that the object will be in between the gripper. """ def __init__(self, whicharm = "left_arm", #octomap_filters_service="/create_filter", planner = None, padding = 0.1): """ whicharm: a string, right_arm or left_arm. padding: a float """ self.whicharm = whicharm self.padding = padding #Don't use the object recognition-detection, otherwise it will populate #the collision map self.detector = GenericDetector(detector_service = None, collision_processing = None ) if planner is None: self.planner = PR2MoveArm() else: self.planner = planner self.planner.update_planning_scene() #rospy.loginfo("Waiting for %s", octomap_filters_service) #rospy.wait_for_service(octomap_filters_service) #self.filter_srv = rospy.ServiceProxy(octomap_filters_service,FilterDefine) self.manager = ControllerManagerClient() if self.whicharm.startswith("right"): self.manager.switch_controllers(["r_arm_controller"], ["r_arm_cart_imped_controller"], ) else: self.manager.switch_controllers(["l_arm_controller"], ["l_arm_cart_imped_controller"], ) haptics_service_name = "start_haptic_exploration" rospy.loginfo("Waiting for service %s", haptics_service_name) self.haptics_service = rospy.ServiceProxy(haptics_service_name, Empty) self.move_base = PR2BaseMover(listener = self.planner.tf_listener, use_safety_dist=True) rospy.loginfo("%s is ready", self.__class__.__name__) def filter_bounding_box(self, box): pass '''assert isinstance(box, FindClusterBoundingBox2Response) xmin, xmax = self.detector.get_min_max_box(box, self.padding) #now filtering the octomap req = FilterDefineRequest() req.name = "object" req.operation = req.CREATE req.min.header.frame_id = box.pose.header.frame_id req.min.point.x = xmin[0] req.min.point.y = xmin[1] req.min.point.z = xmin[2] req.max.header.frame_id = box.pose.header.frame_id req.max.point.x = xmax[0] req.max.point.y = xmax[1] req.max.point.z = xmax[2] try: self.filter_srv(req) except Exception, e: rospy.logerr("Error while calling the filtering service: %s", e) return None self.planner.update_planning_scene()''' def detect_and_filter(self): """Detect and object, selects the biggest cluster and removes the points from the map. Returns the object's pointcloud and bounding box on success, None otherwise """ det = self.detector.segment_only() if det is None: rospy.logwarn("Problems while segmenting") return None cluster = self.detector.find_biggest_cluster(det.detection.clusters) box = self.detector.detect_bounding_box(cluster) assert isinstance(box, FindClusterBoundingBox2Response) self.filter_bounding_box(box) return cluster, box def move_arm_to_pre_haptics(self, pre_touch_difference = (-0.08,0,0)): """Moves the arm so that an object will roughly be between the fingers. It first moves to an approach pose, then it will actually move to the pose """ joint_mover = self.planner.joint_mover if self.whicharm.startswith("right"): open_gripper = joint_mover.open_right_gripper move_arm_planning = self.planner.move_right_arm_with_ik move_arm_non_planning = self.planner.move_right_arm_non_collision else: open_gripper = joint_mover.open_left_gripper move_arm_planning = self.planner.move_left_arm_with_ik move_arm_non_planning = self.planner.move_left_arm_non_collision _, box = self.detect_and_filter() assert isinstance(box, FindClusterBoundingBox2Response) #TODO Here I should convert the pose to somenthing like /base_link, instead of #assuming it box_pose = [box.pose.pose.position.x, box.pose.pose.position.y, box.pose.pose.position.z ] gripper_accounted_pose = [box_pose[0] - 0.18 - box.box_dims.x/2., box_pose[1] + 0, box_pose[2] + 0.01 ] orientation = [0,0,0,1] frame_id = box.pose.header.frame_id open_gripper() pre_touch_pose = gripper_accounted_pose[:] pre_touch_pose[0] += pre_touch_difference[0] pre_touch_pose[1] += pre_touch_difference[1] pre_touch_pose[2] += pre_touch_difference[2] rospy.loginfo("Moving to a pre-touch position") if not move_arm_non_planning(pre_touch_pose, orientation, frame_id, 10): rospy.logerr("Could not move to pre-touch position!") return False touch_pose = gripper_accounted_pose[:] touch_pose[0] += 0.0 rospy.loginfo("Moving to a touch position") if not move_arm_non_planning(touch_pose, orientation, frame_id, 3): rospy.logerr("Could not move to touch position!") return False return True def execute_haptics(self): """Loads the right controllers, calls the service, re-loads the old controllers""" if self.whicharm.startswith("right"): self.manager.switch_controllers(["r_arm_cart_imped_controller"], ["r_arm_controller"] ) else: self.manager.switch_controllers(["l_arm_cart_imped_controller"], ["l_arm_controller"] ) try: self.haptics_service.call() except rospy.ServiceException, e: rospy.logwarn("Got an exception %s, probably the service took too long", e) if self.whicharm.startswith("right"): self.manager.switch_controllers(["r_arm_controller"], ["r_arm_cart_imped_controller"], ) else: self.manager.switch_controllers(["l_arm_controller"], ["l_arm_cart_imped_controller"], )