main.cpp 4.28 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 39
void publishCallback(std::shared_ptr<torcs::DataContainer> screenshot, torcs::Affordance affordance)
{
40
    cv::Mat image(torcs::IMAGE_HEIGHT, torcs::IMAGE_WIDTH, CV_8UC3);
41 42
    memcpy(image.data, screenshot->data(), torcs::IMAGE_SIZE_BYTES);

43 44
    cv::Mat flippedImage(torcs::IMAGE_HEIGHT, torcs::IMAGE_WIDTH, CV_8UC3);
    cv::flip(image, flippedImage, 0);
45

46 47
    cv::Mat resizedImage(torcs::IMAGE_HEIGHT, torcs::IMAGE_WIDTH, CV_8UC3);
    cv::resize(flippedImage, resizedImage, cv::Size(ROS_IMAGE_WIDTH, ROS_IMAGE_HEIGHT));
48

49 50
    cv::Mat rgbImage(ROS_IMAGE_HEIGHT, ROS_IMAGE_WIDTH, CV_8UC3);
    cv::cvtColor(resizedImage, rgbImage, cv::COLOR_BGR2RGB);
51 52

    //Code for debugging: displays screenshot in a window and waits for a key stroke to close it and get new one
53 54 55 56 57 58 59 60 61 62
    cv::namedWindow("Screenshot", cv::WINDOW_AUTOSIZE );
    cv::imshow("Screenshot", rgbImage);

    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) {
          data[i] = rgbImage.data[(h*ROS_IMAGE_WIDTH+w)*3+c];
          i++;
63
        }
64
      }
65 66
    }

67 68 69 70 71
    cv::Mat imageForRos(ROS_IMAGE_HEIGHT, ROS_IMAGE_WIDTH, CV_8UC3);
    memcpy(imageForRos.data, data.data(), data.size());
    cv::namedWindow("Image for ROS", cv::WINDOW_AUTOSIZE );
    cv::imshow("Image for ROS", imageForRos);

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

74
    std_msgs::UInt8MultiArray::Ptr msg(new std_msgs::UInt8MultiArray);
75
    msg->data.resize(ROS_IMAGE_SIZE);
76
    memcpy(msg->data.data(), data.data(), data.size());
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

    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
102 103 104 105 106 107 108
//    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);

109 110 111
    client.setPublishCallback(publishCallback);
    ros::init(argc, argv, "TORCSComponent");
    ros::NodeHandle n;
112
    ros::Publisher cameraPub = n.advertise<std_msgs::UInt8MultiArray>("/camera", 1);
113 114 115 116 117 118 119
    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
120
    ros::Subscriber infoSub = n.subscribe("/info", 1, infoChatterCallback);
Svetlana's avatar
Svetlana committed
121
    ros::Subscriber predictionsSub = n.subscribe("/predictions", 1, predictionsChatterCallback);
122 123 124 125 126 127 128
    ros::Rate loop_rate(10);
    std::thread clientRunnerThread(clientRunner);

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