target_obj_pose = Pose() target_obj_pose.position.x = 0.64 target_obj_pose.position.y = 0.9 target_obj_pose.position.z = 0.8 target_obj_pose.orientation.x = 0 target_obj_pose.orientation.y = 0 target_obj_pose.orientation.z = 0 target_obj_pose.orientation.w = 1 workspace_info = getWorkspaceOnMap() parent_obj_geometry = SRSSpatialInfo() parent_obj_geometry.pose.position.x = workspace_info.objectsInfo[0].pose.position.x parent_obj_geometry.pose.position.y = workspace_info.objectsInfo[0].pose.position.y parent_obj_geometry.pose.position.z = workspace_info.objectsInfo[0].pose.position.z parent_obj_geometry.pose.orientation.x = workspace_info.objectsInfo[0].pose.orientation.x parent_obj_geometry.pose.orientation.y = workspace_info.objectsInfo[0].pose.orientation.y parent_obj_geometry.pose.orientation.z = workspace_info.objectsInfo[0].pose.orientation.z parent_obj_geometry.pose.orientation.w = workspace_info.objectsInfo[0].pose.orientation.w parent_obj_geometry.l = workspace_info.objectsInfo[0].l parent_obj_geometry.w = workspace_info.objectsInfo[0].w parent_obj_geometry.h = workspace_info.objectsInfo[0].h #parent_obj_geometry.pose.position.x = -3.3 #parent_obj_geometry.pose.position.y = 0.5
#get the index of the target workspace e.g. table0
index_of_the_target_workspace = all_workspaces_on_map.objects.index(userdata.target_workspace_name)
#get the pose of the workspace from knowledge service
target_object_pose = all_workspaces_on_map.objectsInfo[index_of_the_target_workspace].pose
#get the houseHoldID of the workspace
object_id = all_workspaces_on_map.houseHoldId[index_of_the_target_workspace]
#rospy.loginfo ("target name: %s", userdata.target_workspace_name)
#rospy.loginfo ("target pose: %s", target_object_pose)
#rospy.loginfo ("target id: %s", object_id)
except rospy.ServiceException, e:
print "Service call failed: %s"%e
return 'failed'
parent_obj_geometry = SRSSpatialInfo()
parent_obj_geometry.pose = target_object_pose
parent_obj_geometry.l = all_workspaces_on_map.objectsInfo[index_of_the_target_workspace].l
parent_obj_geometry.w = all_workspaces_on_map.objectsInfo[index_of_the_target_workspace].w
parent_obj_geometry.h = all_workspaces_on_map.objectsInfo[index_of_the_target_workspace].h
rospy.wait_for_service('symbol_grounding_grasp_base_pose_experimental')
symbol_grounding_grasp_base_pose_experimental = rospy.ServiceProxy('symbol_grounding_grasp_base_pose_experimental', SymbolGroundingGraspBasePoseExperimental)
try:
result = symbol_grounding_grasp_base_pose_experimental(object_pose_map.pose, parent_obj_geometry, all_workspaces_on_map.objectsInfo)
userdata.base_pose = [result.grasp_base_pose.x, result.grasp_base_pose.y, result.grasp_base_pose.theta]
return 'retry'
except rospy.ServiceException, e:
print "Service call failed: %s" %e
return 'failed'
#get the pose of the workspace from knowledge service
target_object_pose = all_workspaces_on_map.objectsInfo[
index_of_the_target_workspace].pose
#get the houseHoldID of the workspace
object_id = all_workspaces_on_map.houseHoldId[
index_of_the_target_workspace]
#rospy.loginfo ("target name: %s", userdata.target_workspace_name)
#rospy.loginfo ("target pose: %s", target_object_pose)
#rospy.loginfo ("target id: %s", object_id)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
return 'failed'
parent_obj_geometry = SRSSpatialInfo()
parent_obj_geometry.pose = target_object_pose
parent_obj_geometry.l = all_workspaces_on_map.objectsInfo[
index_of_the_target_workspace].l
parent_obj_geometry.w = all_workspaces_on_map.objectsInfo[
index_of_the_target_workspace].w
parent_obj_geometry.h = all_workspaces_on_map.objectsInfo[
index_of_the_target_workspace].h
rospy.wait_for_service('symbol_grounding_grasp_base_pose_experimental')
symbol_grounding_grasp_base_pose_experimental = rospy.ServiceProxy(
'symbol_grounding_grasp_base_pose_experimental',
SymbolGroundingGraspBasePoseExperimental)
try:
result = symbol_grounding_grasp_base_pose_experimental(
object_pose_map.pose, parent_obj_geometry,
def getWorkspaceOnMap():
print 'test get all workspace (furnitures basically here) from map'
try:
requestNewTask = rospy.ServiceProxy('get_workspace_on_map',
GetWorkspaceOnMap)
res = requestNewTask('ipa-kitchen-map', True)
return res
except rospy.ServiceException, e:
print "Service call failed: %s" % e
if __name__ == "__main__":
workspace_info = getWorkspaceOnMap()
parent_obj_geometry = SRSSpatialInfo()
parent_obj_geometry.pose.position.x = workspace_info.objectsInfo[
2].pose.position.x
parent_obj_geometry.pose.position.y = workspace_info.objectsInfo[
2].pose.position.y
parent_obj_geometry.pose.position.z = workspace_info.objectsInfo[
2].pose.position.z
parent_obj_geometry.pose.orientation.x = workspace_info.objectsInfo[
2].pose.orientation.x
parent_obj_geometry.pose.orientation.y = workspace_info.objectsInfo[
2].pose.orientation.y
parent_obj_geometry.pose.orientation.z = workspace_info.objectsInfo[
2].pose.orientation.z
parent_obj_geometry.pose.orientation.w = workspace_info.objectsInfo[
2].pose.orientation.w
target_obj_pose = Pose() target_obj_pose.position.x = 0.64 target_obj_pose.position.y = 0.9 target_obj_pose.position.z = 0.8 target_obj_pose.orientation.x = 0 target_obj_pose.orientation.y = 0 target_obj_pose.orientation.z = 0 target_obj_pose.orientation.w = 1 #workspace_info = getWorkspaceOnMap() parent_obj_geometry = SRSSpatialInfo() ''' parent_obj_geometry.pose.position.x = workspace_info.objectsInfo[0].pose.position.x parent_obj_geometry.pose.position.y = workspace_info.objectsInfo[0].pose.position.y parent_obj_geometry.pose.position.z = workspace_info.objectsInfo[0].pose.position.z parent_obj_geometry.pose.orientation.x = workspace_info.objectsInfo[0].pose.orientation.x parent_obj_geometry.pose.orientation.y = workspace_info.objectsInfo[0].pose.orientation.y parent_obj_geometry.pose.orientation.z = workspace_info.objectsInfo[0].pose.orientation.z parent_obj_geometry.pose.orientation.w = workspace_info.objectsInfo[0].pose.orientation.w parent_obj_geometry.l = workspace_info.objectsInfo[0].l parent_obj_geometry.w = workspace_info.objectsInfo[0].w parent_obj_geometry.h = workspace_info.objectsInfo[0].h ''' parent_obj_geometry.pose.position.x = -3.3
