torcsclient.h 4.96 KB
Newer Older
1 2 3 4 5 6 7
#pragma once

#include <cstdint>
#include <array>
#include <memory>
#include <functional>
#include <std_msgs/Float32MultiArray.h>
8
#include <atomic>
9

10 11
#define DEBUG_LOG false

Svetlana's avatar
Svetlana committed
12 13 14 15
constexpr auto ROS_IMAGE_WIDTH = 280u;
constexpr auto ROS_IMAGE_HEIGHT = 210u;
constexpr auto ROS_IMAGE_SIZE = ROS_IMAGE_WIDTH*ROS_IMAGE_HEIGHT*3u;

16 17 18 19 20 21 22 23
namespace torcs
{

constexpr auto IMAGE_WIDTH = 640u;
constexpr auto IMAGE_HEIGHT = 480u;
constexpr auto IMAGE_CHANNELS = 3u;
constexpr auto IMAGE_SIZE_BYTES = IMAGE_WIDTH*IMAGE_HEIGHT*IMAGE_CHANNELS;

Svetlana's avatar
Svetlana committed
24 25 26
constexpr auto SEMANTIC_WIDTH = 320;
constexpr auto SEMANTIC_HEIGHT = 660;

27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102
struct shared_use_st
{
    int written = 0;  //a label, if 1: available to read, if 0: available to write
    uint8_t data[IMAGE_SIZE_BYTES];  // image data field
    int control = 0;
    int pause = 0;
    double fast = 0.0;

    double dist_L = 0.0;
    double dist_R = 0.0;

    double toMarking_L = 0.0;
    double toMarking_M = 0.0;
    double toMarking_R = 0.0;

    double dist_LL = 0.0;
    double dist_MM = 0.0;
    double dist_RR = 0.0;

    double toMarking_LL = 0.0;
    double toMarking_ML = 0.0;
    double toMarking_MR = 0.0;
    double toMarking_RR = 0.0;

    double toMiddle = 0.0;
    double angle = 0.0;
    double speed = 0.0;

    double steerCmd = 0.0;
    double accelCmd = 0.0;
    double brakeCmd = 0.0;
};

struct Command
{
    Command() = default;
    Command(float accel, float steer, float brake)
        : accelCmd(accel), steerCmd(steer), brakeCmd(brake)
    {
    }

    float accelCmd = 0.f;
    float steerCmd = 0.f;
    float brakeCmd = 0.f;
};

using DataContainer = std::array<uint8_t, IMAGE_SIZE_BYTES>;

class Affordance
{
public:
    void populate(const shared_use_st& affordance)
    {
        affordance_[0] = float(affordance.fast);
        affordance_[1] = float(affordance.dist_L);
        affordance_[2] = float(affordance.dist_R);
        affordance_[3] = float(affordance.toMarking_L);
        affordance_[4] = float(affordance.toMarking_M);
        affordance_[5] = float(affordance.toMarking_R);
        affordance_[6] = float(affordance.dist_LL);
        affordance_[7] = float(affordance.dist_MM);
        affordance_[8] = float(affordance.dist_RR);
        affordance_[9] = float(affordance.toMarking_LL);
        affordance_[10] = float(affordance.toMarking_ML);
        affordance_[11] = float(affordance.toMarking_MR);
        affordance_[12] = float(affordance.toMarking_RR);
        affordance_[13] = float(affordance.toMiddle); // not used
        affordance_[14] = float(affordance.angle);
        affordance_[15] = float(affordance.speed);
    }

    void populate(const std_msgs::Float32MultiArray& affordance)
    {
        affordance_ = affordance.data;
    }

Svetlana's avatar
Svetlana committed
103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118
    float getFast() const { return affordance_[0]; }
    float getDistL() const { return affordance_[1]; }
    float getDistR() const { return affordance_[2]; }
    float getToMarkingL() const { return affordance_[3]; }
    float getToMarkingM() const { return affordance_[4]; }
    float getToMarkingR() const { return affordance_[5]; }
    float getDistLL() const { return affordance_[6]; }
    float getDistMM() const { return affordance_[7]; }
    float getDistRR() const { return affordance_[8]; }
    float getToMarkingLL() const { return affordance_[9]; }
    float getToMarkingML() const { return affordance_[10]; }
    float getToMarkingMR() const { return affordance_[11]; }
    float getToMarkingRR() const { return affordance_[12]; }
    float getToMiddle() const { return affordance_[13]; }
    float getAngle() const { return affordance_[14]; }
    float getSpeed() const { return affordance_[15]; }
119 120 121 122 123 124 125 126 127 128 129 130 131 132

    const std::vector<float>& getAsVector()
    {
        return affordance_;
    }

private:
    std::vector<float> affordance_ = std::vector<float>(16);
};

class TorcsClient
{
public:
    explicit TorcsClient();
Svetlana's avatar
Svetlana committed
133
    ~TorcsClient();
134 135 136 137

    void setPublishCallback(std::function<void(std::shared_ptr<DataContainer>, Affordance)> callback);
    void iterate();
    void sendCommand(const Command& cmd);
Svetlana's avatar
Svetlana committed
138
    void setLanesCount(int count);
Svetlana's avatar
Svetlana committed
139
    void visualizePredictions(const Affordance& affordance);
140 141 142 143 144 145 146 147

private:

    struct shared_use_st *shared;
    bool connected = false;
    std::atomic<bool> gotCommand;
    Command command;
    std::function<void(std::shared_ptr<DataContainer>, Affordance)> publishCallback;
Svetlana's avatar
Svetlana committed
148 149
    int lanesCount = 3; // Use three lanes configuration by default

Svetlana's avatar
Svetlana committed
150 151 152 153 154 155
    void visualize(float angle, float true_angle, float toMarking_ML, float true_toMarking_ML,
                   float toMarking_MR, float true_toMarking_MR, float toMarking_M, float true_toMarking_M,
                   float toMarking_LL, float true_toMarking_LL, float toMarking_R, float true_toMarking_R,
                   float toMarking_L, float true_toMarking_L, float toMarking_RR, float true_toMarking_RR,
                   float dist_LL, float true_dist_LL, float dist_RR, float true_dist_RR, float dist_MM, float true_dist_MM,
                   float dist_L, float true_dist_L, float dist_R, float true_dist_R);
Svetlana's avatar
Svetlana committed
156 157
    void initVisualization();

158 159 160
};

}