main.cpp 4.29 KB
Newer Older
1 2 3 4
#include "torcsclient.h"

#include <sstream>
#include <thread>
5
#include <fstream>
6 7 8 9 10 11

#include <ml.h>
#include <highgui.h>
#include <cv.hpp>
#include <ros/ros.h>
#include <std_msgs/Float32.h>
Svetlana's avatar
Svetlana committed
12
#include <std_msgs/Int32.h>
13
#include <std_msgs/Float32MultiArray.h>
14
#include <std_msgs/UInt8MultiArray.h>
15 16 17 18 19 20 21 22 23 24 25

torcs::TorcsClient client;
ros::Publisher* cameraPublisher;
ros::Publisher* speedPublisher;
ros::Publisher* affordancePublisher;

void chatterCallback(const std_msgs::Float32MultiArray::ConstPtr& msg)
{
    client.sendCommand(torcs::Command(msg->data[0], msg->data[1], msg->data[2]));
}

Svetlana's avatar
Svetlana committed
26 27 28 29 30
void infoChatterCallback(const std_msgs::Int32::ConstPtr& msg)
{
    client.setLanesCount(msg->data);
}

Svetlana's avatar
Svetlana committed
31 32 33 34 35 36 37
void predictionsChatterCallback(const std_msgs::Float32MultiArray::ConstPtr& msg)
{
    torcs::Affordance affordance;
    affordance.populate(*msg);
    client.visualizePredictions(affordance);
}

38
void publishCallback(std::vector<uint8_t> screenshot, torcs::Affordance affordance)
39 40
{

41 42
    cv::Mat screenRGB(torcs::IMAGE_HEIGHT, torcs::IMAGE_WIDTH, CV_8UC3);
    cv::Mat resizeRGB(ROS_IMAGE_HEIGHT, ROS_IMAGE_WIDTH, CV_8UC3);
43

44 45 46 47 48 49 50 51 52 53 54 55
    uchar* ptr = screenRGB.data;
    for (int h = 0; h < torcs::IMAGE_HEIGHT; h++) {
      for (int w = 0; w < torcs::IMAGE_WIDTH; w++) {
        ptr[(h*torcs::IMAGE_WIDTH+w)*3+2]=screenshot.data()[((torcs::IMAGE_HEIGHT-h-1)*torcs::IMAGE_WIDTH+w)*3+0];
        ptr[(h*torcs::IMAGE_WIDTH+w)*3+1]=screenshot.data()[((torcs::IMAGE_HEIGHT-h-1)*torcs::IMAGE_WIDTH+w)*3+1];
        ptr[(h*torcs::IMAGE_WIDTH+w)*3+0]=screenshot.data()[((torcs::IMAGE_HEIGHT-h-1)*torcs::IMAGE_WIDTH+w)*3+2];
      }
    }

    //Code for debugging: displays screenshot in a window
    //cv::namedWindow("Screenshot", cv::WINDOW_AUTOSIZE);
    //cv::imshow("Screenshot", screenRGB);
56

57
    cv::resize(screenRGB, resizeRGB, resizeRGB.size(), 0, 0);
58

59 60 61
    //Code for debugging: displays screenshot in a window
    cv::namedWindow("Screenshot", cv::WINDOW_AUTOSIZE);
    cv::imshow("Screenshot", resizeRGB);
62 63 64 65 66 67

    std::vector<uint8_t> data(ROS_IMAGE_SIZE);
    size_t i = 0;
    for (int c = 0; c < 3; ++c) {
      for (int h = 0; h < ROS_IMAGE_HEIGHT; ++h) {
        for (int w = 0; w < ROS_IMAGE_WIDTH; ++w) {
68
          data[i] = resizeRGB.data[(h*ROS_IMAGE_WIDTH+w)*3+c];
69
          i++;
70
        }
71
      }
72 73
    }

74

75 76
    static_assert(sizeof(float) == sizeof(std_msgs::Float32), "Float is not 32 bit long!");

77
    std_msgs::UInt8MultiArray::Ptr msg(new std_msgs::UInt8MultiArray);
78
    msg->data.resize(ROS_IMAGE_SIZE);
79
    memcpy(msg->data.data(), data.data(), data.size());
80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104

    std_msgs::Float32::Ptr speedMsg(new std_msgs::Float32);
    speedMsg->data = affordance.getSpeed();

    std_msgs::Float32MultiArray::Ptr affordanceMsg(new std_msgs::Float32MultiArray);
    affordanceMsg->data = affordance.getAsVector();

    cameraPublisher->publish(msg);
    speedPublisher->publish(speedMsg);
    affordancePublisher->publish(affordanceMsg);
}

void clientRunner()
{
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wmissing-noreturn"
    while(true)
    {
        client.iterate();
    }
#pragma clang diagnostic pop
}

int main(int argc, char **argv)
{
Svetlana's avatar
Svetlana committed
105 106 107 108 109 110 111
//    cvNamedWindow("Semantic Visualization",1);
//    IplImage* background=cvLoadImage("TORCSComponent/assets/semantic_background_3lane.png");
//    IplImage* semanticRGB=cvCreateImage(cvSize(torcs::SEMANTIC_WIDTH,torcs::SEMANTIC_HEIGHT),IPL_DEPTH_8U,3);
//    cvCopy(background,semanticRGB);
//    cvShowImage("Semantic Visualization",semanticRGB);
//    cv::waitKey(0);

112 113 114
    client.setPublishCallback(publishCallback);
    ros::init(argc, argv, "TORCSComponent");
    ros::NodeHandle n;
115
    ros::Publisher cameraPub = n.advertise<std_msgs::UInt8MultiArray>("/camera", 1);
116 117 118 119 120 121 122
    cameraPublisher = &cameraPub;
    ros::Publisher speedPub = n.advertise<std_msgs::Float32>("/speed", 1);
    speedPublisher = &speedPub;
    ros::Publisher affordancePub = n.advertise<std_msgs::Float32MultiArray>("/affordance", 1);
    affordancePublisher = &affordancePub;

    ros::Subscriber sub = n.subscribe("/commands", 1, chatterCallback);
Svetlana's avatar
Svetlana committed
123
    ros::Subscriber infoSub = n.subscribe("/info", 1, infoChatterCallback);
Svetlana's avatar
Svetlana committed
124
    ros::Subscriber predictionsSub = n.subscribe("/predictions", 1, predictionsChatterCallback);
125 126 127 128 129 130 131
    ros::Rate loop_rate(10);
    std::thread clientRunnerThread(clientRunner);

    ros::spin();
    clientRunnerThread.join();
    return 0;
}