class NavigateToPlace(NavigateTo):
def __init__(self, robot, place_pose_designator, arm_designator=None):
"""Navigate so that the arm can reach the place point
@param place_pose_designator designator that resolves to a geometry_msgs.msg.PoseStamped
@param arm which arm to eventually place with?
"""
super(NavigateToPlace, self).__init__(robot)
self.robot = robot
# Check that place_pose_designator actually returns a PoseStamped
check_resolve_type(place_pose_designator, FrameStamped)
self.place_pose_designator = place_pose_designator
self.arm_designator = arm_designator
if not arm_designator:
rospy.logerr(
'NavigateToPlace: side should be determined by arm_designator.'
'Please specify left or right, will default to left')
self.arm_designator = Designator(robot.leftArm)
def generateConstraint(self):
arm = self.arm_designator.resolve()
if not arm:
rospy.logerr("Could not resolve arm")
return None
angle_offset = -math.atan2(arm.base_offset.y(), arm.base_offset.x())
radius = math.hypot(arm.base_offset.x(), arm.base_offset.y())
place_fs = self.place_pose_designator.resolve()
if not place_fs:
rospy.logerr("No such place_pose")
return None
rospy.loginfo("Navigating to place at {0}".format(place_fs).replace(
'\n', ' '))
try:
x = place_fs.frame.p.x()
y = place_fs.frame.p.y()
except KeyError as ke:
rospy.logerr("Could not determine pose: ".format(ke))
return None
# Outer radius
radius -= 0.1
ro = "(x-%f)^2+(y-%f)^2 < %f^2" % (x, y, radius + 0.075)
ri = "(x-%f)^2+(y-%f)^2 > %f^2" % (x, y, radius - 0.075)
# pc = PositionConstraint(constraint=ri+" and "+ro, frame="/map")
# oc = OrientationConstraint(look_at=Point(x, y, 0.0), frame="/map", angle_offset=angle_offset)
pc = PositionConstraint(constraint=ri + " and " + ro,
frame=place_fs.frame_id)
oc = OrientationConstraint(look_at=Point(x, y, 0.0),
frame=place_fs.frame_id,
angle_offset=angle_offset)
return pc, oc
class NavigateToGrasp(NavigateTo):
def __init__(self, robot, entity_designator, arm_designator=None):
super(NavigateToGrasp, self).__init__(robot, reset_head=False)
self.robot = robot
check_resolve_type(
entity_designator,
Entity) # Check that the entity_designator resolves to an Entity
self.entity_designator = entity_designator
check_resolve_type(
arm_designator,
Arm) #Check that the arm_designator resolves to an Arm
self.arm_designator = arm_designator
if not arm_designator:
rospy.logerr(
'NavigateToGrasp: side should be determined by entity_designator. Please specify left or right, will default to left'
)
self.arm_designator = Designator(robot.leftArm)
def generateConstraint(self):
arm = self.arm_designator.resolve()
if not arm:
rospy.logerr("Could not resolve arm")
return None
if arm == self.robot.arms['left']:
angle_offset = math.atan2(-self.robot.grasp_offset.y,
self.robot.grasp_offset.x)
elif arm == self.robot.arms['right']:
angle_offset = math.atan2(self.robot.grasp_offset.y,
self.robot.grasp_offset.x)
radius = math.hypot(self.robot.grasp_offset.x,
self.robot.grasp_offset.y)
entity = self.entity_designator.resolve()
if not entity:
rospy.logerr("No such entity")
return None
rospy.loginfo("Navigating to grasp entity id:{0}".format(entity.id))
try:
pose = entity.pose #TODO Janno: Not all entities have pose information
x = pose.frame.p.x()
y = pose.frame.p.y()
except KeyError, ke:
rospy.logerr("Could not determine pose: ".format(ke))
return None
try:
rz, _, _ = entity.pose.frame.M.GetEulerZYX()
except KeyError, ke:
rospy.logerr("Could not determine pose.rz: ".format(ke))
rz = 0
class NavigateToPlace(NavigateTo):
def __init__(self, robot, place_pose_designator, arm_designator=None):
"""Navigate so that the arm can reach the place point
@param place_pose_designator designator that resolves to a geometry_msgs.msg.PoseStamped
@param arm which arm to eventually place with?
"""
super(NavigateToPlace, self).__init__(robot)
self.robot = robot
# Check that place_pose_designator actually returns a PoseStamped
check_resolve_type(place_pose_designator, FrameStamped)
self.place_pose_designator = place_pose_designator
self.arm_designator = arm_designator
if not arm_designator:
rospy.logerr('NavigateToPlace: side should be determined by arm_designator.'
'Please specify left or right, will default to left')
self.arm_designator = Designator(robot.leftArm)
def generateConstraint(self):
arm = self.arm_designator.resolve()
if not arm:
rospy.logerr("Could not resolve arm")
return None
angle_offset = -math.atan2(arm.base_offset.y(), arm.base_offset.x())
radius = math.hypot(arm.base_offset.x(), arm.base_offset.y())
place_fs = self.place_pose_designator.resolve()
if not place_fs:
rospy.logerr("No such place_pose")
return None
rospy.loginfo("Navigating to place at {0}".format(place_fs).replace('\n', ' '))
try:
x = place_fs.frame.p.x()
y = place_fs.frame.p.y()
except KeyError as ke:
rospy.logerr("Could not determine pose: ".format(ke))
return None
# Outer radius
radius -= 0.1
ro = "(x-%f)^2+(y-%f)^2 < %f^2"%(x, y, radius+0.075)
ri = "(x-%f)^2+(y-%f)^2 > %f^2"%(x, y, radius-0.075)
# pc = PositionConstraint(constraint=ri+" and "+ro, frame="/map")
# oc = OrientationConstraint(look_at=Point(x, y, 0.0), frame="/map", angle_offset=angle_offset)
pc = PositionConstraint(constraint=ri+" and "+ro, frame=place_fs.frame_id)
oc = OrientationConstraint(look_at=Point(x, y, 0.0), frame=place_fs.frame_id, angle_offset=angle_offset)
return pc, oc
class NavigateToGrasp(NavigateTo):
def __init__(self, robot, entity_designator, arm_designator=None):
super(NavigateToGrasp, self).__init__(robot, reset_head=False)
self.robot = robot
check_resolve_type(entity_designator, ed.msg.EntityInfo) #Check that the entity_designator resolves to an Entity
self.entity_designator = entity_designator
check_resolve_type(arm_designator, Arm) #Check that the arm_designator resolves to an Arm
self.arm_designator = arm_designator
if not arm_designator:
rospy.logerr('NavigateToGrasp: side should be determined by entity_designator. Please specify left or right, will default to left')
self.arm_designator = Designator(robot.leftArm)
def generateConstraint(self):
arm = self.arm_designator.resolve()
if not arm:
rospy.logerr("Could not resolve arm")
return None
if arm == self.robot.arms['left']:
angle_offset = math.atan2(-self.robot.grasp_offset.y, self.robot.grasp_offset.x)
elif arm == self.robot.arms['right']:
angle_offset = math.atan2(self.robot.grasp_offset.y, self.robot.grasp_offset.x)
radius = math.hypot(self.robot.grasp_offset.x, self.robot.grasp_offset.y)
entity = self.entity_designator.resolve()
if not entity:
rospy.logerr("No such entity")
return None
rospy.loginfo("Navigating to grasp entity id:{0}".format(entity.id))
try:
pose = entity.pose #TODO Janno: Not all entities have pose information
x = pose.position.x
y = pose.position.y
except KeyError, ke:
rospy.logerr("Could not determine pose: ".format(ke))
return None
try:
rz = entity.pose.orientation.z
except KeyError, ke:
rospy.logerr("Could not determine pose.rz: ".format(ke))
rz = 0
objects_to_grab = ["coke"]
objects_to_navigate = ["hallway_table", "dinner_table"]
objects_to_navigate_ds = []
robot.base.set_initial_pose(0.2, 5, -1.4) # not nescessary in demo extern mode
leftarm = Designator(robot.leftArm)
rightarm = Designator(robot.rightArm)
objects_to_grab.reverse() # Reverse here, because we are going to use list.pop()
objects_to_navigate.reverse() # Reverse here, because we are going to use list.pop()
for item in objects_to_navigate:
objects_to_navigate_ds.append(ds.EntityByIdDesignator(robot, id=item))
leftarm.resolve().reset()
rightarm.resolve().reset()
leftarm.resolve().send_gripper_goal('close')
rightarm.resolve().send_gripper_goal('close')
raw_input('Press enter to continue: ')
robot.base.set_initial_pose(0.2, 5, -1.4) # not nescessary in demo extern mode
lib.Amigo_EngineFirstStep()
while not rospy.is_shutdown():
lib.Amigo_EngineTimeStep(0.5)
rospy.sleep(0.5)
reset_variables()