def test_filled_constructor_should_create_valid_object():
random = Command.random()
command = Command(wheels_commands=random.wheels_commands)
assert command is not None
assert command.wheels_commands is not None
assert len(command.wheels_commands) == len(random.wheels_commands)
zip_wheels_commands = zip(command.wheels_commands, random.wheels_commands)
for zip_wheels_command in zip_wheels_commands:
assert zip_wheels_command[0] == zip_wheels_command[1]
def test_clean_should_works_properly():
command = Command.random()
command.clean()
assert command is not None
assert command.wheels_commands is not None
assert len(command.wheels_commands) == 0
def send_command(self, command=Command()):
global_commands = CommandMapper.command_to_global_commands(command)
serialized = global_commands.SerializeToString()
self.socket.send(serialized)
return 0
def __build_command(self):
command = Command()
command.wheels_commands.append(WheelsCommand(10, -10))
command.wheels_commands.append(WheelsCommand(10, -10))
command.wheels_commands.append(WheelsCommand(10, -10))
return command
def __build_command(self, data, state_data, team_color):
command = Command()
command.clean()
commands_obj = json.loads(data.decode('utf-8'))
commands_obj.sort(key=lambda x: x[0], reverse=False)
last_angles = [0, 0, 0, 0, 0, 0]
allowed_ids = [0, 1, 2] if team_color == 'yellow' else [3, 4, 5]
output = []
for obj, r_pid in zip(commands_obj, self.robots_pid):
# print(state_data)
if (obj[0] not in allowed_ids):
continue
sp_lin = state_data['detection']['robots_{}'.format(team_color)][
obj[0] % 3]['linear_speed']
ang = math.degrees(state_data['detection']['robots_{}'.format(
team_color)][obj[0] % 3]['orientation'])
delta_angle = (ang - last_angles[obj[0]])
if delta_angle > 180:
delta_angle -= 360
elif delta_angle < -180:
delta_angle += 360
last_angles[obj[0]] = ang
print(obj)
sp_ang = delta_angle / r_pid.dt
r_pid.set_target(obj[2], obj[3])
wheel_right, wheel_left = r_pid.speed_to_power(sp_lin, sp_ang)
lin = wheel_right
ang = wheel_left
# print('wheels_power: ', wheel_right, wheel_left)
command.commands.append(WheelsCommand(wheel_right, wheel_left))
# Partindo do principio que havera no maximo 3 robos em cada time
mock_commands_left = 3 - len(command.commands)
for i in range(mock_commands_left):
command.commands.append(WheelsCommand(0, 0))
print(len(command.commands))
return command
def test_rand_should_create_an_valid_object():
command = Command.random()
assert command is not None
assert command.wheels_commands is not None
assert len(command.wheels_commands) > 0
for wheels_command in command.wheels_commands:
assert wheels_command is not None
def command_to_global_commands(cls, command=Command()):
global_commands = Global_Commands()
for i in range(0, 3):
robot_command = cls.__get_robot_command(command.wheels_commands[i])
global_commands.robot_commands.add()
global_commands.robot_commands[i].left_vel = robot_command.left_vel
global_commands.robot_commands[
i].right_vel = robot_command.right_vel
return global_commands
def __build_command(self, rvel):
command = Command()
command.wheels_commands.append(WheelsCommand(rvel[0][0], rvel[0][1]))
command.wheels_commands.append(WheelsCommand(rvel[1][0], rvel[1][1]))
command.wheels_commands.append(WheelsCommand(rvel[2][0], rvel[2][1]))
return command
def test_default_constructor_should_create_zero_object():
command = Command()
assert command is not None
assert command.wheels_commands is not None
assert len(command.wheels_commands) == 0