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
15490e1f
Commit
15490e1f
authored
Sep 02, 2018
by
Svetlana
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
Run TORCS client in a separate thread for correct driver controller functionality.
parent
f05e7021
Changes
9
Hide whitespace changes
Inline
Side-by-side
Showing
9 changed files
with
332 additions
and
103 deletions
+332
-103
EMADL2TORCS/generate2ros.sh
EMADL2TORCS/generate2ros.sh
+2
-2
EMADL2TORCS/resources/TORCSComponent/build.sh
EMADL2TORCS/resources/TORCSComponent/build.sh
+2
-1
EMADL2TORCS/resources/TORCSComponent/src/TORCSComponent/CMakeLists.txt
...esources/TORCSComponent/src/TORCSComponent/CMakeLists.txt
+13
-0
EMADL2TORCS/resources/TORCSComponent/src/TORCSComponent/src/main.cpp
.../resources/TORCSComponent/src/TORCSComponent/src/main.cpp
+116
-0
EMADL2TORCS/resources/TORCSComponent/src/TORCSComponent/src/torcsclient.cpp
...ces/TORCSComponent/src/TORCSComponent/src/torcsclient.cpp
+57
-11
EMADL2TORCS/resources/TORCSComponent/src/TORCSComponent/src/torcsclient.h
...urces/TORCSComponent/src/TORCSComponent/src/torcsclient.h
+138
-0
EMADL2TORCS/resources/scripts/build_all.sh
EMADL2TORCS/resources/scripts/build_all.sh
+2
-2
EMADL2TORCS/resources/scripts/run_all.sh
EMADL2TORCS/resources/scripts/run_all.sh
+2
-2
EMADL2TORCS/resources/torcs/src/camera/src/main.cpp
EMADL2TORCS/resources/torcs/src/camera/src/main.cpp
+0
-85
No files found.
EMADL2TORCS/generate2ros.sh
View file @
15490e1f
...
...
@@ -11,8 +11,8 @@ cp -rf resources/scripts/* target/
chmod
u+x target/
*
.sh
echo
"Copying CNN weights..."
cp
-rf
resources/
torcs target/torcs
chmod
u+x resources/
torcs
/
*
.sh
cp
-rf
resources/
TORCSComponent target/TORCSComponent
chmod
u+x resources/
TORCSComponent
/
*
.sh
cp
-rf
resources/dpnet target/model
echo
"Building generated code..."
...
...
EMADL2TORCS/resources/
torcs
/build.sh
→
EMADL2TORCS/resources/
TORCSComponent
/build.sh
View file @
15490e1f
...
...
@@ -2,9 +2,10 @@
cd
src
cd
camera
cd
TORCSComponent
mkdir
-p
build
cd
build
cmake ..
make
-j
cd
../..
EMADL2TORCS/resources/
torcs/src/camera
/CMakeLists.txt
→
EMADL2TORCS/resources/
TORCSComponent/src/TORCSComponent
/CMakeLists.txt
View file @
15490e1f
cmake_minimum_required
(
VERSION 3.5
)
project
(
ROSCamera
)
project
(
TORCSComponentProject
)
set
(
CXX_STANDARD_REQUIRED 14
)
find_package
(
roscpp REQUIRED
)
find_package
(
std_msgs REQUIRED
)
find_package
(
OpenCV REQUIRED
)
add_executable
(
camera
src/main.cpp src/torcsclient.cpp
)
add_executable
(
TORCSComponent
src/main.cpp src/torcsclient.cpp
)
set_property
(
TARGET
camera
PROPERTY CXX_STANDARD 14
)
target_link_libraries
(
camera
${
roscpp_LIBRARIES
}
${
std_msgs_LIBRARIES
}
${
OpenCV_LIBS
}
)
target_include_directories
(
camera
PUBLIC
${
CMAKE_CURRENT_SOURCE_DIR
}
${
roscpp_INCLUDE_DIRS
}
${
std_msgs_INCLUDE_DIRS
}
)
set_property
(
TARGET
TORCSComponent
PROPERTY CXX_STANDARD 14
)
target_link_libraries
(
TORCSComponent
${
roscpp_LIBRARIES
}
${
std_msgs_LIBRARIES
}
${
OpenCV_LIBS
}
)
target_include_directories
(
TORCSComponent
PUBLIC
${
CMAKE_CURRENT_SOURCE_DIR
}
${
roscpp_INCLUDE_DIRS
}
${
std_msgs_INCLUDE_DIRS
}
)
EMADL2TORCS/resources/TORCSComponent/src/TORCSComponent/src/main.cpp
0 → 100644
View file @
15490e1f
#include "torcsclient.h"
#include <sstream>
#include <thread>
#include <ml.h>
#include <highgui.h>
#include <cv.hpp>
#include <ros/ros.h>
#include <std_msgs/Float32.h>
#include <std_msgs/Float32MultiArray.h>
constexpr
auto
ROS_IMAGE_WIDTH
=
280u
;
constexpr
auto
ROS_IMAGE_HEIGHT
=
210u
;
constexpr
auto
ROS_IMAGE_SIZE
=
ROS_IMAGE_WIDTH
*
ROS_IMAGE_HEIGHT
*
3u
;
torcs
::
TorcsClient
client
;
ros
::
Publisher
*
cameraPublisher
;
ros
::
Publisher
*
speedPublisher
;
ros
::
Publisher
*
affordancePublisher
;
void
chatterCallback
(
const
std_msgs
::
Float32MultiArray
::
ConstPtr
&
msg
)
{
client
.
sendCommand
(
torcs
::
Command
(
msg
->
data
[
0
],
msg
->
data
[
1
],
msg
->
data
[
2
]));
}
void
publishCallback
(
std
::
shared_ptr
<
torcs
::
DataContainer
>
screenshot
,
torcs
::
Affordance
affordance
)
{
cv
::
Mat
image
(
torcs
::
IMAGE_HEIGHT
,
torcs
::
IMAGE_WIDTH
,
CV_8UC
(
torcs
::
IMAGE_CHANNELS
));
memcpy
(
image
.
data
,
screenshot
->
data
(),
torcs
::
IMAGE_SIZE_BYTES
);
cv
::
Mat
resizedImage
(
ROS_IMAGE_HEIGHT
,
ROS_IMAGE_WIDTH
,
CV_8UC
(
torcs
::
IMAGE_CHANNELS
));
cv
::
resize
(
image
,
resizedImage
,
cv
::
Size
(
ROS_IMAGE_WIDTH
,
ROS_IMAGE_HEIGHT
));
cv
::
Mat
flippedImage
(
torcs
::
IMAGE_HEIGHT
,
torcs
::
IMAGE_WIDTH
,
CV_8UC
(
torcs
::
IMAGE_CHANNELS
));
cv
::
flip
(
resizedImage
,
flippedImage
,
0
);
cv
::
Mat
rgbImage
(
torcs
::
IMAGE_HEIGHT
,
torcs
::
IMAGE_WIDTH
,
CV_8UC
(
torcs
::
IMAGE_CHANNELS
));
cv
::
cvtColor
(
flippedImage
,
rgbImage
,
cv
::
COLOR_RGB2BGR
);
//Code for debugging: displays screenshot in a window and waits for a key stroke to close it and get new one
//cv::namedWindow("Display Image 1", cv::WINDOW_AUTOSIZE );
//cv::imshow("Display Image 1", image);
//cv::waitKey(0);
//cv::destroyWindow("Display Image 1");
std
::
vector
<
float
>
data
(
0
);
data
.
reserve
(
ROS_IMAGE_SIZE
);
for
(
size_t
j
=
0
;
j
<
ROS_IMAGE_HEIGHT
;
j
++
){
for
(
size_t
k
=
0
;
k
<
ROS_IMAGE_WIDTH
;
k
++
){
cv
::
Vec3b
intensity
=
rgbImage
.
at
<
cv
::
Vec3b
>
(
j
,
k
);
for
(
size_t
i
=
0
;
i
<
3
;
i
++
){
data
.
push_back
((
float
)
intensity
[
i
]);
}
}
}
static_assert
(
sizeof
(
float
)
==
sizeof
(
std_msgs
::
Float32
),
"Float is not 32 bit long!"
);
std_msgs
::
Float32MultiArray
::
Ptr
msg
(
new
std_msgs
::
Float32MultiArray
);
msg
->
data
.
resize
(
ROS_IMAGE_SIZE
);
memcpy
(
msg
->
data
.
data
(),
data
.
data
(),
ROS_IMAGE_SIZE
*
sizeof
(
float
));
std_msgs
::
Float32
::
Ptr
speedMsg
(
new
std_msgs
::
Float32
);
speedMsg
->
data
=
affordance
.
getSpeed
();
std_msgs
::
Float32MultiArray
::
Ptr
affordanceMsg
(
new
std_msgs
::
Float32MultiArray
);
affordanceMsg
->
data
=
affordance
.
getAsVector
();
cameraPublisher
->
publish
(
msg
);
speedPublisher
->
publish
(
speedMsg
);
affordancePublisher
->
publish
(
affordanceMsg
);
}
void
clientRunner
()
{
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wmissing-noreturn"
while
(
true
)
{
client
.
iterate
();
}
#pragma clang diagnostic pop
}
int
main
(
int
argc
,
char
**
argv
)
{
client
.
setPublishCallback
(
publishCallback
);
ros
::
init
(
argc
,
argv
,
"TORCSComponent"
);
ros
::
NodeHandle
n
;
ros
::
Publisher
cameraPub
=
n
.
advertise
<
std_msgs
::
Float32MultiArray
>
(
"/camera"
,
1
);
cameraPublisher
=
&
cameraPub
;
ros
::
Publisher
speedPub
=
n
.
advertise
<
std_msgs
::
Float32
>
(
"/speed"
,
1
);
speedPublisher
=
&
speedPub
;
ros
::
Publisher
affordancePub
=
n
.
advertise
<
std_msgs
::
Float32MultiArray
>
(
"/affordance"
,
1
);
affordancePublisher
=
&
affordancePub
;
ros
::
Subscriber
sub
=
n
.
subscribe
(
"/commands"
,
1
,
chatterCallback
);
ros
::
Rate
loop_rate
(
10
);
std
::
thread
clientRunnerThread
(
clientRunner
);
// int count = 0;
//
// while (ros::ok())
// {
//
// client->iterate();
// ros::spinOnce();
// loop_rate.sleep();
// ++count;
// }
ros
::
spin
();
clientRunnerThread
.
join
();
return
0
;
}
EMADL2TORCS/resources/
torcs/src/camera
/src/torcsclient.cpp
→
EMADL2TORCS/resources/
TORCSComponent/src/TORCSComponent
/src/torcsclient.cpp
View file @
15490e1f
...
...
@@ -12,6 +12,7 @@ using namespace torcs;
TorcsClient
::
TorcsClient
()
{
gotCommand
=
false
;
std
::
cout
<<
"TORCS Client!"
<<
std
::
endl
;
void
*
shm
=
NULL
;
...
...
@@ -66,7 +67,11 @@ TorcsClient::TorcsClient()
shared
->
steerCmd
=
0.0
;
shared
->
accelCmd
=
0.0
;
shared
->
brakeCmd
=
0.0
;
}
void
TorcsClient
::
setPublishCallback
(
std
::
function
<
void
(
std
::
shared_ptr
<
DataContainer
>
,
Affordance
)
>
callback
)
{
publishCallback
=
callback
;
}
void
TorcsClient
::
sendCommand
(
const
Command
&
cmd
)
...
...
@@ -75,7 +80,7 @@ void TorcsClient::sendCommand(const Command& cmd)
command
=
cmd
;
}
void
TorcsClient
::
iterate
(
std
::
unique_ptr
<
DataContainer
>&
screenshot
,
float
&
speed
)
void
TorcsClient
::
iterate
()
{
assert
(
connected
);
...
...
@@ -83,25 +88,66 @@ void TorcsClient::iterate(std::unique_ptr<DataContainer>& screenshot, float& spe
#pragma clang diagnostic ignored "-Wmissing-noreturn"
while
(
true
)
{
if
(
shared
->
written
==
1
)
{
// the new image data is ready to be read
auto
screenshot
=
std
::
make_shared
<
DataContainer
>
();
memcpy
(
screenshot
->
data
(),
shared
->
data
,
screenshot
->
size
());
speed
=
float
(
shared
->
speed
);
if
(
gotCommand
&&
shared
->
control
==
1
)
{
gotCommand
=
false
;
ROS_INFO
(
"Command acc: %f, brake: %f, steer: %f"
,
command
.
accelCmd
,
command
.
brakeCmd
,
command
.
steerCmd
);
shared
->
accelCmd
=
command
.
accelCmd
;
shared
->
steerCmd
=
command
.
steerCmd
;
shared
->
brakeCmd
=
command
.
brakeCmd
;
}
shared
->
fast
=
0.0
;
Affordance
affordance
;
affordance
.
populate
(
*
shared
);
publishCallback
(
screenshot
,
affordance
);
// ROS_INFO("Affordance:\n"
// "dist_L: %f\n"
// "dist_R: %f\n"
// "dist_LL: %f\n"
// "dist_MM: %f\n"
// "dist_RR: %f\n"
// "toMarking_L: %f\n"
// "toMarking_M: %f\n"
// "toMarking_R: %f\n"
// "toMarking_LL: %f\n"
// "toMarking_ML: %f\n"
// "toMarking_MR: %f\n"
// "toMarking_RR: %f\n"
// "toMiddle: %f\n"
// "angle: %f\n"
// "fast: %f\n"
// "speed: %f\n",
// affordance.getDistL(),
// affordance.getDistR(),
// affordance.getDistLL(),
// affordance.getDistMM(),
// affordance.getDistRR(),
// affordance.getToMarkingL(),
// affordance.getToMarkingM(),
// affordance.getToMarkingR(),
// affordance.getToMarkingLL(),
// affordance.getToMarkingML(),
// affordance.getToMarkingMR(),
// affordance.getToMarkingRR(),
// affordance.getToMiddle(),
// affordance.getAngle(),
// affordance.getFast(),
// affordance.getSpeed());
shared
->
written
=
0
;
shared
->
pause
=
0
;
break
;
}
else
{
if
(
shared
->
pause
!=
1
)
{
shared
->
pause
=
1
;
}
}
if
(
shared
->
control
==
1
)
{
if
(
gotCommand
)
{
gotCommand
=
false
;
ROS_INFO
(
"Command acc: %f, brake: %f, steer: %f"
,
command
.
accelCmd
,
command
.
brakeCmd
,
command
.
steerCmd
);
shared
->
accelCmd
=
command
.
accelCmd
;
shared
->
steerCmd
=
command
.
steerCmd
;
shared
->
brakeCmd
=
command
.
brakeCmd
;
}
else
{
//ROS_INFO("No command");
//shared->steerCmd = 0.0;
}
}
}
#pragma clang diagnostic pop
}
EMADL2TORCS/resources/
torcs/src/camera
/src/torcsclient.h
→
EMADL2TORCS/resources/
TORCSComponent/src/TORCSComponent
/src/torcsclient.h
View file @
15490e1f
...
...
@@ -3,6 +3,8 @@
#include <cstdint>
#include <array>
#include <memory>
#include <functional>
#include <std_msgs/Float32MultiArray.h>
namespace
torcs
{
...
...
@@ -60,20 +62,76 @@ struct Command
using
DataContainer
=
std
::
array
<
uint8_t
,
IMAGE_SIZE_BYTES
>
;
class
Affordance
{
public:
void
populate
(
const
shared_use_st
&
affordance
)
{
affordance_
[
0
]
=
float
(
affordance
.
fast
);
affordance_
[
1
]
=
float
(
affordance
.
dist_L
);
affordance_
[
2
]
=
float
(
affordance
.
dist_R
);
affordance_
[
3
]
=
float
(
affordance
.
toMarking_L
);
affordance_
[
4
]
=
float
(
affordance
.
toMarking_M
);
affordance_
[
5
]
=
float
(
affordance
.
toMarking_R
);
affordance_
[
6
]
=
float
(
affordance
.
dist_LL
);
affordance_
[
7
]
=
float
(
affordance
.
dist_MM
);
affordance_
[
8
]
=
float
(
affordance
.
dist_RR
);
affordance_
[
9
]
=
float
(
affordance
.
toMarking_LL
);
affordance_
[
10
]
=
float
(
affordance
.
toMarking_ML
);
affordance_
[
11
]
=
float
(
affordance
.
toMarking_MR
);
affordance_
[
12
]
=
float
(
affordance
.
toMarking_RR
);
affordance_
[
13
]
=
float
(
affordance
.
toMiddle
);
// not used
affordance_
[
14
]
=
float
(
affordance
.
angle
);
affordance_
[
15
]
=
float
(
affordance
.
speed
);
}
void
populate
(
const
std_msgs
::
Float32MultiArray
&
affordance
)
{
affordance_
=
affordance
.
data
;
}
float
getFast
()
{
return
affordance_
[
0
];
}
float
getDistL
()
{
return
affordance_
[
1
];
}
float
getDistR
()
{
return
affordance_
[
2
];
}
float
getToMarkingL
()
{
return
affordance_
[
3
];
}
float
getToMarkingM
()
{
return
affordance_
[
4
];
}
float
getToMarkingR
()
{
return
affordance_
[
5
];
}
float
getDistLL
()
{
return
affordance_
[
6
];
}
float
getDistMM
()
{
return
affordance_
[
7
];
}
float
getDistRR
()
{
return
affordance_
[
8
];
}
float
getToMarkingLL
()
{
return
affordance_
[
9
];
}
float
getToMarkingML
()
{
return
affordance_
[
10
];
}
float
getToMarkingMR
()
{
return
affordance_
[
11
];
}
float
getToMarkingRR
()
{
return
affordance_
[
12
];
}
float
getToMiddle
()
{
return
affordance_
[
13
];
}
float
getAngle
()
{
return
affordance_
[
14
];
}
float
getSpeed
()
{
return
affordance_
[
15
];
}
const
std
::
vector
<
float
>&
getAsVector
()
{
return
affordance_
;
}
private:
std
::
vector
<
float
>
affordance_
=
std
::
vector
<
float
>
(
16
);
};
class
TorcsClient
{
public:
TorcsClient
();
explicit
TorcsClient
();
void
iterate
(
std
::
unique_ptr
<
DataContainer
>&
screenshot
,
float
&
speed
);
void
setPublishCallback
(
std
::
function
<
void
(
std
::
shared_ptr
<
DataContainer
>
,
Affordance
)
>
callback
);
void
iterate
();
void
sendCommand
(
const
Command
&
cmd
);
private:
struct
shared_use_st
*
shared
;
bool
connected
=
false
;
bool
gotCommand
=
false
;
std
::
atomic
<
bool
>
gotCommand
;
Command
command
;
std
::
function
<
void
(
std
::
shared_ptr
<
DataContainer
>
,
Affordance
)
>
publishCallback
;
};
}
...
...
EMADL2TORCS/resources/scripts/build_all.sh
View file @
15490e1f
...
...
@@ -8,8 +8,8 @@ cmake ..
make
-j
cd
../..
echo
"Building TORCS
adapters
"
cd
torcs
echo
"Building TORCS
Component
"
cd
TORCSComponent
./build.sh
echo
...
...
EMADL2TORCS/resources/scripts/run_all.sh
View file @
15490e1f
...
...
@@ -5,6 +5,6 @@ xterm -T ROSCORE -e roscore &
echo
"Waiting for ROSCORE to start..."
sleep
2
xterm
-T
TORCSC
amera
-e
torcs/src/camera/build/camera
&
xterm
-T
TORCSC
omponent
-e
TORCSComponent/src/TORCSComponent/build/TORCSComponent
&
xterm
-T
Mastercomponent
-e
Mastercomponent/build/coordinator/Coordinator_dp_mastercomponent &
xterm
-T
Mastercomponent
-e
Mastercomponent/build/
dp_mastercomponent/
coordinator/Coordinator_dp_mastercomponent &
EMADL2TORCS/resources/torcs/src/camera/src/main.cpp
deleted
100644 → 0
View file @
f05e7021
#include "torcsclient.h"
#include <sstream>
#include <ml.h>
#include <highgui.h>
#include <cv.hpp>
#include <ros/ros.h>
#include <std_msgs/Float32.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
::
TorcsClient
client
;
std
::
unique_ptr
<
torcs
::
DataContainer
>
screenshot
=
std
::
make_unique
<
torcs
::
DataContainer
>
();
void
chatterCallback
(
const
std_msgs
::
Float32MultiArray
::
ConstPtr
&
msg
)
{
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
cameraPub
=
n
.
advertise
<
std_msgs
::
Float32MultiArray
>
(
"/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
;
while
(
ros
::
ok
())
{
float
speed
;
client
.
iterate
(
screenshot
,
speed
);
cv
::
Mat
image
(
torcs
::
IMAGE_HEIGHT
,
torcs
::
IMAGE_WIDTH
,
CV_8UC
(
torcs
::
IMAGE_CHANNELS
));
memcpy
(
image
.
data
,
screenshot
->
data
(),
torcs
::
IMAGE_SIZE_BYTES
);
cv
::
Mat
resizedImage
(
ROS_IMAGE_HEIGHT
,
ROS_IMAGE_WIDTH
,
CV_8UC
(
torcs
::
IMAGE_CHANNELS
));
cv
::
resize
(
image
,
resizedImage
,
cv
::
Size
(
ROS_IMAGE_WIDTH
,
ROS_IMAGE_HEIGHT
));
cv
::
Mat
flippedImage
(
torcs
::
IMAGE_HEIGHT
,
torcs
::
IMAGE_WIDTH
,
CV_8UC
(
torcs
::
IMAGE_CHANNELS
));
cv
::
flip
(
resizedImage
,
flippedImage
,
0
);
cv
::
Mat
rgbImage
(
torcs
::
IMAGE_HEIGHT
,
torcs
::
IMAGE_WIDTH
,
CV_8UC
(
torcs
::
IMAGE_CHANNELS
));
cv
::
cvtColor
(
flippedImage
,
rgbImage
,
cv
::
COLOR_RGB2BGR
);
//Code for debugging: displays screenshot in a window and waits for a key stroke to close it and get new one
//cv::namedWindow("Display Image 1", cv::WINDOW_AUTOSIZE );
//cv::imshow("Display Image 1", image);
//cv::waitKey(0);
//cv::destroyWindow("Display Image 1");
std
::
vector
<
float
>
data
(
0
);
data
.
reserve
(
ROS_IMAGE_SIZE
);
for
(
size_t
j
=
0
;
j
<
ROS_IMAGE_HEIGHT
;
j
++
){
for
(
size_t
k
=
0
;
k
<
ROS_IMAGE_WIDTH
;
k
++
){
cv
::
Vec3b
intensity
=
rgbImage
.
at
<
cv
::
Vec3b
>
(
j
,
k
);
for
(
size_t
i
=
0
;
i
<
3
;
i
++
){
data
.
push_back
((
float
)
intensity
[
i
]);
}
}
}
assert
(
sizeof
(
float
)
==
sizeof
(
std_msgs
::
Float32
));
std_msgs
::
Float32MultiArray
::
Ptr
msg
(
new
std_msgs
::
Float32MultiArray
);
msg
->
data
.
resize
(
ROS_IMAGE_SIZE
);
memcpy
(
msg
->
data
.
data
(),
data
.
data
(),
ROS_IMAGE_SIZE
*
sizeof
(
float
));
std_msgs
::
Float32
::
Ptr
speedMsg
(
new
std_msgs
::
Float32
);
speedMsg
->
data
=
speed
;
cameraPub
.
publish
(
msg
);
speedPub
.
publish
(
speedMsg
);
ros
::
spinOnce
();
loop_rate
.
sleep
();
++
count
;
}
return
0
;
}
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