Commit 3a4dc1d3 authored by Svetlana's avatar Svetlana

Real time visualization

parent 454a8bd7
......@@ -8,12 +8,9 @@
#include <cv.hpp>
#include <ros/ros.h>
#include <std_msgs/Float32.h>
#include <std_msgs/Int32.h>
#include <std_msgs/Float32MultiArray.h>
constexpr auto ROS_IMAGE_WIDTH = 280u;
constexpr auto ROS_IMAGE_HEIGHT = 210u;
constexpr auto ROS_IMAGE_SIZE = ROS_IMAGE_WIDTH*ROS_IMAGE_HEIGHT*3u;
torcs::TorcsClient client;
ros::Publisher* cameraPublisher;
ros::Publisher* speedPublisher;
......@@ -24,6 +21,11 @@ void chatterCallback(const std_msgs::Float32MultiArray::ConstPtr& msg)
client.sendCommand(torcs::Command(msg->data[0], msg->data[1], msg->data[2]));
}
void infoChatterCallback(const std_msgs::Int32::ConstPtr& msg)
{
client.setLanesCount(msg->data);
}
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));
......@@ -86,6 +88,13 @@ void clientRunner()
int main(int argc, char **argv)
{
// 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);
client.setPublishCallback(publishCallback);
ros::init(argc, argv, "TORCSComponent");
ros::NodeHandle n;
......@@ -97,18 +106,9 @@ int main(int argc, char **argv)
affordancePublisher = &affordancePub;
ros::Subscriber sub = n.subscribe("/commands", 1, chatterCallback);
ros::Subscriber infoSub = n.subscribe("/info", 1, infoChatterCallback);
ros::Rate loop_rate(10);
std::thread clientRunnerThread(clientRunner);
// int count = 0;
//
// while (ros::ok())
// {
//
// client->iterate();
// ros::spinOnce();
// loop_rate.sleep();
// ++count;
// }
ros::spin();
clientRunnerThread.join();
......
......@@ -6,6 +6,10 @@
#include <functional>
#include <std_msgs/Float32MultiArray.h>
constexpr auto ROS_IMAGE_WIDTH = 280u;
constexpr auto ROS_IMAGE_HEIGHT = 210u;
constexpr auto ROS_IMAGE_SIZE = ROS_IMAGE_WIDTH*ROS_IMAGE_HEIGHT*3u;
namespace torcs
{
......@@ -14,6 +18,9 @@ constexpr auto IMAGE_HEIGHT = 480u;
constexpr auto IMAGE_CHANNELS = 3u;
constexpr auto IMAGE_SIZE_BYTES = IMAGE_WIDTH*IMAGE_HEIGHT*IMAGE_CHANNELS;
constexpr auto SEMANTIC_WIDTH = 320;
constexpr auto SEMANTIC_HEIGHT = 660;
struct shared_use_st
{
int written = 0; //a label, if 1: available to read, if 0: available to write
......@@ -120,10 +127,18 @@ class TorcsClient
{
public:
explicit TorcsClient();
~TorcsClient();
void setPublishCallback(std::function<void(std::shared_ptr<DataContainer>, Affordance)> callback);
void iterate();
void sendCommand(const Command& cmd);
void setLanesCount(int count);
void visualize(float angle, float true_angle, float toMarking_ML, float true_toMarking_ML,
float toMarking_MR, float true_toMarking_MR, float toMarking_M, float true_toMarking_M,
float toMarking_LL, float true_toMarking_LL, float toMarking_R, float true_toMarking_R,
float toMarking_L, float true_toMarking_L, float toMarking_RR, float true_toMarking_RR,
float dist_LL, float true_dist_LL, float dist_RR, float true_dist_RR, float dist_MM, float true_dist_MM,
float dist_L, float true_dist_L, float dist_R, float true_dist_R);
private:
......@@ -132,6 +147,10 @@ private:
std::atomic<bool> gotCommand;
Command command;
std::function<void(std::shared_ptr<DataContainer>, Affordance)> publishCallback;
int lanesCount = 3; // Use three lanes configuration by default
void initVisualization();
};
}
......
......@@ -5,4 +5,5 @@ tags Mastercomponent {
tag mastercomponent.imageIn with RosConnection = {topic=(/camera,std_msgs/Float32MultiArray)};
tag mastercomponent.speedIn with RosConnection = {topic=(/speed,std_msgs/Float32)};
tag mastercomponent.commandsOut with RosConnection = {topic=(/commands,std_msgs/Float32MultiArray)};
tag mastercomponent.lanesCountOut with RosConnection = {topic=(/info,std_msgs/Int32)};
}
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment