main.cpp 833 Bytes
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
#include "ros/ros.h"
#include "std_msgs/Float64MultiArray.h"
#include "torcsclient.h"

#include <sstream>

int main(int argc, char **argv)
{
    ros::init(argc, argv, "camera");
    ros::NodeHandle n;
    ros::Publisher chatter_pub = n.advertise<std_msgs::Float64MultiArray>("/camera", 1000);
    ros::Rate loop_rate(10);
    int count = 0;

    torcs::TorcsClient client;

    while (ros::ok())
    {
        std_msgs::Float64MultiArray::Ptr msg(new std_msgs::Float64MultiArray);
	msg->data.resize(torcs::IMAGE_SIZE_BYTES);
        
        const auto imageData = client.getScreenshot();
        memcpy(msg->data.data(), imageData->data(), imageData->size());
        
        ROS_INFO("%f", msg->data[0]);
        chatter_pub.publish(msg);
        ros::spinOnce();
        loop_rate.sleep();
        ++count;
    }
    return 0;
}