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 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()
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(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()