def lift_goal_cb(userdata, goal): lift_goal = LiftObjectGoal() lift_goal.category_id = 0 lift_goal.graspable_object = userdata.segmentation_result.graspable_objects[userdata.closest_index] lift_goal.collision_object_name = userdata.segmentation_result.collision_object_names[userdata.closest_index] lift_goal.collision_support_surface_name = userdata.segmentation_result.collision_support_surface_name return lift_goal
def do_it(msg): rospy.loginfo('We are DOING IT!') # connect to cluster bounding box finding service rospy.loginfo('waiting for find_cluster_bounding_box service') rospy.wait_for_service('/find_cluster_bounding_box') find_bounding_box_srv = rospy.ServiceProxy('/find_cluster_bounding_box', FindClusterBoundingBox) rospy.loginfo('connected to find_cluster_bounding_box') target_location = msg.target_location t_p1 = msg.object_bbox.points[0] t_p2 = msg.object_bbox.points[1] t_p1.y, t_p2.y = t_p2.y, t_p1.y reset = ResetRobot() reset.execute() rospy.loginfo('Robot reset') segment = SegmentScene() res = segment.execute() if not res: exit(1) rospy.loginfo('Scene segmented') # find a cluster that intersects with a provided bounding box for idx,cluster in enumerate(res[1]['segmentation_result'].clusters): bbox_response = find_bounding_box_srv(cluster) if bbox_response.error_code != FindClusterBoundingBoxResponse.SUCCESS: rospy.logwarn('unable to find for cluster %d bounding box' % idx) continue o_center = bbox_response.pose.pose.position o_p1 = Point32(o_center.x - bbox_response.box_dims.x, o_center.y - bbox_response.box_dims.y, o_center.z) o_p2 = Point32(o_center.x + bbox_response.box_dims.x, o_center.y + bbox_response.box_dims.y, o_center.z) print idx, t_p1, t_p2, o_p1, o_p2 if ((t_p1.x < o_p2.x and t_p1.y < o_p2.y) and (o_p1.x < t_p2.x and o_p1.y < t_p2.y)): closest_index = idx break grasp_client = SimpleActionClient('grasp_object', GraspObjectAction) grasp_client.wait_for_server() grasp_goal = GraspObjectGoal() grasp_goal.category_id = -1 grasp_goal.graspable_object = res[0].graspable_objects[closest_index] grasp_goal.collision_object_name = res[0].collision_object_names[closest_index] grasp_goal.collision_support_surface_name = res[0].collision_support_surface_name result = grasp_client.send_goal_and_wait(grasp_goal) if result != GoalStatus.SUCCEEDED: rospy.logerr('failed to grasp') reset.execute() exit(1) rospy.loginfo('Grasped an object') lift_client = SimpleActionClient('lift_object', LiftObjectAction) lift_client.wait_for_server() lift_goal = LiftObjectGoal() lift_goal.category_id = 0 lift_goal.graspable_object = res[0].graspable_objects[closest_index] lift_goal.collision_object_name = res[0].collision_object_names[closest_index] lift_goal.collision_support_surface_name = res[0].collision_support_surface_name result = lift_client.send_goal_and_wait(lift_goal) if result != GoalStatus.SUCCEEDED: rospy.logerr('failed to lift') exit(1) rospy.loginfo('Lifted an object') pudown_client = SimpleActionClient('put_down_at', PutDownAtAction) pudown_client.wait_for_server() putdown_goal = PutDownAtGoal() putdown_goal.category_id = -1 putdown_goal.graspable_object = res[0].graspable_objects[closest_index] putdown_goal.collision_object_name = res[0].collision_object_names[closest_index] putdown_goal.collision_support_surface_name = res[0].collision_support_surface_name putdown_goal.target_location = Point(target_location.x, target_location.y, target_location.z) #Point(0.468, 0.226, 0.251) result = pudown_client.send_goal_and_wait(putdown_goal) if result != GoalStatus.SUCCEEDED: rospy.logerr('failed to put down') reset.execute() exit(1) rospy.loginfo('Pu down the object') reset.execute()
grasp_goal.category_id = -1 grasp_goal.graspable_object = res[0].graspable_objects[closest_index] grasp_goal.collision_object_name = res[0].collision_object_names[closest_index] grasp_goal.collision_support_surface_name = res[0].collision_support_surface_name result = grasp_client.send_goal_and_wait(grasp_goal) if result != GoalStatus.SUCCEEDED: rospy.logerr('failed to grasp') reset.execute() exit(1) lift_client = SimpleActionClient('lift_object', LiftObjectAction) lift_client.wait_for_server() lift_goal = LiftObjectGoal() lift_goal.category_id = 0 lift_goal.graspable_object = res[0].graspable_objects[closest_index] lift_goal.collision_object_name = res[0].collision_object_names[closest_index] lift_goal.collision_support_surface_name = res[0].collision_support_surface_name result = lift_client.send_goal_and_wait(lift_goal) if result != GoalStatus.SUCCEEDED: rospy.logerr('failed to lift') exit(1) move_client = SimpleActionClient('/move_base', MoveBaseAction) move_client.wait_for_server() mbg = MoveBaseGoal()
grasp_goal.collision_object_name = res[0].collision_object_names[ closest_index] grasp_goal.collision_support_surface_name = res[ 0].collision_support_surface_name result = grasp_client.send_goal_and_wait(grasp_goal) if result != GoalStatus.SUCCEEDED: rospy.logerr('failed to grasp') reset.execute() exit(1) lift_client = SimpleActionClient('lift_object', LiftObjectAction) lift_client.wait_for_server() lift_goal = LiftObjectGoal() lift_goal.category_id = 0 lift_goal.graspable_object = res[0].graspable_objects[closest_index] lift_goal.collision_object_name = res[0].collision_object_names[ closest_index] lift_goal.collision_support_surface_name = res[ 0].collision_support_surface_name result = lift_client.send_goal_and_wait(lift_goal) if result != GoalStatus.SUCCEEDED: rospy.logerr('failed to lift') exit(1) move_client = SimpleActionClient('/move_base', MoveBaseAction) move_client.wait_for_server()
def do_it(msg): rospy.loginfo('We are DOING IT!') # connect to cluster bounding box finding service rospy.loginfo('waiting for find_cluster_bounding_box service') rospy.wait_for_service('/find_cluster_bounding_box') find_bounding_box_srv = rospy.ServiceProxy('/find_cluster_bounding_box', FindClusterBoundingBox) rospy.loginfo('connected to find_cluster_bounding_box') target_location = msg.target_location t_p1 = msg.object_bbox.points[0] t_p2 = msg.object_bbox.points[1] t_p1.y, t_p2.y = t_p2.y, t_p1.y reset = ResetRobot() reset.execute() rospy.loginfo('Robot reset') segment = SegmentScene() res = segment.execute() if not res: exit(1) rospy.loginfo('Scene segmented') # find a cluster that intersects with a provided bounding box for idx, cluster in enumerate(res[1]['segmentation_result'].clusters): bbox_response = find_bounding_box_srv(cluster) if bbox_response.error_code != FindClusterBoundingBoxResponse.SUCCESS: rospy.logwarn('unable to find for cluster %d bounding box' % idx) continue o_center = bbox_response.pose.pose.position o_p1 = Point32(o_center.x - bbox_response.box_dims.x, o_center.y - bbox_response.box_dims.y, o_center.z) o_p2 = Point32(o_center.x + bbox_response.box_dims.x, o_center.y + bbox_response.box_dims.y, o_center.z) print idx, t_p1, t_p2, o_p1, o_p2 if ((t_p1.x < o_p2.x and t_p1.y < o_p2.y) and (o_p1.x < t_p2.x and o_p1.y < t_p2.y)): closest_index = idx break grasp_client = SimpleActionClient('grasp_object', GraspObjectAction) grasp_client.wait_for_server() grasp_goal = GraspObjectGoal() grasp_goal.category_id = -1 grasp_goal.graspable_object = res[0].graspable_objects[closest_index] grasp_goal.collision_object_name = res[0].collision_object_names[ closest_index] grasp_goal.collision_support_surface_name = res[ 0].collision_support_surface_name result = grasp_client.send_goal_and_wait(grasp_goal) if result != GoalStatus.SUCCEEDED: rospy.logerr('failed to grasp') reset.execute() exit(1) rospy.loginfo('Grasped an object') lift_client = SimpleActionClient('lift_object', LiftObjectAction) lift_client.wait_for_server() lift_goal = LiftObjectGoal() lift_goal.category_id = 0 lift_goal.graspable_object = res[0].graspable_objects[closest_index] lift_goal.collision_object_name = res[0].collision_object_names[ closest_index] lift_goal.collision_support_surface_name = res[ 0].collision_support_surface_name result = lift_client.send_goal_and_wait(lift_goal) if result != GoalStatus.SUCCEEDED: rospy.logerr('failed to lift') exit(1) rospy.loginfo('Lifted an object') pudown_client = SimpleActionClient('put_down_at', PutDownAtAction) pudown_client.wait_for_server() putdown_goal = PutDownAtGoal() putdown_goal.category_id = -1 putdown_goal.graspable_object = res[0].graspable_objects[closest_index] putdown_goal.collision_object_name = res[0].collision_object_names[ closest_index] putdown_goal.collision_support_surface_name = res[ 0].collision_support_surface_name putdown_goal.target_location = Point( target_location.x, target_location.y, target_location.z) #Point(0.468, 0.226, 0.251) result = pudown_client.send_goal_and_wait(putdown_goal) if result != GoalStatus.SUCCEEDED: rospy.logerr('failed to put down') reset.execute() exit(1) rospy.loginfo('Pu down the object') reset.execute()
def __init__(self): self.object_detector = ObjectDetector() # connect to collision map processing service rospy.loginfo('waiting for tabletop_collision_map_processing service') rospy.wait_for_service( '/tabletop_collision_map_processing/tabletop_collision_map_processing' ) self.collision_map_processing_srv = rospy.ServiceProxy( '/tabletop_collision_map_processing/tabletop_collision_map_processing', TabletopCollisionMapProcessing) rospy.loginfo('connected to tabletop_collision_map_processing service') # connect to gripper action server rospy.loginfo('waiting for wubble_gripper_grasp_action') self.posture_controller = SimpleActionClient( '/wubble_gripper_grasp_action', GraspHandPostureExecutionAction) self.posture_controller.wait_for_server() rospy.loginfo('connected to wubble_gripper_grasp_action') rospy.loginfo('waiting for wubble_grasp_status service') rospy.wait_for_service('/wubble_grasp_status') self.get_grasp_status_srv = rospy.ServiceProxy('/wubble_grasp_status', GraspStatus) rospy.loginfo('connected to wubble_grasp_status service') rospy.loginfo('waiting for classify service') rospy.wait_for_service('/classify') self.classification_srv = rospy.ServiceProxy('/classify', classify) rospy.loginfo('connected to classify service') # connect to gripper action server rospy.loginfo('waiting for wubble_gripper_action') self.gripper_controller = SimpleActionClient('/wubble_gripper_action', WubbleGripperAction) self.gripper_controller.wait_for_server() rospy.loginfo('connected to wubble_gripper_action') # connect to wubble actions rospy.loginfo('waiting for drop_object') self.drop_object_client = SimpleActionClient('/drop_object', DropObjectAction) self.drop_object_client.wait_for_server() rospy.loginfo('connected to drop_object') rospy.loginfo('waiting for grasp_object') self.grasp_object_client = SimpleActionClient('/grasp_object', GraspObjectAction) self.grasp_object_client.wait_for_server() rospy.loginfo('connected to grasp_object') rospy.loginfo('waiting for lift_object') self.lift_object_client = SimpleActionClient('/lift_object', LiftObjectAction) self.lift_object_client.wait_for_server() rospy.loginfo('connected to lift_object') rospy.loginfo('waiting for place_object') self.place_object_client = SimpleActionClient('/place_object', PlaceObjectAction) self.place_object_client.wait_for_server() rospy.loginfo('connected to plcae_object') rospy.loginfo('waiting for push_object') self.push_object_client = SimpleActionClient('/push_object', PushObjectAction) self.push_object_client.wait_for_server() rospy.loginfo('connected to push_object') rospy.loginfo('waiting for shake_roll_object') self.shake_roll_object_client = SimpleActionClient( '/shake_roll_object', ShakeRollObjectAction) self.shake_roll_object_client.wait_for_server() rospy.loginfo('connected to drop_object') rospy.loginfo('waiting for shake_pitch_object') self.shake_pitch_object_client = SimpleActionClient( '/shake_pitch_object', ShakePitchObjectAction) self.shake_pitch_object_client.wait_for_server() rospy.loginfo('connected to pitch_object') rospy.loginfo('waiting for ready_arm') self.ready_arm_client = SimpleActionClient('/ready_arm', ReadyArmAction) self.ready_arm_client.wait_for_server() rospy.loginfo('connected to ready_arm') # advertise InfoMax service rospy.Service('get_category_distribution', InfoMax, self.process_infomax_request) rospy.loginfo( 'all services contacted, object_categorization is ready to go') self.ACTION_INFO = { InfomaxAction.GRASP: { 'client': self.grasp_object_client, 'goal': GraspObjectGoal(), 'prereqs': [InfomaxAction.GRASP] }, InfomaxAction.LIFT: { 'client': self.lift_object_client, 'goal': LiftObjectGoal(), 'prereqs': [InfomaxAction.GRASP, InfomaxAction.LIFT] }, InfomaxAction.SHAKE_ROLL: { 'client': self.shake_roll_object_client, 'goal': ShakeRollObjectGoal(), 'prereqs': [ InfomaxAction.GRASP, InfomaxAction.LIFT, InfomaxAction.SHAKE_ROLL ] }, InfomaxAction.DROP: { 'client': self.drop_object_client, 'goal': DropObjectGoal(), 'prereqs': [InfomaxAction.GRASP, InfomaxAction.LIFT, InfomaxAction.DROP] }, InfomaxAction.PLACE: { 'client': self.place_object_client, 'goal': PlaceObjectGoal(), 'prereqs': [InfomaxAction.GRASP, InfomaxAction.LIFT, InfomaxAction.PLACE] }, InfomaxAction.PUSH: { 'client': self.push_object_client, 'goal': PushObjectGoal(), 'prereqs': [ InfomaxAction.GRASP, InfomaxAction.LIFT, InfomaxAction.PLACE, InfomaxAction.PUSH ] }, InfomaxAction.SHAKE_PITCH: { 'client': self.shake_pitch_object_client, 'goal': ShakePitchObjectGoal(), 'prereqs': [ InfomaxAction.GRASP, InfomaxAction.LIFT, InfomaxAction.SHAKE_PITCH ] }, }