def look_in_square( square, cam_pixel_to_point ): # type: (ParkingSquare, CamPixelToPointServer) -> StateMachine sq = Sequence(outcomes=['ok'], connector_outcome='ok', input_keys=['green_shape']) offset = 0.5 v = 0.2 dt = offset / v def goal(): return square.pose with sq: Sequence.add('SHORTCIRCUIT', ShortCircuitParkingSquareState(square), transitions={'shortcircuit': 'ABSORB'}) Sequence.add('PARK', park_into_pose(goal), transitions={'err': 'ABSORB'}) Sequence.add('FIND', SearchForShapeInSquareState(square, cam_pixel_to_point)) Sequence.add('MOVE_BACK', ForwardState(-v, dt)) Sequence.add('ABSORB', AbsorbResultState()) return sq
def push_cube(cube, marker): # type: (ARCube, ARTag) -> StateMachine sq = Sequence(outcomes=['ok'], connector_outcome='ok') def distance(): return np.linalg.norm( numpify(cube.pose.pose.position) - numpify(marker.pose.pose.position)) def can_see_cube(): return rospy.get_time() - cube.last_seen < 0.2 with sq: Sequence.add('MOVE_BEHIND_CUBE', navigate_behind_cube( parking_square_target(marker, offset=0.4), cube), transitions={'err': 'ABSORB'}) Sequence.add( 'PUSH', PushToGoalState(cube, parking_square_target(marker, offset=0.4), v=0.2)) Sequence.add('BACK', ForwardState(-0.2)) Sequence.add('REDO_BACK', ReturnFunctionState( lambda: 'ok' if can_see_cube() else 'back', ['ok', 'back']), transitions={'back': 'BACK'}) Sequence.add('REDO', ReturnFunctionState( lambda: 'ok' if distance() < 0.5 else 'redo', ['ok', 'redo']), transitions={'redo': 'MOVE_BEHIND_CUBE'}) Sequence.add('NOTIFY', FunctionState(lambda: notify_pushed())) Sequence.add('ABSORB', AbsorbResultState()) return sq
def search_for_tags( squares, marker, cube): # type: (List[ParkingSquare], ARTag, ARCube) -> StateMachine sm = StateMachine(outcomes=['ok']) with sm: StateMachine.add('GO_TO_AR_SEARCH_1', NavigateToNamedPoseState('ar_search_1'), transitions={ 'ok': 'SEARCH1', 'err': 'ABSORB' }) StateMachine.add('SEARCH1', FindTags(squares, marker, cube), transitions={ 'ok': 'GO_TO_AR_SEARCH_2', 'found': 'ABSORB' }) StateMachine.add('GO_TO_AR_SEARCH_2', NavigateToNamedPoseState('ar_search_2'), transitions={ 'ok': 'SEARCH2', 'err': 'ABSORB' }) StateMachine.add('SEARCH2', FindTags(squares, marker, cube), transitions={ 'ok': 'GO_TO_AR_SEARCH_1', 'found': 'ABSORB' }) StateMachine.add('ABSORB', AbsorbResultState()) return sm
def off_ramp(): # type: () -> StateMachine sq = Sequence(outcomes=['ok'], connector_outcome='ok') with sq: Sequence.add('INIT_AMCL', InitAMCLState(initial_pose=lambda: get_named_pose('initial_pose'), clear_costmaps=True)) Sequence.add('GOAL_OFFRAMP_START', NavigateToNamedPoseState('off_ramp_start'), transitions={'err': 'GOAL_OFFRAMP_END'}) Sequence.add('GOAL_OFFRAMP_END', NavigateToNamedPoseState('off_ramp_end'), transitions={'err': 'ABSORB'}) Sequence.add('ABSORB', AbsorbResultState()) return sq
def on_ramp(): # type: () -> StateMachine sq = Sequence(outcomes=['ok'], connector_outcome='ok') with sq: Sequence.add('GOAL_ONRAMP', NavigateToNamedPoseState('on_ramp'), transitions={'err': 'ABSORB'}) Sequence.add('ABSORB', AbsorbResultState()) return sq
def ar_tag(marker): # type: (ARTag) -> StateMachine sq = Sequence(outcomes=['ok'], connector_outcome='ok') with sq: # Sequence.add('AR_START', NavigateToNamedPoseState('ar_start'), transitions={'err': 'ABSORB'}) # Sequence.add('AR_FIND', FindMarkerState(marker, 'cmd_vel_mux/input/teleop')) Sequence.add('AR_GOTO', NavigateToMarkerState(marker), transitions={'err': 'ABSORB'}) Sequence.add('NOTIFY', FunctionState(notify_artag)) Sequence.add('ABSORB', AbsorbResultState()) return sq
def off_ramp(): # type: () -> StateMachine sq = Sequence(outcomes=['ok'], connector_outcome='ok') with sq: Sequence.add('GOAL_OFFRAMP_START', NavigateToNamedPoseState('off_ramp_start'), transitions={'err': 'GOAL_OFFRAMP_END'}) Sequence.add('GOAL_OFFRAMP_END', NavigateToNamedPoseState('off_ramp_end'), transitions={'err': 'ABSORB'}) Sequence.add('ABSORB', AbsorbResultState()) return sq
def go_to_marker(marker, squares): # type: (ARTag, List[ParkingSquare]) -> StateMachine def marker_square(): return closest_square(marker.pose.pose.position, squares).pose sq = Sequence(outcomes=['ok'], connector_outcome='ok') with sq: # Sequence.add('AR_START', NavigateToNamedPoseState('ar_start'), transitions={'err': 'ABSORB'}) # Sequence.add('AR_FIND', FindMarkerState(marker, 'cmd_vel_mux/input/teleop')) # Sequence.add('AR_GOTO', NavigateToMarkerState(marker), transitions={'err': 'ABSORB'}) Sequence.add('AR_GOTO', NavigateToGoalState(offset_goal(marker_square, -0.5)), transitions={'err': 'ABSORB'}) # Sequence.add('NOTIFY', FunctionState(notify_artag)) Sequence.add('FREEZE_MARKER', FunctionState(lambda: marker.freeze())) Sequence.add('ABSORB', AbsorbResultState()) return sq
def find_shape( squares, cam_pixel_to_point ): # type: (List[ParkingSquare], CamPixelToPointServer) -> StateMachine def found_shape(): return 'found' if any(s.contains_shape() for s in squares) else 'ok' sq = Sequence(outcomes=['ok'], connector_outcome='ok', input_keys=['green_shape']) with sq: Sequence.add('SQUARE1', look_in_square(squares[0], cam_pixel_to_point)) Sequence.add('SHORTCIRCUIT_1', ReturnFunctionState(found_shape, ['ok', 'found']), transitions={'found': 'ABSORB'}) Sequence.add('SQUARE2', look_in_square(squares[1], cam_pixel_to_point)) Sequence.add('SHORTCIRCUIT_2', ReturnFunctionState(found_shape, ['ok', 'found']), transitions={'found': 'ABSORB'}) Sequence.add('SQUARE3', look_in_square(squares[2], cam_pixel_to_point)) Sequence.add('SHORTCIRCUIT_3', ReturnFunctionState(found_shape, ['ok', 'found']), transitions={'found': 'ABSORB'}) Sequence.add('SQUARE4', look_in_square(squares[3], cam_pixel_to_point)) Sequence.add('SHORTCIRCUIT_4', ReturnFunctionState(found_shape, ['ok', 'found']), transitions={'found': 'ABSORB'}) Sequence.add('SQUARE5', look_in_square(squares[4], cam_pixel_to_point)) Sequence.add('SHORTCIRCUIT_5', ReturnFunctionState(found_shape, ['ok', 'found']), transitions={'found': 'ABSORB'}) Sequence.add('SQUARE6', look_in_square(squares[5], cam_pixel_to_point)) Sequence.add('SHORTCIRCUIT_6', ReturnFunctionState(found_shape, ['ok', 'found']), transitions={'found': 'ABSORB'}) Sequence.add('SQUARE7', look_in_square(squares[6], cam_pixel_to_point)) Sequence.add('SHORTCIRCUIT_7', ReturnFunctionState(found_shape, ['ok', 'found']), transitions={'found': 'ABSORB'}) Sequence.add('SQUARE8', look_in_square(squares[7], cam_pixel_to_point)) Sequence.add('SHORTCIRCUIT_8', ReturnFunctionState(found_shape, ['ok', 'found']), transitions={'found': 'ABSORB'}) Sequence.add('ABSORB', AbsorbResultState()) return sq