Skip to content

Commit 250c00c

Browse files
namv1994namv1994
committed
first development version
1 parent 31586a3 commit 250c00c

File tree

7 files changed

+299
-223
lines changed

7 files changed

+299
-223
lines changed

‎CMakeLists.txt‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -67,6 +67,7 @@ add_custom_command(OUTPUT ${CMAKE_BINARY_DIR}/opendlv-standard-message-set.hpp
6767
include_directories(SYSTEM ${CMAKE_BINARY_DIR})
6868
include_directories(${CMAKE_CURRENT_SOURCE_DIR}/src)
6969

70+
add_library(${PROJECT_NAME}-core STATIC ${CMAKE_CURRENT_SOURCE_DIR}/src/collector.cpp ${CMAKE_CURRENT_SOURCE_DIR}/src/cone.cpp ${CMAKE_BINARY_DIR}/opendlv-standard-message-set.hpp)
7071
################################################################################
7172
# Threads are necessary for linking the resulting binaries as UDPReceiver is running in parallel.
7273
set(THREADS_PREFER_PTHREAD_FLAG ON)
@@ -91,7 +92,7 @@ set(LIBRARIES ${LIBRARIES} ${OpenCV_LIBS} ${EIGEN3_LIBS})
9192
################################################################################
9293
# Create executable.
9394
add_executable(${PROJECT_NAME} ${CMAKE_CURRENT_SOURCE_DIR}/src/${PROJECT_NAME}.cpp ${CMAKE_BINARY_DIR}/opendlv-standard-message-set.hpp)
94-
target_link_libraries(${PROJECT_NAME} ${LIBRARIES})
95+
target_link_libraries(${PROJECT_NAME} ${PROJECT_NAME}-core ${LIBRARIES})
9596

9697
################################################################################
9798
# Install executable.

‎docker-compose.yml‎

Lines changed: 1 addition & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -6,13 +6,4 @@ services: # Must be present exactly once at the beginning of the docker-compo
66
ipc: host
77
environment:
88
- DISPLAY=$DISPLAY
9-
command: "/usr/bin/cfsd19-cognition-detectconelane --cid=219--verbose=0 --id=211 --detectConeId=118 --slamId=120 --simDetectConeId=231 --attentionId=116 --yawRateId=112 --speedId1=1504 --speedId2=1505 --gpsId=114 \
10-
--gatheringTimeMs=50 --separationTimeMs=10 \
11-
--alwaysSlam=0 --guessDistance=3.0 --minGuessSeparation=1.5 --latePerpGuessing=0 --useCurveDetection=0 --maxConeAngle=1.570796325 --behindMemoryDistance=3.0 \
12-
--maxConeWidthSeparation=3.0 --widthSeparationMargin=1.0 \
13-
--maxConeLengthSeparation=5.0 --lengthSeparationMargin=1.0 \
14-
--nLapsToGo=1 --lapCounterLockTime=20 --stopSignalDelay=0.0 \
15-
--useOrangeLapCounter=0 --useNewConeLapCounter=1 --nMinOrangeFrames=2 --nMinDisappearanceFrames=2 \
16-
--useGpsLapCounter=0 --trackGpsTolerance=2.0 --useRawGPS=0 --refLatitude=57.710482 --refLongitude=11.950813 \
17-
--useAccelerationMode=0 --usePathMemory=0 --accGpsFinishDistance=75.0 \
18-
--useSkidpadMode=1 --debug"
9+
command: "/usr/bin/cfsd19-cognition-detectconelane --cid=219 --verbose --freq=10"

‎src/cfsd-logic-pathplanner.cpp‎

Lines changed: 31 additions & 147 deletions
Original file line numberDiff line numberDiff line change
@@ -20,9 +20,6 @@
2020

2121
#include <Eigen/Core>
2222

23-
#include <opencv2/highgui/highgui.hpp>
24-
#include <opencv2/imgproc/imgproc.hpp>
25-
2623
#include <algorithm>
2724
#include <cstdint>
2825
#include <iostream>
@@ -42,185 +39,78 @@ int32_t main(int32_t argc, char **argv) {
4239
<< "--freq=<Frequency> [--verbose]" << std::endl;
4340
} else {
4441
bool const verbose{commandlineArguments.count("verbose") != 0};
45-
4642
uint32_t localPathSenderId = 2601;
4743

48-
struct ConeObject{
49-
float x;
50-
float y;
51-
uint32_t type;
52-
};
53-
54-
std::mutex uncompleteFrameMutex;
55-
std::map<uint32_t, Cone> uncompleteFrame;
56-
uint32_t currentUncompleteFrameId;
57-
58-
std::mutex completeFrameMutex;
59-
std::map<uint32_t, Cone> completeFrame;
60-
uint32_t currentCompleteFrameId;
61-
cluon::data::TimeStamp frameStart;
62-
cluon::data::TimeStamp frameEnd;
44+
Collector collector(commandlineArguments);
6345

6446
cluon::OD4Session od4{
6547
static_cast<uint16_t>(std::stoi(commandlineArguments["cid"]))};
6648

67-
auto onObjectFrameStart{[&uncompleteFrameMutex, &uncompleteFrame,
68-
&currentUncompleteFrameId, &frameStart, &verbose](
49+
auto onObjectFrameStart{[&planner = collector](
6950
cluon::data::Envelope &&envelope)
7051
{
7152
// TODO: for now assume senderStamp 0 for perception (add SLAM later)
7253
if (envelope.senderStamp() == 0) {
73-
auto msg =
74-
cluon::extractMessage<opendlv::logic::perception::ObjectFrameStart>(
75-
std::move(envelope));
76-
77-
uint32_t objectFrameId = msg.objectFrameId();
78-
{
79-
std::lock_guard<std::mutex> lock(uncompleteFrameMutex);
80-
uncompleteFrame = std::map<uint32_t, Cone>();
81-
currentUncompleteFrameId = objectFrameId;
82-
frameStart = envelope.sent();
83-
}
84-
85-
if (verbose) {
86-
std::cout << "Got frame START with id=" << objectFrameId
87-
<< std::endl;
88-
}
54+
planner.getObjectFrameStart(envelope);
8955
}
9056
}};
9157

92-
auto onObjectFrameEnd{[&uncompleteFrameMutex, &uncompleteFrame,
93-
&currentUncompleteFrameId, &completeFrameMutex, &completeFrame,
94-
&currentCompleteFrameId, &frameEnd, &verbose](
58+
auto onObjectFrameEnd{[&planner = collector](
9559
cluon::data::Envelope &&envelope)
9660
{
9761
// TODO: for now assume senderStamp 0 for perception (add SLAM later)
9862
if (envelope.senderStamp() == 0) {
99-
auto msg =
100-
cluon::extractMessage<opendlv::logic::perception::ObjectFrameEnd>(
101-
std::move(envelope));
102-
103-
uint32_t objectFrameId = msg.objectFrameId();
104-
{
105-
std::lock_guard<std::mutex> lock1(completeFrameMutex);
106-
std::lock_guard<std::mutex> lock2(uncompleteFrameMutex);
107-
completeFrame = uncompleteFrame;
108-
currentCompleteFrameId = currentUncompleteFrameId;
109-
frameEnd = envelope.sent();
110-
}
111-
112-
if (verbose) {
113-
std::cout << "Got frame END with id=" << objectFrameId
114-
<< std::endl;
115-
}
63+
planner.getObjectFrameEnd(envelope);
11664
}
11765
}};
11866

119-
auto onObject{[&uncompleteFrameMutex, &uncompleteFrame, &verbose](
67+
auto onObject{[&planner = collector](
12068
cluon::data::Envelope &&envelope)
12169
{
122-
auto msg =
123-
cluon::extractMessage<opendlv::logic::perception::Object>(
124-
std::move(envelope));
125-
126-
uint32_t objectId = msg.objectId();
127-
{
128-
std::lock_guard<std::mutex> lock(uncompleteFrameMutex);
129-
uncompleteFrame[objectId] = Cone();
130-
}
131-
if (verbose) {
132-
std::cout << "Got NEW OBJECT with id=" << objectId << std::endl;
133-
}
70+
planner.getObject(envelope);
13471
}};
13572

136-
auto onObjectType{[&uncompleteFrameMutex, &uncompleteFrame, &verbose](
73+
auto onObjectType{[&planner = collector](
13774
cluon::data::Envelope &&envelope)
13875
{
139-
auto msg =
140-
cluon::extractMessage<opendlv::logic::perception::ObjectType>(
141-
std::move(envelope));
142-
143-
uint32_t objectId = msg.objectId();
144-
{
145-
std::lock_guard<std::mutex> lock(uncompleteFrameMutex);
146-
if (uncompleteFrame.count(objectId)) {
147-
uncompleteFrame[objectId].type = msg.type();
148-
}
149-
}
150-
if (verbose) {
151-
std::cout << "Got OBJECT TYPE for object with id=" << objectId
152-
<< " and type=" << msg.type() << std::endl;
153-
}
76+
planner.getObjectType(envelope);
15477
}};
15578

156-
auto onObjectPosition{[&uncompleteFrameMutex, &uncompleteFrame, &verbose](
79+
auto onObjectPosition{[&planner = collector](
15780
cluon::data::Envelope &&envelope)
15881
{
159-
auto msg =
160-
cluon::extractMessage<opendlv::logic::perception::ObjectPosition>(
161-
std::move(envelope));
162-
163-
uint32_t objectId = msg.objectId();
164-
{
165-
std::lock_guard<std::mutex> lock(uncompleteFrameMutex);
166-
if (uncompleteFrame.count(objectId)) {
167-
uncompleteFrame[objectId].x = msg.x();
168-
uncompleteFrame[objectId].y = msg.y();
169-
}
170-
}
171-
if (verbose) {
172-
std::cout << "Got OBJECT POSITION for object with id=" << objectId
173-
<< " and x=" << msg.x() << " y=" << msg.y() << std::endl;
174-
}
82+
planner.getObjectPosition(envelope);
17583
}};
17684

177-
auto onEquilibrioception{[&uncompleteFrameMutex, &uncompleteFrame, &verbose](
85+
auto onEquilibrioception{[&planner = collector](
17886
cluon::data::Envelope &&envelope)
17987
{
180-
auto msg =
181-
cluon::extractMessage<opendlv::logic::sensation::Equilibrioception>(
182-
std::move(envelope));
183-
184-
float vx = msg.vx();
185-
float yawRate = msg.yawRate();
186-
if (verbose) {
187-
std::cout << "Got EQUILIBRIOCEPTION vx=" << vx << " and yawRate="
188-
<< yawRate << std::endl;
189-
}
88+
planner.getEquilibrioception(envelope);
19089
}};
191-
192-
auto atFrequency{[&od4, &completeFrameMutex, &completeFrame,
193-
&localPathSenderId, &frameStart, &frameEnd, &verbose]() -> bool
90+
91+
auto atFrequency{[&od4, &verbose, &collector, &localPathSenderId]() -> bool
19492
{
195-
std::vector<std::pair<float, float>> path;
196-
19793
{
198-
std::lock_guard<std::mutex> lock(completeFrameMutex);
94+
collector.GetCompleteFrameCFSD19();
95+
19996

20097
if (verbose) {
201-
uint64_t frameDuration = cluon::time::toMicroseconds(frameEnd)
202-
- cluon::time::toMicroseconds(frameStart);
98+
uint64_t frameDuration = cluon::time::toMicroseconds(collector.frameEnd)
99+
- cluon::time::toMicroseconds(collector.frameStart);
203100
std::cout << "Using frame to find path, frame duration="
204-
<< frameDuration << ", number of cones=" << completeFrame.size()
101+
<< frameDuration << ", number of cones=" << collector.m_currentConeFrame.size()
205102
<< std::endl;
206103
}
207-
208-
for (auto c : completeFrame) {
209-
if (verbose) {
210-
std::cout << " x=" << c.second.x << ", y=" << c.second.y
211-
<< ", type=" << c.second.type << std::endl;
212-
}
213-
}
214-
215-
path.push_back(std::pair<float, float>(1.23f, 2.34f));
216-
path.push_back(std::pair<float, float>(3.45f, 6.78f));
104+
105+
collector.ProcessFrameCFSD19();
217106
}
218107

219-
uint32_t length = path.size();
108+
uint32_t length = collector.middlePath.size();
220109
std::string data;
221-
for (auto c : path) {
222-
float x = c.first;
223-
float y = c.second;
110+
if(collector.middlePath.size()){
111+
for (auto c : collector.middlePath) {
112+
float x = (float)c.x;
113+
float y = (float)c.y;
224114
float z = 0.0f;
225115

226116
char r[12];
@@ -230,22 +120,17 @@ int32_t main(int32_t argc, char **argv) {
230120

231121
data += std::string(r, 12);
232122
}
233-
123+
}
234124
cluon::data::TimeStamp ts = cluon::time::now();
235125

236126
opendlv::logic::action::LocalPath localPath;
237127
localPath.length(length);
238128
localPath.data(data);
239129
od4.send(localPath, ts, localPathSenderId);
240130

241-
if (verbose) {
242-
cv::Mat img(320, 240, CV_8UC3, cv::Scalar(0,0,0));
243-
cv::imshow("Visualization", img);
244-
cv::waitKey(1);
245-
}
246-
247131
return true;
248-
}};
132+
}};
133+
249134

250135
od4.dataTrigger(opendlv::logic::perception::ObjectFrameStart::ID(),
251136
onObjectFrameStart);
@@ -259,9 +144,8 @@ int32_t main(int32_t argc, char **argv) {
259144
onObjectPosition);
260145
od4.dataTrigger(opendlv::logic::sensation::Equilibrioception::ID(),
261146
onEquilibrioception);
262-
263147
od4.timeTrigger(std::stoi(commandlineArguments["freq"]), atFrequency);
264-
148+
265149
retCode = 0;
266150
}
267151
return retCode;

0 commit comments

Comments
 (0)