#pragma once #include #include #include #include #include #include #define DEBUG_LOG false constexpr auto ROS_IMAGE_WIDTH = 280u; constexpr auto ROS_IMAGE_HEIGHT = 210u; constexpr auto ROS_IMAGE_SIZE = ROS_IMAGE_WIDTH*ROS_IMAGE_HEIGHT*3u; 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; constexpr auto SEMANTIC_WIDTH = 320; constexpr auto SEMANTIC_HEIGHT = 660; 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; 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; } 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]; } const std::vector& getAsVector() { return affordance_; } private: std::vector affordance_ = std::vector(16); }; class TorcsClient { public: explicit TorcsClient(); ~TorcsClient(); void setPublishCallback(std::function, Affordance)> callback); void iterate(); void sendCommand(const Command& cmd); void setLanesCount(int count); void visualizePredictions(const Affordance& affordance); private: struct shared_use_st *shared; bool connected = false; std::atomic gotCommand; Command command; std::function, Affordance)> publishCallback; int lanesCount = 3; // Use three lanes configuration by default 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); void initVisualization(); }; }