def main(): rospy.init_node("touring", anonymous=True) sm = smach.StateMachine(outcomes=["success", "failure"]) base_mover = base.Base() detector = TablewareDetection() pickplace = PickPlace() pickplace.min_open_gripper_pos = 0.0014 world = two_arms_states.WorldState() interface = WorldInterface() tasks = Tasks() placer_r = SensingPlacer('right_arm') placer_l = SensingPlacer('left_arm') interface.reset() psi = get_planning_scene_interface() psi.reset() tasks.move_arms_to_side() rospy.loginfo("Creating State Machine") with sm: detect = create_detect_sm(world, base_mover, detector) smach.StateMachine.add("detect", detect, transitions = {"success":"clean_table", "failure":"failure"} ) clean_table = clean_table_sm(world, tasks, base_mover, placer_l, placer_r, interface) smach.StateMachine.add("clean_table", clean_table, transitions = {"success":"setup_table", "failure":"failure" } ) setup_table = setup_table_sm(world, tasks, base_mover, placer_l, placer_r, interface) smach.StateMachine.add("setup_table", setup_table, transitions = {"success":"success", "failure":"failure" } ) outcome = sm.execute() rospy.loginfo("Outcome: %s", outcome)
def main(): tasks = Tasks() tasks.move_arms_to_side()
def main(): rospy.init_node("touring", anonymous=True) sm = smach.StateMachine(outcomes=["success", "failure"]) base_mover = base.Base() detector = TablewareDetection() pickplace = PickPlace() pickplace.min_open_gripper_pos = 0.0014 poses = basic_states.poses world = basic_states.WorldState() interface = WorldInterface() tasks = Tasks() placer = SensingPlacer('right_arm') interface.reset() psi = get_planning_scene_interface() psi.reset() tasks.move_arms_to_side() rospy.loginfo("Creating State Machine") nav_states = {} for name, pos in poses.iteritems(): nav_states[name] = basic_states.NavigateTo( world, base_mover, pos[0], pos[1], pos[2]) detect_state = basic_states.Detect(world, detector) top_pos_place_down = (-2.15, .5, .98) place_down_top = basic_states.PlaceDown(world, placer, top_pos_place_down) T = {"failure":"failure"} with sm: T["success"] = "detect" T["failure"] = "failure" smach.StateMachine.add("move_shelf", nav_states["shelf"], transitions=T.copy()) T["success"] = "choose_bowl" T["failure"] = "failure" smach.StateMachine.add("detect", detect_state, transitions=T.copy() ) T["success"] = "move_to_pickable" T["failure"] = "failure" smach.StateMachine.add("choose_bowl", basic_states.ChooseItem(world, "bowl"), transitions=T.copy() ) T["success"] = "pickup" T["failure"] = "pickup" smach.StateMachine.add("move_to_pickable", basic_states.MoveToPickable(world, base_mover ), transitions=T.copy() ) T["success"] = "move_arms_to_side" T["failure"] = "move_arms_to_side_after_pickup" smach.StateMachine.add("pickup", basic_states.PickUp(world, pickplace, interface), transitions=T.copy() ) T["success"] = "detect" T["failure"] = "failure" smach.StateMachine.add("move_arms_to_side_after_pickup", basic_states.MoveArmsToSide(world, tasks), transitions=T.copy() ) T["success"] = "move_top_table" T["failure"] = "failure" smach.StateMachine.add("move_arms_to_side", basic_states.MoveArmsToSide(world, tasks), transitions=T.copy() ) T["success"] = "approach_top_table" T["failure"] = "failure" smach.StateMachine.add("move_top_table", nav_states["table_top_edge"], transitions=T.copy()) T["success"] = "place_down_top" T["failure"] = "failure" smach.StateMachine.add("approach_top_table", basic_states.MoveToReachable(world, base_mover, top_pos_place_down), transitions=T.copy()) T["success"] = "success" T["failure"] = "failure" smach.StateMachine.add("place_down_top", place_down_top, transitions=T.copy()) outcome = sm.execute() rospy.loginfo("Outcome: %s", outcome)