return 'succeeded'
with self:
self.add('ASK_USER', CBState(_ask_user), transitions={'succeeded': 'succeeded', 'failed': 'failed'})
class NavigateToAndPickItemFromCupboardDrawer(StateMachine):
def __init__(self, robot, cupboard_id, cupboard_navigation_area, required_items):
StateMachine.__init__(self, outcomes=["succeeded", "failed"], output_keys=["item_picked"])
cupboard = EdEntityDesignator(robot=robot, id=cupboard_id)
with self:
StateMachine.add("NAVIGATE_TO_CUPBOARD",
NavigateToSymbolic(robot, {cupboard: cupboard_navigation_area}, cupboard),
transitions={'arrived': 'PICK_ITEM_FROM_CUPBOARD',
'unreachable': 'failed',
'goal_not_defined': 'failed'})
StateMachine.add("PICK_ITEM_FROM_CUPBOARD", PickItemFromCupboardDrawer(robot, cupboard_id, required_items),
transitions={'succeeded': 'succeeded',
'failed': 'failed'})
if __name__ == '__main__':
rospy.init_node(os.path.splitext("test_" + os.path.basename(__file__))[0])
hero = Hero()
hero.reset()
NavigateToAndPickItemFromCupboardDrawer(hero, 'cupboard', 'aside_of').execute()
desired_base_position = trash_bin_position + radius * trash_bin_to_base_vector
goal_pose = PoseStamped(
header=Header(stamp=rospy.Time.now(), frame_id="map"),
pose=Pose(position=Point(x=desired_base_position.x(),
y=desired_base_position.y()),
orientation=Quaternion(*quaternion_from_euler(
0, 0,
math.atan2(trash_bin_to_base_vector.y(),
trash_bin_to_base_vector.x()) -
math.pi + yaw_offset))))
ControlToPose(
robot, goal_pose,
ControlParameters(0.5, 1.0, 0.3, 0.3, 0.3, 0.02,
0.2)).execute({})
return 'done'
with self:
self.add('CONTROL_TO_TRASH_BIN',
CBState(control_to_pose),
transitions={'done': 'done'})
if __name__ == '__main__':
rospy.init_node(os.path.splitext("test_" + os.path.basename(__file__))[0])
hero = Hero()
hero.reset()
ControlToTrashBin(hero, 'trash_bin', 0.45, -0.2).execute()
hero.leftArm().send_joint_goal('grab_trash_bag')
