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():
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)
def main():
rospy.init_node("demo_berkeley", anonymous=True)
sm = smach.StateMachine(outcomes=["success",
"failure"])
base_mover = base.Base()
detector = TablewareDetection()
pickplace = PickPlace()
pickplace.min_open_gripper_pos = 0.0014
rospy.loginfo("Waiting for find_cluster_bounding_box2 service")
find_box = rospy.ServiceProxy("/find_cluster_bounding_box", FindClusterBoundingBox)
world = two_arms_states.WorldState(find_box)
interface = WorldInterface()
tasks = Tasks()
placer_r = SensingPlacer('right_arm')
placer_l = SensingPlacer('left_arm')
head = Head()
rospy.loginfo("Creating State Machine")
inspector = StateInspector(two_arms_states)
inspector.world = world
inspector.tasks = tasks
inspector.base_mover = base_mover
inspector.interface = interface
inspector.detector = detector
inspector.placer_r = placer_r
inspector.placer_l = placer_l
inspector.pickplace = pickplace
inspector.head = head
inspector.find_box = find_box
with sm:
smach.StateMachine.add("reset",
inspector(two_arms_states.Resetter),
transitions={"success":"move_arms_side"})
smach.StateMachine.add("move_arms_side",
inspector(two_arms_states.MoveArmsToSide),
transitions={"success":"spinning_table_pickup"})
spinning_sm = create_spinning_table_clear_sm(inspector)
smach.StateMachine.add("spinning_table_pickup", spinning_sm,
transitions = {"success":"success",
"failure":"failure"}
)
# clean_table = create_clean_table_sm(inspector)
# smach.StateMachine.add("clean_table", clean_table,
# transitions = {"success":"success",
# "failure":"failure"
# }
# )
# setup_table = create_setup_table_sm(inspector)
# smach.StateMachine.add("setup_table", setup_table,
# transitions = {"success":"success",
# "failure":"failure"
# }
# )
sis = smach_ros.IntrospectionServer('sushi_sm', sm, '/SM_ROOT')
sis.start()
outcome = sm.execute()
rospy.loginfo("Outcome: %s", outcome)
sis.stop()
