def ToChase(self):
mode = self.robot.strategy_mode
if mode == "Defense":
self.robot.toChase("Classic")
elif mode == "Attack":
if self.robot.CheckBallHandle():
self.ToMovement()
else:
self.robot.toChase("Straight")
else:
log("Unknown Chase Mode")
def on_toPush(self):
state = self.game_state
t = self.GetObjectInfo()
position = self.GetRobotInfo()
if state == "Penalty_Kick":
x, y, yaw = self.BK.GuardPenalting(t['ball']['dis'],\
t['ball']['ang'])
else:
x, y, yaw = self.BK.ClassicPushing(t['ball']['dis'],\
t['ball']['ang'],position['location']['yaw'])
self.MotionCtrl(x, y, yaw)
log("To Push")
def main(argv):
rospy.init_node('core', anonymous=True)
rate = rospy.Rate(50)
if SysCheck(argv) == "Native Mode":
log("Start Native")
robot = Core(1)
elif SysCheck(argv) == "Simulative Mode":
log("Start Sim")
robot = Core(1, True)
while not rospy.is_shutdown():
robot.Brain()
rate.sleep()
def on_toBlock(self, methods="Classic"):
t = self.GetObjectInfo()
position = self.GetRobotInfo()
if methods == "Classic":
log("to block classic")
x, y, yaw = self.BK.ClassicBlocking(t['ball']['dis'],\
t['ball']['ang'],\
position['location']['yaw'],\
t['ball']['speed_pwm_x'], t['ball']['speed_pwm_y'],\
self.cp_value)
elif methods == "Limit":
log("to block limit")
x = 0
y = 0
yaw = 0
self.MotionCtrl(x, y, yaw)
def on_toRet(self):
t = self.GetObjectInfo()
position = self.GetRobotInfo()
twopoint = self.GetTwopoint()
if self.locate:
x, y, yaw, arrived = self.BC.Go2Point(self.run_x, self.run_y,
self.run_yaw)
else:
x, y, yaw = self.BK.Return(t[self.our_side]['dis'],
t[self.our_side]['ang'],
position['location']['yaw'],
self.cp_value)
arrived = 0
self.MotionCtrl(x, y, yaw)
log("Returnig")
return arrived
def on_toPoint(self):
if self.game_state == "Kick_Off" and self.our_side == "Yellow" :
x, y, yaw, arrived = self.BC.Go2Point(-60, 0, 0)
elif self.game_state == "Kick_Off" and self.our_side == "Blue" :
x, y, yaw, arrived = self.BC.Go2Point(60, 0, 180)
elif self.game_state == "Free_Kick" :
x, y, yaw, arrived = self.BC.Go2Point(100, 100, 90)
elif self.game_state == "Free_Ball" :
x, y, yaw, arrived = self.BC.Go2Point(-100, -100, 180)
elif self.game_state == "Throw_In" :
x, y, yaw, arrived = self.BC.Go2Point(-100, -100, 270)
elif self.game_state == "Coner_Kick":
x, y, yaw, arrived = self.BC.Go2Point(300, 200, 45)
elif self.game_state == "Penalty_Kick" :
x, y, yaw, arrived = self.BC.Go2Point(-100, 100, 135)
elif self.game_state == "Run_Specific_Point" :
x, y, yaw, arrived = self.BC.Go2Point(self.run_x, self.run_y, self.run_yaw)
else:
log("Unknown Game State")
self.MotionCtrl(x, y, yaw)
return arrived
def main(self, argv):
rospy.init_node('core', anonymous=True)
rate = rospy.Rate(1000)
dsrv = Server(GameStateConfig, self.Callback)
if SysCheck(argv) == "Native Mode":
log("Start Native")
robot = Core(1)
elif SysCheck(argv) == "Simulative Mode":
log("Start Sim")
robot = Core(1, True)
while not rospy.is_shutdown():
robot.PubCurrentState()
targets = robot.GetObjectInfo()
if targets is not None:
if not robot.is_idle and not self.game_start:
robot.toIdle()
elif robot.is_idle and self.game_start:
robot.toChase(targets, self.side)
elif robot.is_chase:
robot.toChase(targets, self.side)
if robot.is_chase and abs(targets['ball']['ang']) <= 20 \
and targets['ball']['dis'] <= 50:
robot.toAttack(targets, self.side)
elif robot.is_attack:
robot.toAttack(targets, self.side)
if robot.is_attack and abs(targets['ball']['ang']) > 20 \
and targets['ball']['dis'] > 50:
robot.toChase(targets, self.side)
if robot.is_attack and abs(targets[self.side]['ang']) < 10:
robot.toShoot(3, 1)
if robot.is_shoot:
robot.toAttack(targets, self.side)
if rospy.is_shutdown():
log('shutdown')
break
rate.sleep()
self.ToChase()
elif abs(targets[self.robot.opp_side]['ang']) < 3:
self.robot.toShoot(100)
else:
self.ToAttack()
if self.robot.is_shoot:
self.ToAttack()
>>>>>>> w1997607/master
## Run point
if self.robot.is_point:
self.RunStatePoint()
if rospy.is_shutdown():
log('shutdown')
break
self.rate.sleep()
if __name__ == '__main__':
try:
if SysCheck(sys.argv[1:]) == "Native Mode":
log("Start Native")
s = Strategy(1, False)
a=0
elif SysCheck(sys.argv[1:]) == "Simulative Mode":
log("Start Sim")
s = Strategy(1, True)
<<<<<<< HEAD
a=1
def main(self):
while not rospy.is_shutdown():
self.robot.PubCurrentState()
targets = self.robot.GetObjectInfo()
position = self.robot.GetRobotInfo()
if targets is None or targets['ball'][
'ang'] == 999 and self.game_start: # Can not find ball when starting
print("Can not find ball")
self.robot.toIdle()
else:
if not self.robot.is_idle and not self.run_point and not self.game_start:
self.robot.toIdle()
if self.robot.is_idle:
if self.game_start:
self.Chase(targets)
elif self.run_point:
self.RunStatePoint(self.game_state)
if self.robot.is_chase:
if self.robot.CheckBallHandle():
if self.strategy_mode == "Attack":
self.robot.toOrbit(targets, self.opp_side)
elif self.strategy_mode == "Defense":
self.robot.toAttack(targets, self.opp_side)
else:
self.Chase(targets)
if self.robot.is_orbit:
if abs(targets[self.opp_side]
['ang']) < self.orb_attack_ang:
self.robot.toAttack(targets, self.opp_side)
elif not self.robot.CheckBallHandle():
self.Chase(targets)
else:
self.robot.toOrbit(targets, self.opp_side)
if self.robot.is_attack:
if not self.robot.CheckBallHandle():
self.Chase(targets)
elif abs(targets[self.opp_side]
['ang']) < self.atk_shoot_ang:
self.robot.toShoot(3, 1)
else:
self.robot.toAttack(targets, self.opp_side)
if self.robot.is_shoot:
self.robot.toAttack(targets, self.opp_side)
## Run point
if self.robot.is_point:
self.RunStatePoint(self.game_state)
if rospy.is_shutdown():
log('shutdown')
break
self.rate.sleep()
def main(self):
while not rospy.is_shutdown():
self.robot.PubCurrentState()
self.robot.Supervisor()
targets = self.robot.GetObjectInfo()
position = self.robot.GetRobotInfo()
mode = self.robot.strategy_mode
state = self.robot.game_state
laser = self.robot.GetObstacleInfo()
point = self.robot.run_point
# Can not find ball when starting
if targets is None or targets['ball'][
'ang'] == 999 and self.robot.game_start:
print("Can not find ball")
self.robot.toIdle()
else:
if not self.robot.is_idle and not self.robot.game_start:
self.robot.toIdle()
if self.robot.is_idle:
if self.robot.game_start:
if self.robot.shooting_start:
if self.robot.CheckBallHandle():
self.robot.RobotShoot(80, 1)
else:
x = time.time()
while 1:
self.robot.MotionCtrl(30, 0, 0)
if (time.time() - x) > 1: break
self.dclient.update_configuration(
{"shooting_start": False})
elif state == "Penalty_Kick":
self.robot.record_angle()
self.ToMovement()
elif self.robot.run_point == "empty_hand":
self.RunStatePoint()
else:
print('idle to chase')
self.ToChase()
if self.robot.is_chase:
#log(self.robot.dest_angle)
if self.robot.CheckBallHandle():
print('chase to move')
self.ToMovement()
else:
self.ToChase()
if self.robot.is_movement:
if state == "Penalty_Kick":
if self.robot.left_ang <= self.robot.atk_shoot_ang:
log("stop")
self.robot.toShoot(100)
self.dclient.update_configuration(
{"game_state": "Kick_Off"})
else:
self.ToMovement()
elif mode == 'At_Orbit':
if abs(targets[self.robot.opp_side]
['ang']) < self.robot.orb_attack_ang:
self.ToAttack()
elif not self.robot.CheckBallHandle():
self.ToChase()
else:
self.ToMovement()
elif mode == 'At_Post_up':
if targets[self.robot.opp_side][
'dis'] <= self.robot.atk_shoot_dis:
self.ToAttack()
elif not self.robot.CheckBallHandle():
self.ToChase()
else:
self.ToMovement()
elif mode == "Defense_ball" or mode == "Defense_goal":
if self.robot.CheckBallHandle():
self.dclient.update_configuration(
{"strategy_mode": "Fast_break"})
self.dclient.update_configuration(
{"attack_mode": "Attack"})
self.ToChase()
else:
self.ToMovement()
elif mode == "Fast_break":
self.ToAttack()
if self.robot.is_attack:
if not self.robot.CheckBallHandle():
self.robot.last_goal_dis = 0
self.ToChase()
elif abs(targets[self.robot.opp_side]['ang']) < self.robot.atk_shoot_ang and \
abs(targets[self.robot.opp_side]['dis']) < self.robot.atk_shoot_dis:
self.robot.toShoot(100)
else:
self.ToAttack()
if self.robot.is_shoot:
self.ToAttack()
## Run point
if self.robot.is_point:
if point == "ball_hand":
if self.robot.CheckBallHandle():
self.RunStatePoint()
else:
self.ToChase()
else:
self.RunStatePoint()
if rospy.is_shutdown():
log('shutdown')
break
self.rate.sleep()
def on_toAttack(self, t, side ,l,a):
x, y, yaw = self.AC.ClassicAttacking(t[side]['dis'], t[side]['ang'], l['ranges'], l['angle']['increment'],a)
# self.Accelerate(1, t, 80)
self.MotionCtrl(x, y, yaw)
log('attack')
def Brain(self):
obj = self.GetObjectInfo()
if obj['ball']['dis'] is None:
log("NONE")
else:
print("Ball: {}\tCyan Goal: {}\tMagenta Goal: {}".format(obj['ball'], obj['cyan_goal'], obj['magenta_goal']))
def on_go_back(self):
log("I'm going back.")
def on_go_forward(self):
log("I'm running~")
def on_enter(self):
log("I'm running~")
def on_toIdle(self):
self.goal_dis = 0
for i in range(0, 10):
self.MotionCtrl(0,0,0)
log("To Idle1")
def main(self):
while not rospy.is_shutdown():
self.robot.PubCurrentState()
targets = self.robot.GetObjectInfo()
position = self.robot.GetRobotInfo()
if self.robot.game_level == "Level1":
level = self.robot.level1
elif self.robot.game_level == "Level2":
level = self.robot.level2
elif self.robot.game_level == "Level3":
level = self.robot.level3
elif self.robot.game_level == "Level4":
level = self.robot.level4
if not self.robot.is_idle and not self.robot.game_start:
self.robot.toIdle()
if self.robot.is_idle:
if self.robot.game_start:
print("This level is ", self.robot.game_level)
Strategy.current_index = 0
Strategy.can_shoot = False
Strategy.current_rV = 8
Strategy.current_rW = 2
self.UpdateCurrentPoint(level['balls_point'][Strategy.current_index][0], level['balls_point'][Strategy.current_index][1], 180)
self.robot.toPoint(Strategy.current_point[0], Strategy.current_point[1], Strategy.current_point[2], Strategy.current_rV, Strategy.current_rW)
if self.robot.is_aim:
if self.robot.toAim():
self.robot.toShoot(self.robot.passing_power)
else:
self.robot.toAim()
if self.robot.is_point:
if self.robot.toPoint(Strategy.current_point[0], Strategy.current_point[1], Strategy.current_point[2], Strategy.current_rV, Strategy.current_rW):
if self.robot.CheckBallHandle():
if Strategy.can_shoot:
double_check = True
print("This level color is ", level['targets_color'][Strategy.current_index])
if level['targets_color'][Strategy.current_index] == 'red' and self.robot.target_vision_red:
if abs(targets['Red']['ang']) > 2:
double_check = False
Strategy.aim_target = "Red"
if level['targets_color'][Strategy.current_index] == 'blue' and self.robot.target_vision_blue:
if abs(targets['Blue']['ang']) > 2:
double_check = False
Strategy.aim_target = "Blue"
if level['targets_color'][Strategy.current_index] == 'yellow' and self.robot.target_vision_yellow:
print("Using vision: ", targets['Yellow']['ang'])
if abs(targets['Yellow']['ang']) > 2:
double_check = False
Strategy.aim_target = "Yellow"
if level['targets_color'][Strategy.current_index] == 'white' and self.robot.target_vision_white:
if abs(targets['White']['ang']) > 2:
double_check = False
Strategy.aim_target = "White"
if double_check:
self.robot.toShoot(self.robot.passing_power)
else:
print("Check again")
self.robot.toAim()
else:
self.UpdateCurrentPoint(level['targets_point'][Strategy.current_index][0], level['targets_point'][Strategy.current_index][1], 0)
if self.robot.using_orbit:
self.robot.toMovement(0 - position['location']['yaw'])
else:
Strategy.current_rV = 5
Strategy.current_rW = 1
self.robot.toPoint(Strategy.current_point[0], Strategy.current_point[1], Strategy.current_point[2], Strategy.current_rV, Strategy.current_rW)
Strategy.can_shoot = True
else:
self.robot.toChase("Straight")
else:
self.robot.toPoint(Strategy.current_point[0], Strategy.current_point[1], Strategy.current_point[2], Strategy.current_rV, Strategy.current_rW)
if self.robot.is_chase:
if self.robot.CheckBallHandle():
#cpx = (level['balls_point'][Strategy.current_index][0] + level['targets_point'][Strategy.current_index][0])*0.5
#cpy = (level['balls_point'][Strategy.current_index][1] + level['targets_point'][Strategy.current_index][1])*0.5
#cpw = 90
cpx = level['targets_point'][Strategy.current_index][0]
cpy = level['targets_point'][Strategy.current_index][1]
cpw = 0
self.UpdateCurrentPoint(cpx, cpy, cpw)
self.robot.toPoint(Strategy.current_point[0], Strategy.current_point[1], Strategy.current_point[2])
else:
self.robot.toChase("Straight")
if self.robot.is_movement:
if not self.robot.CheckBallHandle():
self.robot.toChase("Straight")
elif self.robot.toMovement(0 - position['location']['yaw']):
self.robot.toPoint(Strategy.current_point[0], Strategy.current_point[1], Strategy.current_point[2])
Strategy.can_shoot = True
else:
self.robot.toMovement(0 - position['location']['yaw'])
if self.robot.is_shoot:
if Strategy.current_index + 1 >= len(level['balls_point']):
print("Endgame")
self.dclient.update_configuration({"game_start": False})
self.robot.toIdle()
else:
Strategy.current_index += 1
Strategy.can_shoot = False
self.UpdateCurrentPoint(level['balls_point'][Strategy.current_index][0], level['balls_point'][Strategy.current_index][1], 180)
self.robot.toPoint(Strategy.current_point[0], Strategy.current_point[1], Strategy.current_point[2])
if rospy.is_shutdown():
log('shutdown')
break
self.rate.sleep()
break
self.robot.RobotCtrlS(0, lim_v * -1, 0)
print("Move Stop")
else:
print(req)
res.nav_res = 'finish'
return res
def web_customer(self, req):
res = AddDiagnosticsResponse()
self.publish_status.publish(req.load_namespace)
res.message = "Receive Order, Please Wait a minute"
return res
if __name__ == '__main__':
try:
if SysCheck(sys.argv[1:]) == "Native Mode":
log("Start Native")
s = Strategy(False)
elif SysCheck(sys.argv[1:]) == "Simulative Mode":
log("Start Sim")
s = Strategy(True)
# Initializes a rospy node so that the SimpleActionClient can
# publish and subscribe over ROS.
# print "Result:", ', '.join([str(n) for n in result.sequence])
except rospy.ROSInterruptException:
print("program interrupted before completion")
pass
def on_toIdle(self):
log("To Idle")
