def sendPacket(): global udpsocket global reseting if reseting: sendBtnClicked() reseting = False packet = grSim_Packet() yellow = False if (cmbTeam.currentText()=="Yellow"): yellow = True packet.commands.isteamyellow = yellow packet.commands.timestamp = 0.0 command = packet.commands.robot_commands.add() command.id = int(edtId.text()) if edtId.text() else 0 command.wheelsspeed = (not chkVel.isChecked()) command.wheel1 = float(edtV1.text()) if edtV1.text() else 0 command.wheel2 = float(edtV2.text()) if edtV2.text() else 0 command.wheel3 = float(edtV3.text()) if edtV3.text() else 0 command.wheel4 = float(edtV4.text()) if edtV4.text() else 0 command.veltangent = float(edtVx.text()) if edtVx.text() else 0 command.velnormal = float(edtVy.text()) if edtVy.text() else 0 command.velangular = float(edtW.text()) if edtW.text() else 0 command.kickspeedx = float(edtKick.text()) if edtKick.text() else 0 command.kickspeedz = float(edtChip.text()) if edtChip.text() else 0 command.spinner = (chkSpin.isChecked()) udpsocket.sendto(packet.SerializeToString(), (_addr, _port))
def sendCommands(self, data, id): packet = grSim_Packet_pb2.grSim_Packet() now_time = (time.mktime(datetime.now().timetuple())) packet.commands.timestamp = now_time if self.friend_color == "yellow": packet.commands.isteamyellow = True else: packet.commands.isteamyellow = False commands = packet.commands.robot_commands.add() commands.id = id commands.kickspeedx = data.kick_speed_x commands.kickspeedz = data.kick_speed_z if math.isnan(data.vel_surge) \ or math.isnan(data.vel_sway) \ or math.isnan(data.omega): commands.veltangent = 0 commands.velnormal = 0 commands.velangular = 0 else: commands.veltangent = data.vel_surge commands.velnormal = data.vel_sway commands.velangular = data.omega if data.dribble_power > 0: commands.spinner = True else: commands.spinner = False commands.wheelsspeed = False message = packet.SerializeToString() self.sock.sendto(message, (self.host, self.port))
def reset_ball(self): package = sim_pkg.grSim_Packet() replacement = package.replacement ball_rep = replacement.ball ball_rep.x = np.random.randint(-600, 600) / 100.0 ball_rep.y = np.random.randint(-450, 450) / 100.0 ball_rep.vx = 0.0 ball_rep.vy = 0.0 self.socket.sendto(package.SerializeToString(), self.address)
def send_commands(self): """ Sends all robot outputs in a packet to grSim """ sim_packet = grSim_Packet() sim_packet.commands.isteamyellow = self.team_bots.team == Team.YELLOW sim_packet.commands.timestamp = 0 self.team_bots.write_output(sim_packet.commands.robot_commands) self.sender.send(sim_packet)
def send_ball_replacement(self, data): packet = grSim_Packet_pb2.grSim_Packet() replace_ball = packet.replacement.ball replace_ball.x = self.REVERSE * data.pos_x replace_ball.y = self.REVERSE * data.pos_y replace_ball.vx = self.REVERSE * data.vel_x replace_ball.vy = self.REVERSE * data.vel_y message = packet.SerializeToString() self.sock.sendto(message, (self.host, self.port))
def mouseMoveEvent(self, e): global udpsocket packet = grSim_Packet() packet.replacement.ball.x = e.x() * self.ratio / 1000 - 10400 / 1000 / 2 packet.replacement.ball.y = -e.y() * self.ratio / 1000 + 7400 / 1000 / 2 packet.replacement.ball.vx = 0 packet.replacement.ball.vy = 0 print("Clicked! {}, {}".format(packet.replacement.ball.x, packet.replacement.ball.y)) udpsocket.sendto(packet.SerializeToString(), (SEND_ADDR, SEND_PORT))
def reset_bot(self, robot_num): package = sim_pkg.grSim_Packet() replacement = package.replacement bot_rep = replacement.robots.add() bot_rep.x = np.random.randint(-600, 600) / 100.0 bot_rep.y = np.random.randint(-450, 450) / 100.0 bot_rep.dir = np.pi * np.random.randint(-180, 180) / 180.0 bot_rep.id = robot_num bot_rep.yellowteam = False self.socket.sendto(package.SerializeToString(), self.address)
def send_robot_replacement(self, data): packet = grSim_Packet_pb2.grSim_Packet() replace_robot = packet.replacement.robots.add() replace_robot.id = data.robot_id replace_robot.yellowteam = data.is_yellow replace_robot.x = self.REVERSE * data.pos_x replace_robot.y = self.REVERSE * data.pos_y replace_robot.dir = self.REVERSE_ANGLE + data.dir replace_robot.turnon = data.turn_on message = packet.SerializeToString() self.sock.sendto(message, (self.host, self.port))
def send_action(self, robot_num=0, vx=0, vy=0, w=0, kp=0): package = sim_pkg.grSim_Packet() commands = package.commands commands.timestamp = 0 commands.isteamyellow = False command = commands.robot_commands.add() command.id = robot_num command.kickspeedx = kp command.kickspeedz = 0 command.veltangent = vx command.velnormal = vy command.velangular = w command.spinner = 1 command.wheelsspeed = False self.socket.sendto(package.SerializeToString(), self.address)
def send_reset(self, robot_num=0, ifrandom=0): package = sim_pkg.grSim_Packet() replacement = package.replacement if ifrandom: x = 2 * (random() - 0.5) * 4.0 y = 2 * (random() - 0.5) * 2.5 theta = 4 * (random() - 0.5) * math.pi ball = replacement.ball ball.x = x + 0.1 ball.y = y ball.vx = 0.0 ball.vy = 0.0 for i in range(16): robot = replacement.robots.add() if i != robot_num: robot.x = i * 0.3 robot.y = -5.0 robot.id = i robot.dir = 0 robot.yellowteam = False robot.turnon = True else: if ifrandom == 0: robot.x = i * 0.3 robot.y = 0.0 robot.id = i robot.dir = 0.0 robot.yellowteam = False robot.turnon = True else: robot.x = x robot.y = y robot.id = i robot.dir = theta robot.yellowteam = False robot.turnon = True robot = replacement.robots.add() robot.x = -i * 0.3 robot.y = -5.5 robot.id = i robot.dir = 0 robot.yellowteam = True robot.turnon = True self.socket.sendto(package.SerializeToString(), self.address)
def reset(self, robot_num): package = sim_pkg.grSim_Packet() replacement = package.replacement #ball_rep = replacement.ball bot_rep = replacement.robots.add() #ball_rep.x = 100.0 #ball_rep.y = 0.0 #ball_rep.vx = 0.0 #ball_rep.vy = 0.0 bot_rep.x = 0.0 bot_rep.y = 0.0 bot_rep.dir = 0.0 bot_rep.id = robot_num bot_rep.yellowteam = False self.socket.sendto(package.SerializeToString(), self.address)
def _send_replacements(self, msg): packet = grSim_Packet_pb2.grSim_Packet() if msg.ball.is_enabled: replace_ball = packet.replacement.ball replace_ball.x = msg.ball.x replace_ball.y = msg.ball.y replace_ball.vx = msg.ball.vx replace_ball.vy = msg.ball.vy for robot in msg.robots: replace_robot = packet.replacement.robots.add() replace_robot.x = robot.x replace_robot.y = robot.y replace_robot.dir = robot.dir replace_robot.id = robot.id replace_robot.yellowteam = robot.yellowteam replace_robot.turnon = robot.turnon message = packet.SerializeToString() self._sock.sendto(message, (self._host, self._port))
def _send_commands(self, msg): packet = grSim_Packet_pb2.grSim_Packet() packet.commands.timestamp = msg.header.stamp.to_sec() packet.commands.isteamyellow = msg.is_yellow for command in msg.commands: packet_command = packet.commands.robot_commands.add() # ロボットID packet_command.id = command.robot_id # 走行速度 packet_command.veltangent = command.vel_surge if not math.isnan( command.vel_surge) else 0 packet_command.velnormal = command.vel_sway if not math.isnan( command.vel_sway) else 0 packet_command.velangular = command.vel_angular if not math.isnan( command.vel_angular) else 0 # キック速度 packet_command.kickspeedx = command.kick_power * self._MAX_KICK_SPEED # チップキック if command.chip_enable: packet_command.kickspeedz = packet_command.kickspeedx else: packet_command.kickspeedz = 0 # ドリブラー if command.dribble_power > 0: packet_command.spinner = True else: packet_command.spinner = False # タイヤ個別に速度設定しない packet_command.wheelsspeed = False message = packet.SerializeToString() self._sock.sendto(message, (self._host, self._port))
def computeAI(self): global udpsocket global shoot commandList = [] packet = grSim_Packet() packet.commands.isteamyellow = True packet.commands.timestamp = 0.0 commandList.append(packet.commands.robot_commands.add()) commandList.append(packet.commands.robot_commands.add()) resetCommand(commandList[0], 0) resetCommand(commandList[1], 1) while playAll and len(wc.teams) == 0: pass while playAll: # goalX = -3100 # goalY = 0 goalX = wc.teams[0][1].x goalY = wc.teams[0][1].y if not shoot: commandList[0].kickspeedx = 0 bX = wc.ball.x bY = wc.ball.y else: commandList[0].kickspeedx = 5 bX = goalX bY = goalY shoot = False pX = wc.teams[0][0].x pY = wc.teams[0][0].y angle = math.atan2((bY - pY), (bX - pX)) angle2 = math.atan2((bY - wc.teams[0][1].y), (bX - wc.teams[0][1].x)) if fakeOri == 0: aimAngle = -wc.teams[0][0].orientation angle = angle elif fakeOri == 1: aimAngle = angle + ida(0) angle = 0 # aimAngle = angle + math.pi / 2 # angle = math.pi + math.pi / 2 + math.pi / 4 elif fakeOri == 2: aimAngle = angle + ida(math.pi + math.pi / 2) angle = math.pi + math.pi / 2 elif fakeOri == 3: aimAngle = angle + ida(math.pi) angle = math.pi elif fakeOri == 4: aimAngle = angle + ida(math.pi / 2) angle = math.pi / 2 elif fakeOri == 5: aimAngle = -wc.teams[0][0].orientation + math.pi / 2 angle = angle elif fakeOri == 6: aimAngle = -wc.teams[0][0].orientation - math.pi / 2 angle = angle elif fakeOri == 7: # You can adjust the factor. Lower means # that it will go towards the destination # using a smaller arc aimAngle = -wc.teams[0][0].orientation + 0.5 angle = angle elif fakeOri == 8: # You can adjust the factor. Lower means # that it will go towards the destination # using a smaller arc aimAngle = -wc.teams[0][0].orientation - 0.5 angle = angle elif fakeOri == 9: bX, bY, angle, aimAngle = fetchAndRotate(bX, bY, goalX, goalY, pX, pY) elif fakeOri == 10: aimAngle = -wc.teams[0][0].orientation angle = angle bX, bY = rotatePoint(bX, bY, pX, pY, aimAngle) # angle = The angle to look towards # pX, pY = rotatePoint(pX, pY, bX, bY, -angle) tempD = computeDistance(bX, bY, pX, pY) if bX == pX: bX += 1 if bY == pY: bY += 1 ratioX = tempD / (bX - pX) ratioY = tempD / (bY - pY) # offsetX = bX - (bX - tempD) # offsetX = bY - (bY - tempD) if fakeOri != 10: commandList[0].velnormal = 1 / ratioY commandList[0].veltangent = 1 / ratioX else: commandList[0].velnormal = -1 / ratioY commandList[0].veltangent = -1 / ratioX # angle = 0 angleDiff = getAngleDiff(angle, wc.teams[0][0].orientation) angleDiff2 = getAngleDiff(angle2, wc.teams[0][1].orientation) commandList[0].velangular = angleDiff * 10 commandList[1].velangular = angleDiff2 * 10 debugP("Mode: {}".format(fakeOri)) debugP("Angle: {}".format(angle)) debugP("Diff: {}".format(angleDiff)) debugP("RatioX: {}".format(ratioX)) debugP("RatioY: {}".format(ratioY)) debugP("Ball at:") debugP("\tx: {}".format(wc.ball.x)) debugP("\ty: {}".format(wc.ball.y)) debugP("Robot 0 of {}:".format(len(wc.teams[0]))) debugP("\tx: {}".format(wc.teams[0][0].x)) debugP("\ty: {}".format(wc.teams[0][0].y)) debugP("\tOri: {}".format(wc.teams[0][0].orientation)) debugP("************") udpsocket.sendto(packet.SerializeToString(), (SEND_ADDR, SEND_PORT)) time.sleep(0.02) pass
def computeAI(self): global udpsocket global shoot commandList = [] packet = grSim_Packet() packet.commands.isteamyellow = True packet.commands.timestamp = 0.0 commandList.append(packet.commands.robot_commands.add()) commandList.append(packet.commands.robot_commands.add()) resetCommand(commandList[0], 0) resetCommand(commandList[1], 1) while playAll and len(wc.teams) == 0: pass while playAll: #goalX = -3100 #goalY = 0 goalX = wc.teams[0][1].x goalY = wc.teams[0][1].y if not shoot: commandList[0].kickspeedx = 0 bX = wc.ball.x bY = wc.ball.y else: commandList[0].kickspeedx = 5 bX = goalX bY = goalY shoot = False pX = wc.teams[0][0].x pY = wc.teams[0][0].y angle = math.atan2((bY - pY), (bX - pX)) angle2 = math.atan2((bY - wc.teams[0][1].y), (bX - wc.teams[0][1].x)) if fakeOri == 0: aimAngle = -wc.teams[0][0].orientation angle = angle elif fakeOri == 1: aimAngle = angle + ida(0) angle = 0 #aimAngle = angle + math.pi / 2 #angle = math.pi + math.pi / 2 + math.pi / 4 elif fakeOri == 2: aimAngle = angle + ida(math.pi + math.pi / 2) angle = math.pi + math.pi / 2 elif fakeOri == 3: aimAngle = angle + ida(math.pi) angle = math.pi elif fakeOri == 4: aimAngle = angle + ida(math.pi / 2) angle = math.pi / 2 elif fakeOri == 5: aimAngle = -wc.teams[0][0].orientation + math.pi / 2 angle = angle elif fakeOri == 6: aimAngle = -wc.teams[0][0].orientation - math.pi / 2 angle = angle elif fakeOri == 7: #You can adjust the factor. Lower means #that it will go towards the destination #using a smaller arc aimAngle = -wc.teams[0][0].orientation + 0.5 angle = angle elif fakeOri == 8: #You can adjust the factor. Lower means #that it will go towards the destination #using a smaller arc aimAngle = -wc.teams[0][0].orientation - 0.5 angle = angle elif fakeOri == 9: bX, bY, angle, aimAngle = fetchAndRotate( bX, bY, goalX, goalY, pX, pY) elif fakeOri == 10: aimAngle = -wc.teams[0][0].orientation angle = angle bX, bY = rotatePoint(bX, bY, pX, pY, aimAngle) #angle = The angle to look towards #pX, pY = rotatePoint(pX, pY, bX, bY, -angle) tempD = computeDistance(bX, bY, pX, pY) if (bX == pX): bX += 1 if (bY == pY): bY += 1 ratioX = tempD / (bX - pX) ratioY = tempD / (bY - pY) #offsetX = bX - (bX - tempD) #offsetX = bY - (bY - tempD) if fakeOri != 10: commandList[0].velnormal = 1 / ratioY commandList[0].veltangent = 1 / ratioX else: commandList[0].velnormal = -1 / ratioY commandList[0].veltangent = -1 / ratioX #angle = 0 angleDiff = getAngleDiff(angle, wc.teams[0][0].orientation) angleDiff2 = getAngleDiff(angle2, wc.teams[0][1].orientation) commandList[0].velangular = angleDiff * 10 commandList[1].velangular = angleDiff2 * 10 debugP("Mode: {}".format(fakeOri)) debugP("Angle: {}".format(angle)) debugP("Diff: {}".format(angleDiff)) debugP("RatioX: {}".format(ratioX)) debugP("RatioY: {}".format(ratioY)) debugP("Ball at:") debugP("\tx: {}".format(wc.ball.x)) debugP("\ty: {}".format(wc.ball.y)) debugP("Robot 0 of {}:".format(len(wc.teams[0]))) debugP("\tx: {}".format(wc.teams[0][0].x)) debugP("\ty: {}".format(wc.teams[0][0].y)) debugP("\tOri: {}".format(wc.teams[0][0].orientation)) debugP("************") udpsocket.sendto(packet.SerializeToString(), (SEND_ADDR, SEND_PORT)) time.sleep(0.02) pass