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
]
},
}
