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)
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
def __init__(self):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'failed'])
with self:
smach.StateMachine.add('TEST_STATE1',
TestState("Yes", "this", "works"),
transitions={
'yes': 'TEST_STATE2',
'no': 'failed'
})
smach.StateMachine.add('TEST_STATE2',
TestState(Designator("Also"), "works",
Designator("with designators")),
transitions={
'yes': 'succeeded',
'no': 'failed'
})
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)
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
def setup_state_machine(robot):
sm = smach.StateMachine(outcomes=['Done', 'Aborted'])
pointing_des = Designator()
with sm:
smach.StateMachine.add('DETECT_POINTING',
PointingDetector(robot, pointing_des, ""),
transitions={'succeeded': 'Done',
'failed': 'Aborted'})
return sm
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
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 __init__(self, robot):
StateMachine.__init__(self, outcomes=["succeeded", "failed"])
arm = Designator(robot.leftArm)
with self:
StateMachine.add("FIND_CUP",
CustomFindCup(robot),
transitions={
'succeeded': 'GRAB',
'failed': 'failed'
})
StateMachine.add("GRAB",
SimpleGrab(robot, arm),
transitions={
'succeeded': 'succeeded',
'failed': 'failed'
})
def __init__(self, robot):
StateMachine.__init__(self, outcomes=["succeeded", "failed"])
@cb_interface(outcomes=['succeeded'], output_keys=['position'])
def send_point(ud):
ud.position = PointStamped(
header=Header(frame_id='/amigo/base_link'),
point=Point(0.5, 0, 0.7))
return 'succeeded'
arm = Designator(robot.leftArm)
with self:
StateMachine.add("SEND_POINT",
CBState(send_point),
transitions={'succeeded': 'FIND_CUP'})
StateMachine.add("FIND_CUP",
SimpleGrab(robot, arm),
transitions={
'succeeded': 'succeeded',
'failed': 'failed'
})
def __init__(self, robot, dishwasher_id, dishwasher_navigate_area):
StateMachine.__init__(self, outcomes=["succeeded", "failed"])
dishwasher = EdEntityDesignator(robot=robot, id=dishwasher_id)
arm = Designator(robot.leftArm)
with self:
StateMachine.add("NAVIGATE_TO_DISHWASHER",
NavigateToSymbolic(
robot, {dishwasher: dishwasher_navigate_area},
dishwasher),
transitions={
'arrived': 'OPEN_DISHWASHER',
'unreachable': 'failed',
'goal_not_defined': 'failed'
})
StateMachine.add("OPEN_DISHWASHER",
CustomPlace(robot, dishwasher_id, arm),
transitions={
'succeeded': 'succeeded',
'failed': 'failed'
})
from robot_smach_states.util.designators import Designator, VariableDesignator
# def setup_statemachine(robot):
# sm = smach.StateMachine(outcomes=['Done','Aborted'])
# with sm:
# smach.StateMachine.add('DRIVE',
# states.NavigateToPose(robot, x = 1, y = 3, rz = -1.57),
# transitions={ 'arrived':'Done',
# 'unreachable':'Aborted',
# 'goal_not_defined':'Aborted'})
############################## initializing program ##############################
if __name__ == '__main__':
rospy.init_node('test_exec')
if len(sys.argv) > 1:
robot_name = sys.argv[1]
if robot_name == 'sergio':
robot = Sergio()
elif robot_name == 'amigo':
robot = Amigo()
else:
robot = Amigo()
#testexec = states.NavigateToPose(robot, 3, 1, -1.57, 0.15)
testexec = states.NavigateToObserve(robot, Designator("dinner_table"))
testexec.execute()
#startup(setup_statemachine, robot_name=robot_name)
def setup_statemachine(robot):
item = VariableDesignator(resolve_type=Entity)
arm = Designator(robot.leftArm)
sm = StateMachine(outcomes=['done'])
with sm:
# Start challenge via StartChallengeRobust
StateMachine.add('START_CHALLENGE_ROBUST',
StartChallengeRobust(robot, STARTING_POINT, use_entry_points=True),
transitions={'Done': 'OPEN_DISHWASHER',
'Aborted': 'done',
'Failed': 'OPEN_DISHWASHER'})
StateMachine.add('OPEN_DISHWASHER', NavigateAndOpenDishwasher(robot),
transitions={'succeeded': 'GRAB',
'failed': 'GRAB'})
# StateMachine.add('FIND_OBJECT', FindObject(robot, item),
# transitions={'found': 'GRAB',
# 'not_found': 'SAY_EXIT'})
#
# StateMachine.add('GRAB', Grab(robot, item, arm),
# transitions={'done': 'NAVIGATE_BACK_TO_DISHWASHER',
# 'failed': 'FIND_OBJECT2'})
#
# StateMachine.add('FIND_OBJECT2', FindObject(robot, item),
# transitions={'found': 'GRAB2',
# 'not_found': 'SAY_EXIT'})
#
# StateMachine.add('GRAB2', Grab(robot, item, arm),
# transitions={'done': 'NAVIGATE_BACK_TO_DISHWASHER',
# 'failed': 'SAY_EXIT'})
StateMachine.add('GRAB', GrabRobust(robot),
transitions={'succeeded': 'PLACE_DISHWASHER',
'failed': 'SAY_EXIT'})
StateMachine.add('PLACE_DISHWASHER', NavigateAndPlaceDishwasher(robot),
transitions={'succeeded': 'SAY_EXIT',
'failed': 'SAY_EXIT'})
StateMachine.add('SAY_EXIT', Say(robot, ["I will move to the exit now. See you guys later!"], block=False),
transitions={'spoken': 'GO_TO_EXIT'})
StateMachine.add('GO_TO_EXIT',
NavigateToWaypoint(robot, EntityByIdDesignator(robot, id=EXIT_1), radius=0.7),
transitions={'arrived': 'done',
'unreachable': 'GO_TO_EXIT_2',
'goal_not_defined': 'GO_TO_EXIT_2'})
StateMachine.add('GO_TO_EXIT_2',
NavigateToWaypoint(robot, EntityByIdDesignator(robot, id=EXIT_2), radius=0.5),
transitions={'arrived': 'done',
'unreachable': 'GO_TO_EXIT_3',
'goal_not_defined': 'GO_TO_EXIT_3'})
StateMachine.add('GO_TO_EXIT_3',
NavigateToWaypoint(robot, EntityByIdDesignator(robot, id=EXIT_3), radius=0.5),
transitions={'arrived': 'done',
'unreachable': 'done',
'goal_not_defined': 'done'})
return sm
# ############################# initializing program ######################### #
if __name__ == '__main__':
global objects_to_grab, leftarm, rightarm, coke, table1, table2, robot
global objects_to_navigate_ds
rospy.init_node('supervisor_wrapper', log_level=rospy.DEBUG)
robot = robot_constructor("amigo")
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
def __init__(self, robot):
StateMachine.__init__(self, outcomes=['succeeded', 'failed'])
arm = Designator(robot.leftArm)
@cb_interface(outcomes=['done'])
def pre_place_pose(ud):
robot.torso.high()
goal_pose = PoseStamped()
goal_pose.header.stamp = rospy.Time.now()
goal_pose.header.frame_id = 'map'
goal_pose.pose.position.x = -4.55638664735
goal_pose.pose.position.y = 4.99959353359
goal_pose.pose.orientation = Quaternion(
*quaternion_from_euler(0, 0, 0.395625992))
ControlToPose(
robot, goal_pose,
ControlParameters(0.5, 1.0, 0.3, 0.3, 0.3, 0.01,
0.1)).execute({})
robot.torso.wait_for_motion_done()
robot.speech.speak('I am now raising my arm')
arm.resolve()._send_joint_trajectory(
[[-1.1, 0.044, 0.80, 0.297, 0.934, -0.95, 0.4]],
timeout=rospy.Duration(0))
arm.resolve().wait_for_motion_done()
goal_pose.pose.orientation = Quaternion(
*quaternion_from_euler(0, 0, 1.9656259917))
ControlToPose(
robot, goal_pose,
ControlParameters(0.5, 1.0, 0.3, 0.3, 0.3, 0.01,
0.1)).execute({})
arm.resolve().wait_for_motion_done()
arm.resolve()._send_joint_trajectory(
[[-0.92, 0.044, 0.80, 0.497, 0.934, -0.95, 0.4]],
timeout=rospy.Duration(0))
arm.resolve().wait_for_motion_done()
rospy.sleep(1.0)
return 'done'
@cb_interface(outcomes=['done'])
def place_pose(ud):
robot.torso._send_goal([0.35])
robot.torso.wait_for_motion_done()
rospy.sleep(1.0)
return 'done'
@cb_interface(outcomes=['done'])
def open_gripper(ud):
arm.resolve().send_gripper_goal("open", timeout=0)
rospy.sleep(3.0)
robot.speech.speak("This is where I usually place my cup")
return 'done'
@cb_interface(outcomes=['done'])
def move_away(ud):
robot.torso.high()
robot.torso.wait_for_motion_done()
arm.resolve()._send_joint_trajectory(
[[-1.1, 0.044, 0.80, 0.297, 0.934, -0.95, 0.4]],
timeout=rospy.Duration(0))
arm.resolve().wait_for_motion_done()
goal_pose = PoseStamped()
goal_pose.header.stamp = rospy.Time.now()
goal_pose.header.frame_id = 'map'
goal_pose.pose.position.x = -4.47331285184
goal_pose.pose.position.y = 5.25921160575
goal_pose.pose.orientation = Quaternion(
*quaternion_from_euler(0, 0, 1.0))
ControlToPose(
robot, goal_pose,
ControlParameters(0.5, 1.0, 0.3, 0.3, 0.3, 0.01,
0.1)).execute({})
return 'done'
@cb_interface(outcomes=['done'])
def reset_arms(ud):
robot.torso.reset()
arm.resolve().reset()
robot.torso.wait_for_motion_done()
arm.resolve().wait_for_motion_done()
return 'done'
with self:
self.add_auto('PRE_PLACE_POSE', CBState(pre_place_pose), ['done'])
self.add_auto('PLACE_POSE', CBState(place_pose), ['done'])
self.add_auto('OPEN_GRIPPER', CBState(open_gripper), ['done'])
self.add_auto('MOVE_AWAY', CBState(move_away), ['done'])
self.add('RESET_ARMS',
CBState(reset_arms),
transitions={'done': 'succeeded'})