def execute(self, userdata):
btts.print_occupied_platf_poses(userdata.rear_platform_occupied_poses)
btts.print_task_spec(userdata.task_list)
# get objects to be placed at current workstation
objs_for_this_ws = []
for task in userdata.task_list:
if (task.type == "destination"
and task.location == userdata.base_pose_to_approach):
objs_for_this_ws = task.object_names
# filter out all objects, that the robot is currently carrying AND that must be placed at the current platform
stop = False
list_of_carried_objects_that_must_be_placed = []
for i in range(len(userdata.rear_platform_occupied_poses)):
for obj_name in objs_for_this_ws:
rospy.loginfo(
"userdata.rear_platform_occupied_poses[i].obj.name: %s obj_name: %s",
userdata.rear_platform_occupied_poses[i].obj.name,
obj_name,
)
if userdata.rear_platform_occupied_poses[
i].obj.name == obj_name:
list_of_carried_objects_that_must_be_placed.append(
userdata.rear_platform_occupied_poses[i].obj)
if len(list_of_carried_objects_that_must_be_placed) == 0:
return "no_more_obj_for_this_workspace"
userdata.selected_objects = list_of_carried_objects_that_must_be_placed
return "objects_selected"
def execute(self, userdata):
btts.print_occupied_platf_poses(userdata.rear_platform_occupied_poses)
#rospy.loginfo('userdata.selected_object: %s', userdata.selected_object)
pltf_obj_pose = None
for i in range(len(userdata.rear_platform_occupied_poses)):
rospy.loginfo("userdata.rear_platform_occupied_poses[i].obj.name: %s", userdata.rear_platform_occupied_poses[i].obj.name)
if userdata.selected_object.name in userdata.rear_platform_occupied_poses[i].obj.name:
pltf_obj_pose = userdata.rear_platform_occupied_poses.pop(i)
userdata.rear_platform_free_poses.append(pltf_obj_pose)
userdata.last_grasped_obj = pltf_obj_pose.obj
rospy.loginfo("LAST OBJ: %s", userdata.last_grasped_obj.name)
break
btts.print_occupied_platf_poses(userdata.rear_platform_occupied_poses)
#delete placed obj from task list
for j in range(len(userdata.task_list)):
if userdata.task_list[j].type == 'destination' and userdata.task_list[j].location == userdata.base_pose_to_approach:
print "lllll: ", userdata.last_grasped_obj.name
print "list: ", userdata.task_list[j].object_names
userdata.task_list[j].object_names.remove(userdata.last_grasped_obj.name)
if len(userdata.task_list[j].object_names) == 0:
userdata.task_list.pop(j)
break
btts.print_task_spec(userdata.task_list)
if not pltf_obj_pose:
return 'no_more_obj_for_this_workspace'
print "plat_pose: ", pltf_obj_pose.platform_pose
print "plat_name: ", pltf_obj_pose.obj.name
manipulation.gripper_command.set_named_target("open")
manipulation.gripper_command.go()
manipulation.arm_command.set_named_target(str(pltf_obj_pose.platform_pose) + "_pre")
manipulation.arm_command.go()
manipulation.arm_command.set_named_target(str(pltf_obj_pose.platform_pose))
manipulation.arm_command.go()
manipulation.gripper_command.set_named_target("close")
manipulation.gripper_command.go()
manipulation.arm_command.set_named_target(str(pltf_obj_pose.platform_pose) + "_pre")
manipulation.arm_command.go()
return 'object_grasped'
def execute(self, userdata):
btts.print_occupied_platf_poses(userdata.rear_platform_occupied_poses)
btts.print_task_spec(userdata.task_list)
print_all_found_holes(userdata.all_found_holes)
# get objects to be placed at current workstation
objs_for_this_ws = []
for task in userdata.task_list:
if (task.type == "destination"
and task.location == userdata.base_pose_to_approach):
objs_for_this_ws = task.object_names
# filter out all objects, that the robot is currently carrying AND that must be placed at the current platform
stop = False
list_of_carried_objects_that_must_be_placed = []
for i in range(len(userdata.rear_platform_occupied_poses)):
for obj_name in objs_for_this_ws:
rospy.loginfo(
"userdata.rear_platform_occupied_poses[i].obj.name: %s obj_name: %s",
userdata.rear_platform_occupied_poses[i].obj.name,
obj_name,
)
if userdata.rear_platform_occupied_poses[
i].obj.name == obj_name:
list_of_carried_objects_that_must_be_placed.append(
obj_name)
stop = True
break
if stop:
break
if len(list_of_carried_objects_that_must_be_placed) == 0:
return "no_more_obj_for_this_workspace"
# from the objects, that the robot is carrying AND which must be placed at the current platform,
# filter out those objects, for which there is a hole in the current platform
for obj_name in list_of_carried_objects_that_must_be_placed:
for hole in userdata.all_found_holes:
rospy.loginfo("hole: %s obj_name: %s", hole, obj_name)
if hole.name in obj_name:
userdata.selected_hole = hole
return "hole_selected"
# no carried object matched any of the found holes
return "no_match"