main.cpp 4.22 KB
Newer Older
1 2 3 4 5 6 7 8 9 10
#include "torcsclient.h"

#include <sstream>
#include <thread>

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

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
24 25 26 27 28
void infoChatterCallback(const std_msgs::Int32::ConstPtr& msg)
{
    client.setLanesCount(msg->data);
}

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

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
void publishCallback(std::shared_ptr<torcs::DataContainer> screenshot, torcs::Affordance affordance)
{
    cv::Mat image(torcs::IMAGE_HEIGHT, torcs::IMAGE_WIDTH, CV_8UC(torcs::IMAGE_CHANNELS));
    memcpy(image.data, screenshot->data(), torcs::IMAGE_SIZE_BYTES);

    cv::Mat resizedImage(ROS_IMAGE_HEIGHT, ROS_IMAGE_WIDTH, CV_8UC(torcs::IMAGE_CHANNELS));
    cv::resize(image, resizedImage, cv::Size(ROS_IMAGE_WIDTH, ROS_IMAGE_HEIGHT));

    cv::Mat flippedImage(torcs::IMAGE_HEIGHT, torcs::IMAGE_WIDTH, CV_8UC(torcs::IMAGE_CHANNELS));
    cv::flip(resizedImage, flippedImage, 0);

    cv::Mat rgbImage(torcs::IMAGE_HEIGHT, torcs::IMAGE_WIDTH, CV_8UC(torcs::IMAGE_CHANNELS));
    cv::cvtColor(flippedImage, rgbImage, cv::COLOR_RGB2BGR);

    //Code for debugging: displays screenshot in a window and waits for a key stroke to close it and get new one
    //cv::namedWindow("Display Image 1", cv::WINDOW_AUTOSIZE );
    //cv::imshow("Display Image 1", image);
    //cv::waitKey(0);
    //cv::destroyWindow("Display Image 1");


    std::vector<float> data(0);
    data.reserve(ROS_IMAGE_SIZE);
    for(size_t j=0; j<ROS_IMAGE_HEIGHT; j++){
        for(size_t k=0; k<ROS_IMAGE_WIDTH; k++){
            cv::Vec3b intensity = rgbImage.at<cv::Vec3b>(j, k);
            for(size_t i=0; i<3; i++){
                data.push_back((float) intensity[i]);
            }
        }
    }

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

    std_msgs::Float32MultiArray::Ptr msg(new std_msgs::Float32MultiArray);
    msg->data.resize(ROS_IMAGE_SIZE);
    memcpy(msg->data.data(), data.data(), ROS_IMAGE_SIZE*sizeof(float));

    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
98 99 100 101 102 103 104
//    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);

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

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