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")