torcsclient.cpp 2.77 KB
Newer Older
Svetlana's avatar
Svetlana committed
1 2 3 4 5 6 7 8
#include "torcsclient.h"

#include <iostream>
#include <chrono>
#include <thread>
#include <cstring>
#include <sys/shm.h>
#include <assert.h>
9
#include <ros/console.h>
Svetlana's avatar
Svetlana committed
10 11 12 13 14 15 16 17 18 19 20 21 22

using namespace torcs;

TorcsClient::TorcsClient()
{
    std::cout << "TORCS Client!" << std::endl;

    void *shm = NULL;
    int shmid;

    shmid = shmget((key_t)4567, sizeof(struct shared_use_st), 0666);
    if(shmid < 0)
    {
23 24 25
        //std::cerr << "Failed to get shared memory!" << std::endl;
        ROS_ERROR("Failed to get shared memory!");
    return;
Svetlana's avatar
Svetlana committed
26 27 28 29 30
    }

    shm = shmat(shmid, 0, 0);
    if(shm == (void*)-1)
    {
31 32
        ROS_ERROR("Failed to shmat()!");
        //std::cerr << "Failed to shmat()!" << std::endl;
Svetlana's avatar
Svetlana committed
33 34 35 36 37
        return;
    }
    
    connected = true;

38
    std::cout << "Started shared memory at " << std::hex << shm << std::endl;
Svetlana's avatar
Svetlana committed
39 40

    shared = (struct shared_use_st*)shm;
41 42 43 44
    shared->written = 0; // Equals 1 when screenshot data is written and can bea read
    shared->control = 1; // Seems to be not used by TORCS - was only needed by training framework of the original paper
    shared->pause = 0;   // Set to one to pause execution - was used in original paper
    shared->fast = 0.0;
Svetlana's avatar
Svetlana committed
45

46 47
    shared->dist_L = 0.0;
    shared->dist_R = 0.0;
Svetlana's avatar
Svetlana committed
48

49 50 51
    shared->toMarking_L = 0.0;
    shared->toMarking_M = 0.0;
    shared->toMarking_R = 0.0;
Svetlana's avatar
Svetlana committed
52

53 54 55
    shared->dist_LL = 0.0;
    shared->dist_MM = 0.0;
    shared->dist_RR = 0.0;
Svetlana's avatar
Svetlana committed
56

57 58 59 60
    shared->toMarking_LL = 0.0;
    shared->toMarking_ML = 0.0;
    shared->toMarking_MR = 0.0;
    shared->toMarking_RR = 0.0;
Svetlana's avatar
Svetlana committed
61

62 63 64
    shared->toMiddle = 0.0;
    shared->angle = 0.0;
    shared->speed = 0.0;
Svetlana's avatar
Svetlana committed
65

66 67 68
    shared->steerCmd = 0.0;
    shared->accelCmd = 0.0;
    shared->brakeCmd = 0.0;
69 70 71

}

72
void TorcsClient::sendCommand(const Command& cmd)
73
{
74 75 76
    gotCommand = true;
    command = cmd;
}
77

78 79
void TorcsClient::iterate(std::unique_ptr<DataContainer>& screenshot, float& speed)
{
80 81
    assert(connected);

82 83 84 85 86 87 88
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wmissing-noreturn"
    while (true) {
        if (shared->written == 1) { // the new image data is ready to be read
            memcpy(screenshot->data(), shared->data, screenshot->size());
            speed = float(shared->speed);

89
            if (gotCommand && shared->control == 1) {
90 91 92 93 94
                gotCommand = false;
                ROS_INFO("Command acc: %f, brake: %f, steer: %f", command.accelCmd, command.brakeCmd, command.steerCmd);
                shared->accelCmd = command.accelCmd;
                shared->steerCmd = command.steerCmd;
                shared->brakeCmd = command.brakeCmd;
95
            }
96 97

            shared->written = 0;
98
            shared->pause = 0;
99
            break;
100 101 102 103
        } else {
            if (shared->pause != 1) {
                shared->pause = 1;
            }
104 105
        }
    }
106
#pragma clang diagnostic pop
107
}