Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Sign in
Toggle navigation
Menu
Open sidebar
autonomousdriving
torcs_dl
Commits
9651ec32
Commit
9651ec32
authored
Mar 01, 2018
by
Svetlana
Browse files
Torcs client as ROS package prototype. Sending screenshots via ROS message.
parent
44727d19
Changes
5
Show whitespace changes
Inline
Side-by-side
README.md
View file @
9651ec32
...
...
@@ -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
View file @
9651ec32
...
...
@@ -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++1
1
)
add_compile_options
(
-std=c++1
4
)
## 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
View file @
9651ec32
/*
* 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
"t
ests_a_compA_RosWrapper
.h"
#include
"t
orcsclient
.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
(
10
0
);
// %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
0 → 100644
View file @
9651ec32
#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
0 → 100644
View file @
9651ec32
#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
;
};
}
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment