def state_turn_to_goal(timer_event=None):
# Fields
print "turning to goal"
error, idealX, maxSpeed, porp_diff = 20, 320, 110, .25
targetXDiff = D.target_coord[0] - idealX
speed = targetXDiff * porp_diff
# If target isn't a residual, turn if needed
if D.target_size > D.min_T_size:
# If we hit the sweet spot, don't turn anymore
if abs(speed) < 5:
Robo.speed =(0,0)
Robo.move(0,0)
transition(0.1, state_follow)
else:
Robo.speed = (speed,-speed)
Robo.move(speed,-speed)
transition(0.1, state_follow)
# Whelp, we lost the target
else:
Robo.speed =(0,0)
Robo.move(0,0)
HandleData.make_others_wait()
transition(0.1, state_seek)
def transition(time, next_state):
"""Transition function to help state machine."""
if str(D.robots[Robo.name]) == "stop":
print "robot lost target"
Robo.speed = (0, 0)
Robo.move(0, 0)
elif str(D.robots[Robo.name]) == "wait":
print "robot waiting on other robots"
Robo.speed = (0, 0)
Robo.move(0, 0)
elif Robo.state == "not active":
print "robot has not been activated"
Robo.speed = (0, 0)
Robo.move(0, 0)
if D.data.wheeldropCaster:
print "Wheel drop!"
print "Stopping..."
Robo.speed = (0, 0)
Robo.move(0, 0)
HandleData.make_others_wait()
#Robo.mode = "ambivalent"
rospy.signal_shutdown("robot picked up... so we're shutting down")
else:
rospy.Timer(rospy.Duration(time), next_state, oneshot=True)
def state_turn_to_goal(timer_event=None):
# Fields
print "turning to goal"
error, idealX, maxSpeed, porp_diff = 20, 320, 110, .25
targetXDiff = D.target_coord[0] - idealX
speed = targetXDiff * porp_diff
# If target isn't a residual, turn if needed
if D.target_size > D.min_T_size:
# If we hit the sweet spot, don't turn anymore
if abs(speed) < 5:
Robo.speed = (0, 0)
Robo.move(0, 0)
transition(0.1, state_follow)
else:
Robo.speed = (speed, -speed)
Robo.move(speed, -speed)
transition(0.1, state_follow)
# Whelp, we lost the target
else:
Robo.speed = (0, 0)
Robo.move(0, 0)
HandleData.make_others_wait()
transition(0.1, state_seek)
def transition(time, next_state):
"""Transition function to help state machine."""
if str(D.robots[Robo.name]) == "stop":
print "robot lost target"
Robo.speed = (0,0)
Robo.move(0,0)
elif str(D.robots[Robo.name]) == "wait":
print "robot waiting on other robots"
Robo.speed = (0,0)
Robo.move(0,0)
elif Robo.state == "not active":
print "robot has not been activated"
Robo.speed = (0,0)
Robo.move(0,0)
if D.data.wheeldropCaster:
print "Wheel drop!"
print "Stopping..."
Robo.speed = (0,0)
Robo.move(0,0)
HandleData.make_others_wait()
#Robo.mode = "ambivalent"
rospy.signal_shutdown("robot picked up... so we're shutting down")
else:
rospy.Timer(rospy.Duration(time), next_state, oneshot=True)
def activate():
"""Activates robot, or after a deactivation, reactivates."""
if Robo.state != "active":
Robo.state = "active"
print "Robot is now active"
else:
Robo.state = "not active"
print "Robot is no longer active"
Robo.speed = (0, 0)
Robo.move(0, 0)
HandleData.make_others_wait()
def state_seek(timer_event=None):
# Still can't find target
print "in state seek"
if D.target_size < D.min_T_size:
transition(0.1, state_seek)
# We found the target!
else:
print "woot we found the target"
HandleData.set_status(Robo.name, "go")
D.robots[Robo.name] = "go"
HandleData.start_up_who_can()
transition(0.1, state_follow)
def activate():
"""Activates robot, or after a deactivation, reactivates."""
if Robo.state != "active":
Robo.state = "active"
print "Robot is now active"
else:
Robo.state = "not active"
print "Robot is no longer active"
Robo.speed = (0,0)
Robo.move(0,0)
HandleData.make_others_wait()
def state_seek(timer_event=None):
# Still can't find target
print "in state seek"
if D.target_size < D.min_T_size:
transition(0.1, state_seek)
# We found the target!
else:
print "woot we found the target"
HandleData.set_status(Robo.name, "go")
D.robots[Robo.name] = "go"
HandleData.start_up_who_can()
transition(0.1, state_follow)
def state_follow(timer_event=None):
print "following"
idealSize, constantOfP, maxSpeed, error = Robo.ideal_dist, 1/100.0, 110, 20
newSpeed = constantOfP * (idealSize - D.target_size)
if newSpeed > maxSpeed:
newSpeed = maxSpeed
elif newSpeed < -maxSpeed:
newSpeed = -maxSpeed
# If we see a target
if D.target_size > D.min_T_size:
# If it is about to leave the frame, emergency direction change!
if D.target_coord[0] < 100 or D.target_coord[0] > 540:
transition(0.1,state_turn_to_goal)
#If we are alarmingly far from it, charge forward
elif Robo.ideal_dist - D.target_size > (Robo.ideal_dist / 3.0):
Robo.speed = (Robo.max_speed, Robo.max_speed)
Robo.move(Robo.max_speed, Robo.max_speed)
transition(0.1, state_follow)
# If we see the target and it's not too far, check angles
else:
# If the target is pointed in the right direction we can move
if abs(D.target_coord[0] - 320) < error:
print "in the right direction too"
# You hit the sweet spot, so don't mess it up.
if abs(newSpeed) < 5:
Robo.speed = (0,0)
Robo.move(0,0)
transition(0.1, state_follow)
else:
# OK move now
Robo.speed = (int(newSpeed), int(newSpeed))
Robo.move(int(newSpeed), int(newSpeed))
transition(0.1, state_follow)
# We see a target but aren't orientated correctly
else:
transition(0.1,state_turn_to_goal)
# We don't see a target so we need to make everyone wait on us.
else:
Robo.speed = (0,0)
Robo.move(0,0)
HandleData.make_others_wait()
transition(0.1, state_seek)
def state_follow(timer_event=None):
print "following"
idealSize, constantOfP, maxSpeed, error = Robo.ideal_dist, 1 / 100.0, 110, 20
newSpeed = constantOfP * (idealSize - D.target_size)
if newSpeed > maxSpeed:
newSpeed = maxSpeed
elif newSpeed < -maxSpeed:
newSpeed = -maxSpeed
# If we see a target
if D.target_size > D.min_T_size:
# If it is about to leave the frame, emergency direction change!
if D.target_coord[0] < 100 or D.target_coord[0] > 540:
transition(0.1, state_turn_to_goal)
#If we are alarmingly far from it, charge forward
elif Robo.ideal_dist - D.target_size > (Robo.ideal_dist / 3.0):
Robo.speed = (Robo.max_speed, Robo.max_speed)
Robo.move(Robo.max_speed, Robo.max_speed)
transition(0.1, state_follow)
# If we see the target and it's not too far, check angles
else:
# If the target is pointed in the right direction we can move
if abs(D.target_coord[0] - 320) < error:
print "in the right direction too"
# You hit the sweet spot, so don't mess it up.
if abs(newSpeed) < 5:
Robo.speed = (0, 0)
Robo.move(0, 0)
transition(0.1, state_follow)
else:
# OK move now
Robo.speed = (int(newSpeed), int(newSpeed))
Robo.move(int(newSpeed), int(newSpeed))
transition(0.1, state_follow)
# We see a target but aren't orientated correctly
else:
transition(0.1, state_turn_to_goal)
# We don't see a target so we need to make everyone wait on us.
else:
Robo.speed = (0, 0)
Robo.move(0, 0)
HandleData.make_others_wait()
transition(0.1, state_seek)
def state_start(mode, timer_event=None):
"""Starts either leader or follower behavior
depending on what mode the robot is in."""
HandleData.set_all("go")
if mode == "leader":
#Robo.mode = "leader"
print "Leading the way!"
transition(0.1, state_lead)
elif mode == "follower":
#Robo.mode = "follower"
print "Following the leader."
transition(0.1, state_follow)
else:
print "I'm feeling ambivalent right now."
def state_start(mode, timer_event=None):
"""Starts either leader or follower behavior
depending on what mode the robot is in."""
HandleData.set_all("go")
if mode == "leader":
#Robo.mode = "leader"
print "Leading the way!"
transition(0.1, state_lead)
elif mode =="follower":
#Robo.mode = "follower"
print "Following the leader."
transition(0.1, state_follow)
else:
print "I'm feeling ambivalent right now."
def get_ta_data(self):
if self.local_file:
self.data = pd.read_csv(self.local_file_path)
else:
# let's calculate proper timestamp here
timestamp_in_ms = self.input_datetime.timestamp() * 1000
self.data = HandleData.HandleData(self.records).load_ta_from_db(
self.interval, timestamp_in_ms, self.abs_path)
print(self.data.head(3))
print("*" * 40)
def conti_query():
while True:
time.sleep(0.05)
HandleData.get_status()
