Skip to content
Projects
Groups
Snippets
Help
Loading...
Help
Support
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Q
qgroundcontrol
Project
Project
Details
Activity
Releases
Cycle Analytics
Repository
Repository
Files
Commits
Branches
Tags
Contributors
Graph
Compare
Charts
Issues
0
Issues
0
List
Boards
Labels
Milestones
Merge Requests
0
Merge Requests
0
CI / CD
CI / CD
Pipelines
Jobs
Schedules
Charts
Wiki
Wiki
Snippets
Snippets
Members
Members
Collapse sidebar
Close sidebar
Activity
Graph
Charts
Create a new issue
Jobs
Commits
Issue Boards
Open sidebar
Valentin Platzgummer
qgroundcontrol
Commits
d47dc9a9
Commit
d47dc9a9
authored
Jul 31, 2020
by
Valentin Platzgummer
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
bug appearded again
parent
29b83181
Changes
4
Hide whitespace changes
Inline
Side-by-side
Showing
4 changed files
with
44 additions
and
45 deletions
+44
-45
WimaController.cc
src/Wima/WimaController.cc
+19
-19
TopicPublisher.cpp
src/comm/ros_bridge/include/TopicPublisher.cpp
+18
-20
TopicPublisher.h
src/comm/ros_bridge/include/TopicPublisher.h
+6
-5
TopicSubscriber.cpp
src/comm/ros_bridge/include/TopicSubscriber.cpp
+1
-1
No files found.
src/Wima/WimaController.cc
View file @
d47dc9a9
...
...
@@ -719,27 +719,27 @@ void WimaController::_eventTimerHandler()
auto
start
=
std
::
chrono
::
high_resolution_clock
::
now
();
_pRosBridge
->
publish
(
_snakeTilesLocal
,
"/snake/tiles"
);
_pRosBridge
->
publish
(
_snakeOrigin
,
"/snake/origin"
);
_pRosBridge
->
publish
(
_snakeTilesLocal
,
"/snake/tiles"
);
using
namespace
std
::
placeholders
;
auto
progressCallBack
=
std
::
bind
(
&
WimaController
::
_progressFromJson
,
this
,
_1
,
std
::
ref
(
_nemoProgress
));
_pRosBridge
->
subscribe
(
"/nemo/progress"
,
progressCallBack
);
_pRosBridge
->
subscribe
(
"/nemo/heartbeat"
,
[
this
,
&
nemoStatusTicker
](
JsonDocUPtr
pDoc
){
if
(
!
this
->
_pRosBridge
->
casePacker
()
->
unpack
(
pDoc
,
this
->
_nemoHeartbeat
)
)
{
if
(
this
->
_nemoHeartbeat
.
status
()
==
this
->
_fallbackStatus
)
return
;
this
->
_nemoHeartbeat
.
setStatus
(
this
->
_fallbackStatus
);
}
nemoStatusTicker
.
reset
();
this
->
_fallbackStatus
=
-
1
;
/*Timeout*/
emit
WimaController
::
nemoStatusChanged
();
emit
WimaController
::
nemoStatusStringChanged
();
});
//
using namespace std::placeholders;
//
auto progressCallBack = std::bind(&WimaController::_progressFromJson,
//
this,
//
_1,
//
std::ref(_nemoProgress));
//
_pRosBridge->subscribe("/nemo/progress", progressCallBack);
//
_pRosBridge->subscribe("/nemo/heartbeat", [this, &nemoStatusTicker](JsonDocUPtr pDoc){
//
if ( !this->_pRosBridge->casePacker()->unpack(pDoc, this->_nemoHeartbeat) ) {
//
if ( this->_nemoHeartbeat.status() == this->_fallbackStatus )
//
return;
//
this->_nemoHeartbeat.setStatus(this->_fallbackStatus);
//
}
//
nemoStatusTicker.reset();
//
this->_fallbackStatus = -1; /*Timeout*/
//
emit WimaController::nemoStatusChanged();
//
emit WimaController::nemoStatusStringChanged();
//
});
auto
end
=
std
::
chrono
::
high_resolution_clock
::
now
();
qWarning
()
<<
"Duration: "
<<
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
end
-
start
).
count
()
...
...
src/comm/ros_bridge/include/TopicPublisher.cpp
View file @
d47dc9a9
...
...
@@ -37,11 +37,6 @@ void ROSBridge::ComPrivate::TopicPublisher::start()
if
(
_running
.
load
()
)
// start called while thread running.
return
;
_running
.
store
(
true
);
{
std
::
lock_guard
<
std
::
mutex
>
lk
(
*
_rbcMutex
);
_rbc
->
addClient
(
ROSBridge
::
ComPrivate
::
_topicAdvertiserKey
);
_rbc
->
addClient
(
ROSBridge
::
ComPrivate
::
_topicPublisherKey
);
}
ROSBridge
::
ComPrivate
::
ThreadData
data
{
*
_casePacker
,
*
_rbc
,
...
...
@@ -64,17 +59,19 @@ void ROSBridge::ComPrivate::TopicPublisher::reset()
if
(
!
_pThread
)
return
;
_pThread
->
join
();
{
std
::
lock_guard
<
std
::
mutex
>
lk
(
*
_rbcMutex
);
_rbc
->
removeClient
(
ROSBridge
::
ComPrivate
::
_topicAdvertiserKey
);
_rbc
->
removeClient
(
ROSBridge
::
ComPrivate
::
_topicPublisherKey
);
}
_queue
.
clear
();
_advertisedTopicsHashList
.
clear
();
}
void
ROSBridge
::
ComPrivate
::
transmittLoop
(
ROSBridge
::
ComPrivate
::
ThreadData
data
)
{
// Init.
{
std
::
lock_guard
<
std
::
mutex
>
lk
(
data
.
rbcMutex
);
data
.
rbc
.
addClient
(
ROSBridge
::
ComPrivate
::
_topicAdvertiserKey
);
data
.
rbc
.
addClient
(
ROSBridge
::
ComPrivate
::
_topicPublisherKey
);
}
// Main Loop.
while
(
data
.
running
.
load
()){
std
::
unique_lock
<
std
::
mutex
>
lk
(
data
.
queueMutex
);
// Check if new data available, wait if not.
...
...
@@ -87,13 +84,6 @@ void ROSBridge::ComPrivate::transmittLoop(ROSBridge::ComPrivate::ThreadData data
data
.
queue
.
pop_front
();
lk
.
unlock
();
// Debug output.
// std::cout << "Transmitter loop json document:" << std::endl;
// rapidjson::OStreamWrapper out(std::cout);
// rapidjson::Writer<rapidjson::OStreamWrapper> writer(out);
// pJsonDoc->Accept(writer);
// std::cout << std::endl << std::endl;
// Get tag from Json message and remove it.
Tag
tag
;
bool
ret
=
data
.
casePacker
.
getTag
(
pJsonDoc
,
tag
);
...
...
@@ -108,20 +98,28 @@ void ROSBridge::ComPrivate::transmittLoop(ROSBridge::ComPrivate::ThreadData data
if
(
data
.
advertisedTopicsHashList
.
count
(
hash
)
==
0
)
{
data
.
advertisedTopicsHashList
.
insert
(
hash
);
{
std
::
cout
<<
ROSBridge
::
ComPrivate
::
_topicAdvertiserKey
<<
";"
<<
tag
.
topic
()
<<
";"
<<
tag
.
messageType
()
<<
";"
<<
std
::
endl
;
std
::
lock_guard
<
std
::
mutex
>
lk
(
data
.
rbcMutex
);
data
.
rbc
.
advertise
(
ROSBridge
::
ComPrivate
::
_topicAdvertiserKey
,
tag
.
topic
(),
tag
.
messageType
()
);
}
}
// Debug output.
//std::cout << "Hash Set size: " << advertisedTopicsHashList.size() << std::endl;
//
Send
Json message.
//
Publish
Json message.
{
std
::
lock_guard
<
std
::
mutex
>
lk
(
data
.
rbcMutex
);
data
.
rbc
.
publish
(
tag
.
topic
(),
*
pJsonDoc
.
get
());
}
}
// while loop
// Tidy up.
{
std
::
lock_guard
<
std
::
mutex
>
lk
(
data
.
rbcMutex
);
data
.
rbc
.
removeClient
(
ROSBridge
::
ComPrivate
::
_topicAdvertiserKey
);
data
.
rbc
.
removeClient
(
ROSBridge
::
ComPrivate
::
_topicPublisherKey
);
}
}
src/comm/ros_bridge/include/TopicPublisher.h
View file @
d47dc9a9
...
...
@@ -36,11 +36,6 @@ public:
//! @brief Resets the publisher.
void
reset
();
template
<
class
T
>
void
publish
(
const
T
&
msg
,
const
std
::
string
&
topic
){
JsonDocUPtr
docPtr
(
_casePacker
->
pack
(
msg
,
topic
));
publish
(
std
::
move
(
docPtr
));
}
void
publish
(
JsonDocUPtr
docPtr
){
{
std
::
lock_guard
<
std
::
mutex
>
lock
(
_queueMutex
);
...
...
@@ -49,6 +44,12 @@ public:
_cv
.
notify_one
();
// Wake publisher thread.
}
template
<
class
T
>
void
publish
(
const
T
&
msg
,
const
std
::
string
&
topic
){
JsonDocUPtr
docPtr
(
_casePacker
->
pack
(
msg
,
topic
));
publish
(
std
::
move
(
docPtr
));
}
private:
JsonQueue
_queue
;
std
::
mutex
_queueMutex
;
...
...
src/comm/ros_bridge/include/TopicSubscriber.cpp
View file @
d47dc9a9
...
...
@@ -24,13 +24,13 @@ void ROSBridge::ComPrivate::TopicSubscriber::start()
void
ROSBridge
::
ComPrivate
::
TopicSubscriber
::
reset
()
{
if
(
_running
){
_running
=
false
;
{
std
::
lock_guard
<
std
::
mutex
>
lk
(
*
_rbcMutex
);
for
(
std
::
string
clientName
:
_clientList
){
_rbc
->
removeClient
(
clientName
);
}
}
_running
=
false
;
{
std
::
lock_guard
<
std
::
mutex
>
lk
(
_callbackMap
.
mutex
);
_callbackMap
.
map
.
clear
();
...
...
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