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
11163760
Commit
11163760
authored
Feb 06, 2019
by
Carlos Alfredo Yeverino Rodriguez
Browse files
Corrected way to get torcs image from shared memory
parent
60245204
Changes
3
Hide whitespace changes
Inline
Side-by-side
EMADL2TORCS/resources/TORCSComponent/src/TORCSComponent/src/main.cpp
View file @
11163760
...
...
@@ -35,39 +35,42 @@ void predictionsChatterCallback(const std_msgs::Float32MultiArray::ConstPtr& msg
client
.
visualizePredictions
(
affordance
);
}
void
publishCallback
(
std
::
shared_ptr
<
torcs
::
DataContainer
>
screenshot
,
torcs
::
Affordance
affordance
)
void
publishCallback
(
std
::
vector
<
uint8_t
>
screenshot
,
torcs
::
Affordance
affordance
)
{
cv
::
Mat
image
(
torcs
::
IMAGE_HEIGHT
,
torcs
::
IMAGE_WIDTH
,
CV_8UC3
);
memcpy
(
image
.
data
,
screenshot
->
data
(),
torcs
::
IMAGE_SIZE_BYTES
);
cv
::
Mat
flippedImage
(
torcs
::
IMAGE_HEIGHT
,
torcs
::
IMAGE_WIDTH
,
CV_8UC3
);
cv
::
flip
(
image
,
flippedImage
,
0
);
cv
::
Mat
screenRGB
(
torcs
::
IMAGE_HEIGHT
,
torcs
::
IMAGE_WIDTH
,
CV_8UC3
);
cv
::
Mat
resizeRGB
(
ROS_IMAGE_HEIGHT
,
ROS_IMAGE_WIDTH
,
CV_8UC3
);
cv
::
Mat
resizedImage
(
torcs
::
IMAGE_HEIGHT
,
torcs
::
IMAGE_WIDTH
,
CV_8UC3
);
cv
::
resize
(
flippedImage
,
resizedImage
,
cv
::
Size
(
ROS_IMAGE_WIDTH
,
ROS_IMAGE_HEIGHT
));
uchar
*
ptr
=
screenRGB
.
data
;
for
(
int
h
=
0
;
h
<
torcs
::
IMAGE_HEIGHT
;
h
++
)
{
for
(
int
w
=
0
;
w
<
torcs
::
IMAGE_WIDTH
;
w
++
)
{
ptr
[(
h
*
torcs
::
IMAGE_WIDTH
+
w
)
*
3
+
2
]
=
screenshot
.
data
()[((
torcs
::
IMAGE_HEIGHT
-
h
-
1
)
*
torcs
::
IMAGE_WIDTH
+
w
)
*
3
+
0
];
ptr
[(
h
*
torcs
::
IMAGE_WIDTH
+
w
)
*
3
+
1
]
=
screenshot
.
data
()[((
torcs
::
IMAGE_HEIGHT
-
h
-
1
)
*
torcs
::
IMAGE_WIDTH
+
w
)
*
3
+
1
];
ptr
[(
h
*
torcs
::
IMAGE_WIDTH
+
w
)
*
3
+
0
]
=
screenshot
.
data
()[((
torcs
::
IMAGE_HEIGHT
-
h
-
1
)
*
torcs
::
IMAGE_WIDTH
+
w
)
*
3
+
2
];
}
}
//Code for debugging: displays screenshot in a window
//cv::namedWindow("Screenshot", cv::WINDOW_AUTOSIZE);
//cv::imshow("Screenshot", screenRGB);
cv
::
Mat
rgbImage
(
ROS_IMAGE_HEIGHT
,
ROS_IMAGE_WIDTH
,
CV_8UC3
);
cv
::
cvtColor
(
resizedImage
,
rgbImage
,
cv
::
COLOR_BGR2RGB
);
cv
::
resize
(
screenRGB
,
resizeRGB
,
resizeRGB
.
size
(),
0
,
0
);
//Code for debugging: displays screenshot in a window
and waits for a key stroke to close it and get new one
cv
::
namedWindow
(
"Screenshot"
,
cv
::
WINDOW_AUTOSIZE
);
cv
::
imshow
(
"Screenshot"
,
r
gbImage
);
//Code for debugging: displays screenshot in a window
cv
::
namedWindow
(
"Screenshot"
,
cv
::
WINDOW_AUTOSIZE
);
cv
::
imshow
(
"Screenshot"
,
r
esizeRGB
);
std
::
vector
<
uint8_t
>
data
(
ROS_IMAGE_SIZE
);
size_t
i
=
0
;
for
(
int
c
=
0
;
c
<
3
;
++
c
)
{
for
(
int
h
=
0
;
h
<
ROS_IMAGE_HEIGHT
;
++
h
)
{
for
(
int
w
=
0
;
w
<
ROS_IMAGE_WIDTH
;
++
w
)
{
data
[
i
]
=
r
gbImage
.
data
[(
h
*
ROS_IMAGE_WIDTH
+
w
)
*
3
+
c
];
data
[
i
]
=
r
esizeRGB
.
data
[(
h
*
ROS_IMAGE_WIDTH
+
w
)
*
3
+
c
];
i
++
;
}
}
}
cv
::
Mat
imageForRos
(
ROS_IMAGE_HEIGHT
,
ROS_IMAGE_WIDTH
,
CV_8UC3
);
memcpy
(
imageForRos
.
data
,
data
.
data
(),
data
.
size
());
cv
::
namedWindow
(
"Image for ROS"
,
cv
::
WINDOW_AUTOSIZE
);
cv
::
imshow
(
"Image for ROS"
,
imageForRos
);
static_assert
(
sizeof
(
float
)
==
sizeof
(
std_msgs
::
Float32
),
"Float is not 32 bit long!"
);
...
...
EMADL2TORCS/resources/TORCSComponent/src/TORCSComponent/src/torcsclient.cpp
View file @
11163760
...
...
@@ -135,7 +135,7 @@ void TorcsClient::visualizePredictions(const torcs::Affordance &affordance)
affordance
.
getDistR
(),
true_dist_R
);
}
void
TorcsClient
::
setPublishCallback
(
std
::
function
<
void
(
std
::
shared_ptr
<
DataContainer
>
,
Affordance
)
>
callback
)
void
TorcsClient
::
setPublishCallback
(
std
::
function
<
void
(
std
::
vector
<
uint8_t
>
,
Affordance
)
>
callback
)
{
publishCallback
=
callback
;
}
...
...
@@ -189,8 +189,8 @@ void TorcsClient::iterate()
else
{
readyForNewShot
=
false
;
auto
screenshot
=
std
::
make_shared
<
DataContainer
>
(
);
memcpy
(
screenshot
->
data
(),
shared
->
data
,
screenshot
->
size
());
std
::
vector
<
uint8_t
>
screenshot
(
torcs
::
IMAGE_SIZE_BYTES
);
memcpy
(
screenshot
.
data
(),
shared
->
data
,
screenshot
.
size
());
shared
->
written
=
0
;
Affordance
affordance
;
affordance
.
populate
(
*
shared
);
// ground truth affordance
...
...
EMADL2TORCS/resources/TORCSComponent/src/TORCSComponent/src/torcsclient.h
View file @
11163760
...
...
@@ -9,16 +9,16 @@
#define DEBUG_LOG false
constexpr
auto
ROS_IMAGE_WIDTH
=
280
u
;
constexpr
auto
ROS_IMAGE_HEIGHT
=
210
u
;
constexpr
auto
ROS_IMAGE_SIZE
=
ROS_IMAGE_WIDTH
*
ROS_IMAGE_HEIGHT
*
3
u
;
constexpr
auto
ROS_IMAGE_WIDTH
=
280
;
constexpr
auto
ROS_IMAGE_HEIGHT
=
210
;
constexpr
auto
ROS_IMAGE_SIZE
=
ROS_IMAGE_WIDTH
*
ROS_IMAGE_HEIGHT
*
3
;
namespace
torcs
{
constexpr
auto
IMAGE_WIDTH
=
640
u
;
constexpr
auto
IMAGE_HEIGHT
=
480
u
;
constexpr
auto
IMAGE_CHANNELS
=
3
u
;
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
;
constexpr
auto
SEMANTIC_WIDTH
=
320
;
...
...
@@ -132,7 +132,7 @@ public:
explicit
TorcsClient
();
~
TorcsClient
();
void
setPublishCallback
(
std
::
function
<
void
(
std
::
shared_ptr
<
DataContainer
>
,
Affordance
)
>
callback
);
void
setPublishCallback
(
std
::
function
<
void
(
std
::
vector
<
uint8_t
>
,
Affordance
)
>
callback
);
void
iterate
();
void
sendCommand
(
const
Command
&
cmd
);
void
setLanesCount
(
int
count
);
...
...
@@ -144,7 +144,7 @@ private:
bool
connected
=
false
;
std
::
atomic
<
bool
>
gotCommand
;
Command
command
;
std
::
function
<
void
(
std
::
shared_ptr
<
DataContainer
>
,
Affordance
)
>
publishCallback
;
std
::
function
<
void
(
std
::
vector
<
uint8_t
>
,
Affordance
)
>
publishCallback
;
int
lanesCount
=
3
;
// Use three lanes configuration by default
void
visualize
(
float
angle
,
float
true_angle
,
float
toMarking_ML
,
float
true_toMarking_ML
,
...
...
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