Commit 9651ec32 authored by Svetlana's avatar Svetlana

Torcs client as ROS package prototype. Sending screenshots via ROS message.

parent 44727d19
......@@ -19,6 +19,8 @@
## ROS
### Installation
* libarmadillo-dev package
`sudo ln -s /usr/include/armadillo /usr/include/armadillo.h`
......@@ -55,3 +57,10 @@ Check that package is now available: `rospack list | grep torcs`
* Run node with the package: `rosrun torcs torcs`
### Our ROS Packages
* Go to RosWorkspace directory
* Run `catkin_make` to compile our packages
* Run `source devel/setup.bash` to setup environment
* Run `rospack list` and check that it shows our packages among others
......@@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 2.8.3)
project(torcs)
## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)
add_compile_options(-std=c++14)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
......@@ -132,7 +132,7 @@ include_directories(
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
add_executable(${PROJECT_NAME} src/torcs.cpp)
add_executable(${PROJECT_NAME} src/torcs.cpp src/torcsclient.cpp)
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
......
/*
* Copyright (C) 2008, Morgan Quigley and Willow Garage, Inc.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
* * Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the names of Stanford University or Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
// %Tag(FULLTEXT)%
// %Tag(ROS_HEADER)%
#include <ros/ros.h>
// %EndTag(ROS_HEADER)%
// %Tag(MSG_HEADER)%
#include <std_msgs/String.h>
#include <std_msgs/ByteMultiArray.h>
// %EndTag(MSG_HEADER)%
#include "tests_a_compA_RosWrapper.h"
#include "torcsclient.h"
#include <sstream>
/**
* This tutorial demonstrates simple sending of messages over the ROS system.
*/
int main(int argc, char **argv)
{
/**
* The ros::init() function needs to see argc and argv so that it can perform
* any ROS arguments and name remapping that were provided at the command line.
* For programmatic remappings you can use a different version of init() which takes
* remappings directly, but for most command-line programs, passing argc and argv is
* the easiest way to do it. The third argument to init() is the name of the node.
*
* You must call one of the versions of ros::init() before using any other
* part of the ROS system.
*/
// %Tag(INIT)%
ros::init(argc, argv, "torcs");
// %EndTag(INIT)%
/**
* NodeHandle is the main access point to communications with the ROS system.
* The first NodeHandle constructed will fully initialize this node, and the last
* NodeHandle destructed will close down the node.
*/
// %Tag(NODEHANDLE)%
ros::NodeHandle n;
// %EndTag(NODEHANDLE)%
/**
* The advertise() function is how you tell ROS that you want to
* publish on a given topic name. This invokes a call to the ROS
* master node, which keeps a registry of who is publishing and who
* is subscribing. After this advertise() call is made, the master
* node will notify anyone who is trying to subscribe to this topic name,
* and they will in turn negotiate a peer-to-peer connection with this
* node. advertise() returns a Publisher object which allows you to
* publish messages on that topic through a call to publish(). Once
* all copies of the returned Publisher object are destroyed, the topic
* will be automatically unadvertised.
*
* The second parameter to advertise() is the size of the message queue
* used for publishing messages. If messages are published more quickly
* than we can send them, the number here specifies how many messages to
* buffer up before throwing some away.
*/
// %Tag(PUBLISHER)%
// ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);
ros::Publisher pub = n.advertise<std_msgs::ByteMultiArray>("torcsClient", 1000);
// %EndTag(PUBLISHER)%
// %Tag(LOOP_RATE)%
ros::Rate loop_rate(10);
ros::Rate loop_rate(100);
// %EndTag(LOOP_RATE)%
tests_a_compA_RosWrapper testsWrapper(n, n);
torcs::TorcsClient client;
/**
* A count of how many messages we have sent. This is used to create
* a unique string for each message.
*/
// %Tag(ROS_OK)%
int count = 0;
while (ros::ok())
{
// %EndTag(ROS_OK)%
/**
* This is a message object. You stuff it with data, and then publish it.
*/
// %Tag(FILL_MESSAGE)%
//std_msgs::String msg;
//std::stringstream ss;
//ss << "hello world " << count;
//msg.data = ss.str();
// %EndTag(FILL_MESSAGE)%
// %Tag(ROSCONSOLE)%
//ROS_INFO("%s", msg.data.c_str());
ROS_INFO("Tick");
// %EndTag(ROSCONSOLE)%
/**
* The publish() function is how you send messages. The parameter
* is the message object. The type of this object must agree with the type
* given as a template parameter to the advertise<>() call, as was done
* in the constructor above.
*/
// %Tag(PUBLISH)%
//chatter_pub.publish(msg);
std_msgs::ByteMultiArray msg;
msg.data.clear();
msg.data.resize(torcs::IMAGE_SIZE_BYTES);
const auto imageData = client.getScreenshot();
memcpy(msg.data.data(), imageData->data(), imageData->size());
pub.publish(msg);
// %EndTag(PUBLISH)%
// %Tag(SPINONCE)%
//ros::spinOnce();
// %EndTag(SPINONCE)%
testsWrapper.tick();
// %EndTag(SPINONCE)%
// %Tag(RATE_SLEEP)%
loop_rate.sleep();
// %EndTag(RATE_SLEEP)%
++count;
}
......
#include "torcsclient.h"
#include <iostream>
#include <chrono>
#include <thread>
#include <cstring>
#include <sys/shm.h>
#include <assert.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;
return;
}
shm = shmat(shmid, 0, 0);
if(shm == (void*)-1)
{
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;
shared->written = 0;
shared->control = 0;
shared->pause = 0;
shared->fast = 0.0;
shared->dist_L = 0.0;
shared->dist_R = 0.0;
shared->toMarking_L = 0.0;
shared->toMarking_M = 0.0;
shared->toMarking_R = 0.0;
shared->dist_LL = 0.0;
shared->dist_MM = 0.0;
shared->dist_RR = 0.0;
shared->toMarking_LL = 0.0;
shared->toMarking_ML = 0.0;
shared->toMarking_MR = 0.0;
shared->toMarking_RR = 0.0;
shared->toMiddle = 0.0;
shared->angle = 0.0;
shared->speed = 0.0;
shared->steerCmd = 0.0;
shared->accelCmd = 0.0;
shared->brakeCmd = 0.0;
}
std::unique_ptr<DataContainer> TorcsClient::getScreenshot()
{
assert(connected);
while(shared->written == 0)
{
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
auto result = std::make_unique<DataContainer>();
memcpy(result->data(), shared->data, result->size());
shared->written = 0;
return result;
}
void TorcsClient::sendCommand(Command cmd)
{
assert(connected);
while (shared->control == 0) {
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
shared->control = 0;
//shared->accelCmd = 10.0;
//shared->steerCmd = 1.0;
}
/*void writeScreenshot(shared_use_st* shared, unsigned int& num)
{
std::cout << "Read image." << std::endl;
FIBITMAP* newShot = FreeImage_Allocate(IMAGE_WIDTH, IMAGE_HEIGHT, 24);
if (!newShot)
{
std::cerr << "Failed to allocate freeimage file." << std::endl;
FreeImage_Unload(newShot);
return;
}
BYTE *bits = (BYTE*)FreeImage_GetBits(newShot);
for (int h = 0; h < IMAGE_HEIGHT; h++) {
BYTE *pixel = (BYTE*)bits;
for (int w = 0; w < IMAGE_WIDTH; w++) {
pixel[FI_RGBA_RED] = shared->data[((h)*IMAGE_WIDTH+w)*3+0];
pixel[FI_RGBA_GREEN] = shared->data[((h)*IMAGE_WIDTH+w)*3+1];
pixel[FI_RGBA_BLUE] = shared->data[((h)*IMAGE_WIDTH+w)*3+2];
pixel += 3;
}
bits += 3*IMAGE_WIDTH;
}
FreeImage_Save(FIF_BMP, newShot, (std::string("shot-")+std::to_string(num)).c_str());
FreeImage_Unload(newShot);
shared->control = 0;
shared->written = 0;
std::this_thread::sleep_for(std::chrono::seconds(2));std::cout << "Read image." << std::endl;
}*/
#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
{
};
using DataContainer = std::array<uint8_t, IMAGE_SIZE_BYTES>;
class TorcsClient
{
public:
TorcsClient();
std::unique_ptr<DataContainer> getScreenshot();
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