Commit 65a5dcc4 authored by Svetlana Pavlitskaya's avatar Svetlana Pavlitskaya

Removed obsolete TORCS driver, fixed TORCS communication.

parent 8a147557
......@@ -5,8 +5,6 @@ xterm -T ROSCORE -e roscore &
echo "Waiting for ROSCORE to start..."
sleep 2
#xterm -T TORCSDriver -e torcs/src/driver/build/driver &
xterm -T TORCSCamera -e torcs/src/camera/build/camera &
xterm -T Mastercomponent -e Mastercomponent/build/coordinator/Coordinator_dp_mastercomponent &
#!/bin/bash
#source devel/setup.bash#
#catkin_make
cd src
cd camera
......@@ -11,11 +8,3 @@ cd build
cmake ..
make -j
cd ../..
#cd driver
#mkdir -p build
#cd build
#cmake ..
#make -j
#cd ../..
......@@ -49,10 +49,10 @@ int main(int argc, char **argv)
cv::cvtColor(flippedImage, rgbImage, cv::COLOR_RGB2BGR);
//Code for debugging: displays screenshot in a window and waits for a key stroke to close it and get new one
// cv::namedWindow("Display Image 1", cv::WINDOW_AUTOSIZE );
// cv::imshow("Display Image 1", rgbImage);
// cv::waitKey(0);
// cv::destroyWindow("Display Image 1");
//cv::namedWindow("Display Image 1", cv::WINDOW_AUTOSIZE );
//cv::imshow("Display Image 1", image);
//cv::waitKey(0);
//cv::destroyWindow("Display Image 1");
std::vector<float> data(0);
......
......@@ -35,7 +35,7 @@ TorcsClient::TorcsClient()
connected = true;
//std::cout << "Started shared memory at " << std::hex << shm << std::endl;
std::cout << "Started shared memory at " << std::hex << shm << std::endl;
shared = (struct shared_use_st*)shm;
shared->written = 0; // Equals 1 when screenshot data is written and can bea read
......@@ -86,8 +86,7 @@ void TorcsClient::iterate(std::unique_ptr<DataContainer>& screenshot, float& spe
memcpy(screenshot->data(), shared->data, screenshot->size());
speed = float(shared->speed);
if (gotCommand && shared->control == 1)
{
if (gotCommand && shared->control == 1) {
gotCommand = false;
ROS_INFO("Command acc: %f, brake: %f, steer: %f", command.accelCmd, command.brakeCmd, command.steerCmd);
shared->accelCmd = command.accelCmd;
......@@ -96,7 +95,12 @@ void TorcsClient::iterate(std::unique_ptr<DataContainer>& screenshot, float& spe
}
shared->written = 0;
shared->pause = 0;
break;
} else {
if (shared->pause != 1) {
shared->pause = 1;
}
}
}
#pragma clang diagnostic pop
......
cmake_minimum_required(VERSION 3.5)
project (ROSDriver)
set (CXX_STANDARD_REQUIRED 11)
find_package(roscpp REQUIRED)
find_package(std_msgs REQUIRED)
add_executable(driver src/main.cpp src/torcsclient.cpp)
set_property(TARGET driver PROPERTY CXX_STANDARD 14)
target_link_libraries(driver ${roscpp_LIBRARIES} ${std_msgs_LIBRARIES})
target_include_directories(driver PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} ${roscpp_INCLUDE_DIRS} ${std_msgs_INCLUDE_DIRS})
#include "ros/ros.h"
#include "std_msgs/Float32MultiArray.h"
#include "torcsclient.h"
#include <memory>
std::unique_ptr<torcs::TorcsClient> client;
void chatterCallback(const std_msgs::Float32MultiArray::ConstPtr& msg)
{
ROS_INFO("Received data");
client->sendCommand(torcs::Command(msg->data[0], msg->data[1], msg->data[2]));
}
int main(int argc, char **argv)
{
client = std::make_unique<torcs::TorcsClient>();
ros::init(argc, argv, "driver");
ros::NodeHandle n;
ros::Subscriber sub = n.subscribe("/commands", 1, chatterCallback);
ros::spin();
return 0;
}
#include "torcsclient.h"
#include <iostream>
#include <chrono>
#include <thread>
#include <cstring>
#include <sys/shm.h>
#include <assert.h>
#include <ros/console.h>
using namespace torcs;
TorcsClient::TorcsClient()
{
std::cout << "TORCS Client!" << std::endl;
void *shm = NULL;
int shmid;
shmid = shmget((key_t)4567, sizeof(struct shared_use_st), 0666);
if(shmid < 0)
{
//std::cerr << "Failed to get shared memory!" << std::endl;
ROS_ERROR("Failed to get shared memory!");
return;
}
shm = shmat(shmid, 0, 0);
if(shm == (void*)-1)
{
ROS_ERROR("Failed to shmat()!");
//std::cerr << "Failed to shmat()!" << std::endl;
return;
}
connected = true;
//std::cout << "Started shared memory at " << std::hex << shm << std::endl;
shared = (struct shared_use_st*)shm;
}
void TorcsClient::sendCommand(Command cmd)
{
assert(connected);
while (shared->written == 1) {
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
ROS_INFO("Command acc: %f, brake: %f, steer: %f", cmd.accelCmd, cmd.brakeCmd, cmd.steerCmd);
shared->accelCmd = cmd.accelCmd;
shared->steerCmd = cmd.steerCmd;
shared->brakeCmd = cmd.brakeCmd;
shared->control = 1;
shared->written = 1;
}
#pragma once
#include <cstdint>
#include <array>
#include <memory>
namespace torcs
{
constexpr auto IMAGE_WIDTH = 640;
constexpr auto IMAGE_HEIGHT = 480;
constexpr auto IMAGE_CHANNELS = 3;
constexpr auto IMAGE_SIZE_BYTES = IMAGE_WIDTH*IMAGE_HEIGHT*IMAGE_CHANNELS;
struct shared_use_st
{
int written = 0; //a label, if 1: available to read, if 0: available to write
uint8_t data[IMAGE_SIZE_BYTES]; // image data field
int control = 0;
int pause = 0;
double fast = 0.0;
double dist_L = 0.0;
double dist_R = 0.0;
double toMarking_L = 0.0;
double toMarking_M = 0.0;
double toMarking_R = 0.0;
double dist_LL = 0.0;
double dist_MM = 0.0;
double dist_RR = 0.0;
double toMarking_LL = 0.0;
double toMarking_ML = 0.0;
double toMarking_MR = 0.0;
double toMarking_RR = 0.0;
double toMiddle = 0.0;
double angle = 0.0;
double speed = 0.0;
double steerCmd = 0.0;
double accelCmd = 0.0;
double brakeCmd = 0.0;
};
struct Command
{
Command(float accel, float steer, float brakec)
: accelCmd(accel), steerCmd(steer), brakeCmd(brakec)
{
}
float accelCmd;
float steerCmd;
float brakeCmd;
};
using DataContainer = std::array<uint8_t, IMAGE_SIZE_BYTES>;
class TorcsClient
{
public:
TorcsClient();
void sendCommand(Command cmd);
private:
struct shared_use_st *shared;
bool connected = false;
};
}
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