Aufgrund einer Wartung wird GitLab am 29.10. zwischen 9:00 und 10:00 Uhr kurzzeitig nicht zur Verfügung stehen. / Due to maintenance, GitLab will be temporarily unavailable on 29.10. between 9:00 and 10:00 am.

Commit 3a4dc1d3 authored by Svetlana's avatar Svetlana

Real time visualization

parent 454a8bd7
...@@ -8,12 +8,9 @@ ...@@ -8,12 +8,9 @@
#include <cv.hpp> #include <cv.hpp>
#include <ros/ros.h> #include <ros/ros.h>
#include <std_msgs/Float32.h> #include <std_msgs/Float32.h>
#include <std_msgs/Int32.h>
#include <std_msgs/Float32MultiArray.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; torcs::TorcsClient client;
ros::Publisher* cameraPublisher; ros::Publisher* cameraPublisher;
ros::Publisher* speedPublisher; ros::Publisher* speedPublisher;
...@@ -24,6 +21,11 @@ void chatterCallback(const std_msgs::Float32MultiArray::ConstPtr& msg) ...@@ -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])); 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) 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)); cv::Mat image(torcs::IMAGE_HEIGHT, torcs::IMAGE_WIDTH, CV_8UC(torcs::IMAGE_CHANNELS));
...@@ -86,6 +88,13 @@ void clientRunner() ...@@ -86,6 +88,13 @@ void clientRunner()
int main(int argc, char **argv) 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); client.setPublishCallback(publishCallback);
ros::init(argc, argv, "TORCSComponent"); ros::init(argc, argv, "TORCSComponent");
ros::NodeHandle n; ros::NodeHandle n;
...@@ -97,18 +106,9 @@ int main(int argc, char **argv) ...@@ -97,18 +106,9 @@ int main(int argc, char **argv)
affordancePublisher = &affordancePub; affordancePublisher = &affordancePub;
ros::Subscriber sub = n.subscribe("/commands", 1, chatterCallback); ros::Subscriber sub = n.subscribe("/commands", 1, chatterCallback);
ros::Subscriber infoSub = n.subscribe("/info", 1, infoChatterCallback);
ros::Rate loop_rate(10); ros::Rate loop_rate(10);
std::thread clientRunnerThread(clientRunner); std::thread clientRunnerThread(clientRunner);
// int count = 0;
//
// while (ros::ok())
// {
//
// client->iterate();
// ros::spinOnce();
// loop_rate.sleep();
// ++count;
// }
ros::spin(); ros::spin();
clientRunnerThread.join(); clientRunnerThread.join();
......
...@@ -6,6 +6,10 @@ ...@@ -6,6 +6,10 @@
#include <functional> #include <functional>
#include <std_msgs/Float32MultiArray.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;
namespace torcs namespace torcs
{ {
...@@ -14,6 +18,9 @@ constexpr auto IMAGE_HEIGHT = 480u; ...@@ -14,6 +18,9 @@ constexpr auto IMAGE_HEIGHT = 480u;
constexpr auto IMAGE_CHANNELS = 3u; constexpr auto IMAGE_CHANNELS = 3u;
constexpr auto IMAGE_SIZE_BYTES = IMAGE_WIDTH*IMAGE_HEIGHT*IMAGE_CHANNELS; 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 struct shared_use_st
{ {
int written = 0; //a label, if 1: available to read, if 0: available to write int written = 0; //a label, if 1: available to read, if 0: available to write
...@@ -120,10 +127,18 @@ class TorcsClient ...@@ -120,10 +127,18 @@ class TorcsClient
{ {
public: public:
explicit TorcsClient(); explicit TorcsClient();
~TorcsClient();
void setPublishCallback(std::function<void(std::shared_ptr<DataContainer>, Affordance)> callback); void setPublishCallback(std::function<void(std::shared_ptr<DataContainer>, Affordance)> callback);
void iterate(); void iterate();
void sendCommand(const Command& cmd); 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: private:
...@@ -132,6 +147,10 @@ private: ...@@ -132,6 +147,10 @@ private:
std::atomic<bool> gotCommand; std::atomic<bool> gotCommand;
Command command; Command command;
std::function<void(std::shared_ptr<DataContainer>, Affordance)> publishCallback; 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 { ...@@ -5,4 +5,5 @@ tags Mastercomponent {
tag mastercomponent.imageIn with RosConnection = {topic=(/camera,std_msgs/Float32MultiArray)}; tag mastercomponent.imageIn with RosConnection = {topic=(/camera,std_msgs/Float32MultiArray)};
tag mastercomponent.speedIn with RosConnection = {topic=(/speed,std_msgs/Float32)}; tag mastercomponent.speedIn with RosConnection = {topic=(/speed,std_msgs/Float32)};
tag mastercomponent.commandsOut with RosConnection = {topic=(/commands,std_msgs/Float32MultiArray)}; 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