Skip to content
GitLab
Projects
Groups
Snippets
Help
Loading...
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Sign in
Toggle navigation
T
torcs_dl
Project overview
Project overview
Details
Activity
Releases
Repository
Repository
Files
Commits
Branches
Tags
Contributors
Graph
Compare
Locked Files
Issues
0
Issues
0
List
Boards
Labels
Service Desk
Milestones
Iterations
Merge Requests
0
Merge Requests
0
Requirements
Requirements
List
CI / CD
CI / CD
Pipelines
Jobs
Schedules
Test Cases
Security & Compliance
Security & Compliance
Dependency List
License Compliance
Operations
Operations
Incidents
Environments
Packages & Registries
Packages & Registries
Container Registry
Analytics
Analytics
CI / CD
Code Review
Insights
Issue
Repository
Value Stream
Wiki
Wiki
Snippets
Snippets
Members
Members
Collapse sidebar
Close sidebar
Activity
Graph
Create a new issue
Jobs
Commits
Issue Boards
Open sidebar
autonomousdriving
torcs_dl
Commits
a0f0db52
Commit
a0f0db52
authored
Jul 04, 2018
by
Svetlana
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
Fixed TORCS communication sync, added speed messages, implemented within-lane acceleration control.
parent
b16a9a20
Changes
12
Hide whitespace changes
Inline
Side-by-side
Showing
12 changed files
with
145 additions
and
32 deletions
+145
-32
TorcsEMAMGenerator/src/main/models/dp/Mastercomponent.emadl
TorcsEMAMGenerator/src/main/models/dp/Mastercomponent.emadl
+4
-3
TorcsEMAMGenerator/src/main/models/dp/Mastercomponent.tag
TorcsEMAMGenerator/src/main/models/dp/Mastercomponent.tag
+1
-0
TorcsEMAMGenerator/src/main/models/dp/subcomponents/Dpnet.emadl
...MAMGenerator/src/main/models/dp/subcomponents/Dpnet.emadl
+0
-1
TorcsEMAMGenerator/src/main/models/dp/subcomponents/Drivercontroller.emadl
...r/src/main/models/dp/subcomponents/Drivercontroller.emadl
+22
-5
TorcsEMAMGenerator/src/main/models/dp/subcomponents/Imagepreprocessing.emadl
...src/main/models/dp/subcomponents/Imagepreprocessing.emadl
+1
-1
TorcsEMAMGenerator/src/main/resources/scripts/run_all.sh
TorcsEMAMGenerator/src/main/resources/scripts/run_all.sh
+1
-1
TorcsEMAMGenerator/src/main/resources/torcs/build.sh
TorcsEMAMGenerator/src/main/resources/torcs/build.sh
+6
-6
TorcsEMAMGenerator/src/main/resources/torcs/src/camera/src/main.cpp
...enerator/src/main/resources/torcs/src/camera/src/main.cpp
+22
-3
TorcsEMAMGenerator/src/main/resources/torcs/src/camera/src/torcsclient.cpp
...r/src/main/resources/torcs/src/camera/src/torcsclient.cpp
+62
-6
TorcsEMAMGenerator/src/main/resources/torcs/src/camera/src/torcsclient.h
...tor/src/main/resources/torcs/src/camera/src/torcsclient.h
+16
-1
TorcsEMAMGenerator/src/main/resources/torcs/src/driver/src/main.cpp
...enerator/src/main/resources/torcs/src/driver/src/main.cpp
+2
-1
TorcsEMAMGenerator/src/main/resources/torcs/src/driver/src/torcsclient.cpp
...r/src/main/resources/torcs/src/driver/src/torcsclient.cpp
+8
-4
No files found.
TorcsEMAMGenerator/src/main/models/dp/Mastercomponent.emadl
View file @
a0f0db52
...
...
@@ -3,8 +3,9 @@ import dp.subcomponents.*;
component
Mastercomponent
{
ports
in
Z
(
0
:
255
)^{
3
,
210
,
280
}
imageIn
,
out
Q
(
0
:
1
)^{
3
,
1
,
1
}
commandsOut
;
in
Z
(
0
:
255
)^{
3
,
210
,
280
}
imageIn
,
in
Q
speedIn
,
out
Q
(
0
:
1
)^{
3
,
1
,
1
}
commandsOut
;
instance
Imagepreprocessing
ip
;
instance
Dpnet
dpnet
;
...
...
@@ -13,6 +14,6 @@ component Mastercomponent {
connect
imageIn
->
ip
.
imageIn
;
connect
ip
.
imageOut
->
dpnet
.
image
;
connect
dpnet
.
predictions
->
dc
.
affordanceIn
;
connect
speedIn
->
dc
.
speedIn
;
connect
dc
.
commandsOut
->
commandsOut
;
}
\ No newline at end of file
TorcsEMAMGenerator/src/main/models/dp/Mastercomponent.tag
View file @
a0f0db52
...
...
@@ -3,5 +3,6 @@ conforms to de.monticore.lang.monticar.generator.roscpp.RosToEmamTagSchema;
tags
Mastercomponent
{
tag
mastercomponent
.
imageIn
with
RosConnection
=
{
topic
=(/
camera
,
std_msgs
/
Int8MultiArray
)};
tag
mastercomponent
.
speedIn
with
RosConnection
=
{
topic
=(/
speed
,
std_msgs
/
Float32
)};
tag
mastercomponent
.
commandsOut
with
RosConnection
=
{
topic
=(/
commands
,
std_msgs
/
Float32MultiArray
)};
}
TorcsEMAMGenerator/src/main/models/dp/subcomponents/Dpnet.emadl
View file @
a0f0db52
...
...
@@ -28,6 +28,5 @@ component Dpnet{
FullyConnected
(
units
=
13
)
->
Sigmoid
()
->
predictions
}
}
TorcsEMAMGenerator/src/main/models/dp/subcomponents/Drivercontroller.emadl
View file @
a0f0db52
...
...
@@ -3,6 +3,7 @@ package dp.subcomponents;
component
Drivercontroller
{
ports
in
Q
(
0
:
1
)^{
1
,
1
,
13
}
affordanceIn
,
in
Q
speedIn
,
out
Q
(
0
:
1
)^{
3
,
1
,
1
}
commandsOut
;
implementation
Math
{
...
...
@@ -13,15 +14,31 @@ component Drivercontroller {
Q
distToLaneCenter
=
abs
(
laneWidth
/
2
-
affordanceIn
(
0
,
0
,
2
));
//
Steering
commandsOut
(
2
,
1
,
1
)
=
Constant
*
(
angle
-
distToLaneCenter
/
roadWidth
);
//
steering
commandsOut
(
2
,
1
,
1
)
=
0
;//
Constant
*
(
angle
-
distToLaneCenter
/
roadWidth
);
//
steering
//
Acceleration
Q
vMax
=
72
;
Q
vMax
=
25
;
Q
vC
=
1
;
Q
vD
=
0
;
Q
desiredSpeed
=
vMax
*
(
exp
(-(
vC
/
vMax
)
*
affordanceIn
(
0
,
0
,
6
)
-
vD
));
commandsOut
(
1
,
1
,
1
)
=
desiredSpeed
/
vMax
;
//
acceleration
Q
desiredSpeed
=
15
;//
vMax
*
(
exp
(-(
vC
/
vMax
)
*
affordanceIn
(
0
,
0
,
6
)
-
vD
));
commandsOut
(
3
,
1
,
1
)=
0.0
;
//
brake
Q
accelCmd
=
0.1
*
(
desiredSpeed
-
speedIn
+
1
);
if
accelCmd
>
1
accelCmd
=
1.0
;
end
Q
brakeCmd
=
0.1
*
(
speedIn
-
desiredSpeed
);
if
brakeCmd
>
1
brakeCmd
=
1
;
end
//
acceleration
and
brake
if
speedIn
<
desiredSpeed
commandsOut
(
1
,
1
,
1
)
=
accelCmd
;
commandsOut
(
3
,
1
,
1
)
=
0
;
else
commandsOut
(
1
,
1
,
1
)
=
0
;
commandsOut
(
3
,
1
,
1
)
=
brakeCmd
;
end
}
}
\ No newline at end of file
TorcsEMAMGenerator/src/main/models/dp/subcomponents/Imagepreprocessing.emadl
View file @
a0f0db52
...
...
@@ -6,6 +6,6 @@ component Imagepreprocessing {
out
Z
(
0
:
255
)^{
3
,
210
,
280
}
imageOut
;
implementation
Math
{
imageOut
=
imageIn
;
imageOut
=
imageIn
;
}
}
\ No newline at end of file
TorcsEMAMGenerator/src/main/resources/scripts/run_all.sh
View file @
a0f0db52
...
...
@@ -5,7 +5,7 @@ xterm -T ROSCORE -e roscore &
echo
"Waiting for ROSCORE to start..."
sleep
2
xterm
-T
TORCSDriver
-e
torcs/src/driver/build/driver &
#
xterm -T TORCSDriver -e torcs/src/driver/build/driver &
xterm
-T
TORCSCamera
-e
torcs/src/camera/build/camera &
...
...
TorcsEMAMGenerator/src/main/resources/torcs/build.sh
View file @
a0f0db52
...
...
@@ -12,10 +12,10 @@ cmake ..
make
-j
cd
../..
cd
driver
mkdir
-p
build
cd
build
cmake ..
make
-j
cd
../..
#
cd driver
#
mkdir -p build
#
cd build
#
cmake ..
#
make -j
#
cd ../..
TorcsEMAMGenerator/src/main/resources/torcs/src/camera/src/main.cpp
View file @
a0f0db52
#include "ros/ros.h"
#include "std_msgs/Int8MultiArray.h"
#include "std_msgs/Float32.h"
#include "torcsclient.h"
#include <sstream>
...
...
@@ -7,23 +8,37 @@
#include <ml.h>
#include <highgui.h>
#include <cv.h>
#include <ros/console.h>
#include <std_msgs/Float32MultiArray.h>
constexpr
auto
ROS_IMAGE_WIDTH
=
280
;
constexpr
auto
ROS_IMAGE_HEIGHT
=
210
;
constexpr
auto
ROS_IMAGE_SIZE
=
ROS_IMAGE_WIDTH
*
ROS_IMAGE_HEIGHT
*
3
;
torcs
::
Command
cmd
(
1.
,
0.
,
0.
);
void
chatterCallback
(
const
std_msgs
::
Float32MultiArray
::
ConstPtr
&
msg
)
{
cmd
=
torcs
::
Command
(
msg
->
data
[
0
],
msg
->
data
[
1
],
msg
->
data
[
2
]);
//client->sendCommand(torcs::Command(msg->data[0], msg->data[1], msg->data[2]));
}
int
main
(
int
argc
,
char
**
argv
)
{
ros
::
init
(
argc
,
argv
,
"camera"
);
ros
::
NodeHandle
n
;
ros
::
Publisher
chatter_pub
=
n
.
advertise
<
std_msgs
::
Int8MultiArray
>
(
"/camera"
,
1000
);
ros
::
Publisher
cameraPub
=
n
.
advertise
<
std_msgs
::
Int8MultiArray
>
(
"/camera"
,
1
);
ros
::
Publisher
speedPub
=
n
.
advertise
<
std_msgs
::
Float32
>
(
"/speed"
,
1
);
ros
::
Subscriber
sub
=
n
.
subscribe
(
"/commands"
,
1
,
chatterCallback
);
ros
::
Rate
loop_rate
(
10
);
int
count
=
0
;
torcs
::
TorcsClient
client
;
while
(
ros
::
ok
())
{
const
auto
screenshot
=
client
.
getScreenshot
();
float
speed
;
std
::
unique_ptr
<
torcs
::
DataContainer
>
screenshot
=
std
::
make_unique
<
torcs
::
DataContainer
>
();
client
.
iterate
(
screenshot
,
speed
,
cmd
);
cv
::
Mat
image
(
torcs
::
IMAGE_HEIGHT
,
torcs
::
IMAGE_WIDTH
,
CV_8UC
(
torcs
::
IMAGE_CHANNELS
));
memcpy
(
image
.
data
,
screenshot
.
get
(),
torcs
::
IMAGE_SIZE_BYTES
);
...
...
@@ -41,7 +56,11 @@ int main(int argc, char **argv)
msg
->
data
.
resize
(
ROS_IMAGE_SIZE
);
memcpy
(
msg
->
data
.
data
(),
resizedImage
.
data
,
ROS_IMAGE_SIZE
);
chatter_pub
.
publish
(
msg
);
std_msgs
::
Float32
::
Ptr
speedMsg
(
new
std_msgs
::
Float32
);
speedMsg
->
data
=
speed
;
cameraPub
.
publish
(
msg
);
speedPub
.
publish
(
speedMsg
);
ros
::
spinOnce
();
loop_rate
.
sleep
();
++
count
;
...
...
TorcsEMAMGenerator/src/main/resources/torcs/src/camera/src/torcsclient.cpp
View file @
a0f0db52
...
...
@@ -6,6 +6,7 @@
#include <cstring>
#include <sys/shm.h>
#include <assert.h>
#include <ros/console.h>
using
namespace
torcs
;
...
...
@@ -19,26 +20,28 @@ TorcsClient::TorcsClient()
shmid
=
shmget
((
key_t
)
4567
,
sizeof
(
struct
shared_use_st
),
0666
);
if
(
shmid
<
0
)
{
std
::
cerr
<<
"Failed to get shared memory!"
<<
std
::
endl
;
return
;
//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
)
{
std
::
cerr
<<
"Failed to shmat()!"
<<
std
::
endl
;
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
;
//
std::cout << "Started shared memory at " << std::hex << shm << std::endl;
shared
=
(
struct
shared_use_st
*
)
shm
;
}
std
::
unique_ptr
<
DataContainer
>
TorcsClient
::
getScreenshot
()
std
::
unique_ptr
<
DataContainer
>
TorcsClient
::
getScreenshot
(
float
&
speed
)
{
assert
(
connected
);
...
...
@@ -48,12 +51,65 @@ std::unique_ptr<DataContainer> TorcsClient::getScreenshot()
}
shared
->
pause
=
1
;
std
::
cout
<<
"Got screenshot"
<<
std
::
endl
;
auto
result
=
std
::
make_unique
<
DataContainer
>
();
memcpy
(
result
->
data
(),
shared
->
data
,
result
->
size
());
speed
=
shared
->
speed
;
shared
->
pause
=
0
;
shared
->
written
=
0
;
return
result
;
}
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
;
}
void
TorcsClient
::
iterate
(
std
::
unique_ptr
<
DataContainer
>&
screenshot
,
float
&
speed
,
const
Command
&
cmd
)
{
//ROS_INFO("Iteration");
assert
(
connected
);
bool
commandSent
=
false
;
bool
screenshotRead
=
false
;
while
(
!
(
commandSent
&&
screenshotRead
))
{
if
(
shared
->
written
==
0
)
{
//ROS_INFO("Commanding");
if
(
!
commandSent
)
{
//ROS_INFO("Command done");
shared
->
accelCmd
=
cmd
.
accelCmd
;
shared
->
steerCmd
=
cmd
.
steerCmd
;
shared
->
brakeCmd
=
cmd
.
brakeCmd
;
shared
->
control
=
1
;
shared
->
written
=
1
;
commandSent
=
true
;
}
}
else
{
//ROS_INFO("Reading");
if
(
!
screenshotRead
)
{
//ROS_INFO("Reading done");
shared
->
pause
=
1
;
memcpy
(
screenshot
->
data
(),
shared
->
data
,
screenshot
->
size
());
speed
=
float
(
shared
->
speed
);
shared
->
pause
=
0
;
shared
->
written
=
0
;
screenshotRead
=
true
;
}
}
}
}
TorcsEMAMGenerator/src/main/resources/torcs/src/camera/src/torcsclient.h
View file @
a0f0db52
...
...
@@ -45,13 +45,28 @@ struct shared_use_st
double
brakeCmd
=
0.0
;
};
struct
Command
{
Command
(
float
accel
,
float
steer
,
float
brake
)
:
accelCmd
(
accel
),
steerCmd
(
steer
),
brakeCmd
(
brake
)
{
}
float
accelCmd
;
float
steerCmd
;
float
brakeCmd
;
};
using
DataContainer
=
std
::
array
<
uint8_t
,
IMAGE_SIZE_BYTES
>
;
class
TorcsClient
{
public:
TorcsClient
();
std
::
unique_ptr
<
DataContainer
>
getScreenshot
();
std
::
unique_ptr
<
DataContainer
>
getScreenshot
(
float
&
speed
);
void
sendCommand
(
Command
cmd
);
void
iterate
(
std
::
unique_ptr
<
DataContainer
>&
screenshot
,
float
&
speed
,
const
Command
&
cmd
);
private:
struct
shared_use_st
*
shared
;
...
...
TorcsEMAMGenerator/src/main/resources/torcs/src/driver/src/main.cpp
View file @
a0f0db52
...
...
@@ -8,6 +8,7 @@ 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
]));
}
...
...
@@ -19,7 +20,7 @@ int main(int argc, char **argv)
ros
::
NodeHandle
n
;
ros
::
Subscriber
sub
=
n
.
subscribe
(
"/commands"
,
1
00
,
chatterCallback
);
ros
::
Subscriber
sub
=
n
.
subscribe
(
"/commands"
,
1
,
chatterCallback
);
ros
::
spin
();
return
0
;
...
...
TorcsEMAMGenerator/src/main/resources/torcs/src/driver/src/torcsclient.cpp
View file @
a0f0db52
...
...
@@ -6,6 +6,7 @@
#include <cstring>
#include <sys/shm.h>
#include <assert.h>
#include <ros/console.h>
using
namespace
torcs
;
...
...
@@ -19,20 +20,22 @@ TorcsClient::TorcsClient()
shmid
=
shmget
((
key_t
)
4567
,
sizeof
(
struct
shared_use_st
),
0666
);
if
(
shmid
<
0
)
{
std
::
cerr
<<
"Failed to get shared memory!"
<<
std
::
endl
;
//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
)
{
std
::
cerr
<<
"Failed to shmat()!"
<<
std
::
endl
;
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
;
//
std::cout << "Started shared memory at " << std::hex << shm << std::endl;
shared
=
(
struct
shared_use_st
*
)
shm
;
}
...
...
@@ -45,7 +48,8 @@ void TorcsClient::sendCommand(Command cmd)
std
::
this_thread
::
sleep_for
(
std
::
chrono
::
milliseconds
(
100
));
}
std
::
cout
<<
"Sent command"
<<
std
::
endl
;
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
;
...
...
Write
Preview
Markdown
is supported
0%
Try again
or
attach a new file
.
Attach a 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