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