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