예제 #1
0
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]
예제 #2
0
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
예제 #3
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    
예제 #4
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
예제 #5
0
    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
예제 #6
0
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
예제 #7
0
    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
예제 #8
0
 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
예제 #9
0
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