def obj_response_cb(userdata, response):
if response.exists:
pose = Pose()
#Wont there be problems mixing float32 with 64? TODO
pose.position = Point(response.base_coordinates.x, response.base_coordinates.y, 0)
#TODO, this may give problems
pose.orientation = Quaternion(*quaternion_from_euler(0, 0, response.base_coordinates.z))
if MANTAIN_DISTANCE_FROM_OBJECT:
pose = pose_at_distance(pose, DISTANCE_FROM_OBJECT)
userdata.object_location = pose
release_pose = ObjectPose()
release_pose.pose = response.arm_coordinates
userdata.object_release_location = release_pose
print "\n\n printing pose for move to object"
print pose
print "\n\n printing pose for releasing the object"
print release_pose
print "\n that was the pose of the object\n\n"
print userdata.object_name
print "is the object name"
return succeeded
else:
userdata.object_location = None
return aborted
def obj_response_cb(userdata, response):
if response.exists:
pose = Pose()
pose.position = Point(response.base_coordinates.x, response.base_coordinates.y, 0)
pose.orientation = Quaternion(*quaternion_from_euler(0, 0, response.base_coordinates.z))
userdata.object_location = pose
release_pose = ObjectPose()
release_pose.pose = response.arm_coordinates
userdata.object_release_location = release_pose
print "\n\n printing pose for move to object"
print pose
print "\n\n printing pose for releasing the object"
print release_pose
print "\n that was the pose of the object\n\n"
print userdata.object_name
print "is the object name"
return succeeded
else:
userdata.object_location = None
return aborted