Real time visualization

EMADL2TORCS/resources/TORCSComponent/src/TORCSComponent/src/main.cpp

@@ -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();

EMADL2TORCS/resources/TORCSComponent/src/TORCSComponent/src/torcsclient.h

@@ -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();
+
 };
 
 }

EMADL2TORCS/src/main/dp/Mastercomponent.tag

@@ -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)};
 }