torcs.cpp 1.18 KB
Newer Older
Svetlana's avatar
Svetlana committed
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47
#include <ros/ros.h>
#include <std_msgs/Int8MultiArray.h>
#include <std_msgs/Float32MultiArray.h>

#include "torcsclient.h"

#include <sstream>

torcs::TorcsClient client;

void commandMsgCallback(const std_msgs::Float32MultiArray::ConstPtr& msg)
{
    torcs::Command cmd;
    cmd.accelCmd = msg->data[0];
    cmd.steerCmd = msg->data[1];
    cmd.brakeCmd = msg->data[2];
    ROS_INFO("Received command.");
    ROS_INFO("    accel: %f", cmd.accelCmd);
    ROS_INFO("    steer: %f", cmd.steerCmd);
    ROS_INFO("    break: %f", cmd.brakeCmd);
    
    client.sendCommand(cmd);
}

int main(int argc, char **argv) {

    ros::init(argc, argv, "torcs");

    ros::NodeHandle n;
    ros::Subscriber sub = n.subscribe("/commands", 1000, commandMsgCallback);
    ros::Publisher pub = n.advertise<std_msgs::Int8MultiArray>("/camera", 1000);
    ros::Rate loop_rate(100);

    while (ros::ok()) {
        std_msgs::Int8MultiArray msg;

        msg.data.clear();
        msg.data.resize(torcs::IMAGE_SIZE_BYTES);
        const auto imageData = client.getScreenshot();
        memcpy(msg.data.data(), imageData->data(), imageData->size());
        pub.publish(msg);
        loop_rate.sleep();
    }


    return 0;
}