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

README.md

@@ -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
+

RosWorkspace/src/torcs/CMakeLists.txt

@@ -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

RosWorkspace/src/torcs/src/torcs.cpp

-/*
- * 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;
   }
 
 

RosWorkspace/src/torcs/src/torcsclient.cpp

+#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;
+}*/
+

RosWorkspace/src/torcs/src/torcsclient.h

+#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;
+};
+
+}
+