"evaluator:lost": "exit:searching",
"seeker:reached": "exit:waypoint",
"seeker:lost": "exit:searching",
}
waypoint = StateMachine(waypoint_states, waypoint_transitions, "nextGoal")
searching = StateMachine(searching_states, searching_transitions, "evalulator")
blob = StateMachine(blob_states, blob_transitions, "blobsearch")
seek = StateMachine(seek_states, seek_transitions, "evalulator")
main_states = {
"waypoint": waypoint,
"searching": searching,
"blobsearch": blob,
"seek": seek,
"end": EndSuccess(),
}
main_transitions = {
"seek": "seek",
"searching": "searching",
"waypoint": "waypoint",
"blobsearch": "blobsearch",
"end": "end",
}
main = StateMachine(main_states, main_transitions, "waypoint")
main.attach()
rospy.spin()
DIST_THRESHOLD = 5
MAX_SPEED_AT_DIST = 10 # distance at which rover should be traveling at max speed
MAX_SPEED_AT_ANGLE = math.pi / 2 # angular distance at which rover should be turning at max speed
MIN_DRIVE_SPEED = 150
MIN_TURNING_SPEED = 180
seek_states = {
"evaluator":
Evaluator(CONFIDENCE_THRESHOLD, DIST_THRESHOLD, goalTracker),
"seeker":
Seeker(CONFIDENCE_THRESHOLD, MAX_SPEED_AT_DIST, MAX_SPEED_AT_ANGLE,
MIN_DRIVE_SPEED, MIN_TURNING_SPEED_SPEED),
"failure":
EndFail(),
"success":
EndSuccess(),
}
seek_transitions = {
"evaluator:far": "failure",
"evaluator:close": "seeker",
"evaluator:lost": "success",
"seeker:reached": "exit:waypoint",
"seeker:lost": "failure",
}
seek = StateMachine(seek_states, seek_transitions, "evalulator")
seek.attach()
rospy.spin()
pose1.latitude = 32.882112
pose1.longitude = -117.2343985
pose2 = PoseStamped()
pose2.pose.position.x = 2
pose3 = PoseStamped()
pose3.pose.position.x = 2
pose3.pose.position.y = 2
goalTracker = NextGoal([pose1, pose2, pose3])
waypoint_states = {
"nextGoal":
goalTracker,
"following":
FollowingState(MAX_SPEED_AT_DIST, MAX_SPEED_AT_ANGLE, MIN_DRIVE_SPEED,
MIN_TURNING_SPEED),
"success":
EndSuccess(),
}
waypoint_transitions = {
"nextGoal:set": "following",
"nextGoal:done": "success",
"following:reached": "nextGoal",
}
waypoint = StateMachine(waypoint_states, waypoint_transitions, "nextGoal")
waypoint.attach()
rospy.spin()
