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