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
fd49a04a
Commit
fd49a04a
authored
Aug 20, 2020
by
Valentin Platzgummer
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
enableLowBatteryHandling issue
parent
46b4c65d
Changes
11
Show whitespace changes
Inline
Side-by-side
Showing
11 changed files
with
152 additions
and
67 deletions
+152
-67
qgroundcontrol.pro
qgroundcontrol.pro
+1
-0
Wima.SettingsGroup.json
src/Settings/Wima.SettingsGroup.json
+6
-0
WimaSettings.cc
src/Settings/WimaSettings.cc
+1
-0
WimaSettings.h
src/Settings/WimaSettings.h
+1
-0
WimaController.cc
src/Wima/WimaController.cc
+41
-18
RosBridgeClient.cpp
src/comm/ros_bridge/include/RosBridgeClient.cpp
+62
-38
RosBridgeClient.h
src/comm/ros_bridge/include/RosBridgeClient.h
+7
-2
TopicPublisher.cpp
src/comm/ros_bridge/include/TopicPublisher.cpp
+3
-1
TopicSubscriber.cpp
src/comm/ros_bridge/include/TopicSubscriber.cpp
+3
-1
ROSBridge.cpp
src/comm/ros_bridge/src/ROSBridge.cpp
+5
-4
GeneralSettings.qml
src/ui/preferences/GeneralSettings.qml
+22
-3
No files found.
qgroundcontrol.pro
View file @
fd49a04a
...
@@ -32,6 +32,7 @@ DebugBuild {
...
@@ -32,6 +32,7 @@ DebugBuild {
}
}
else
{
else
{
DESTDIR
=
$$
{
OUT_PWD
}
/
release
DESTDIR
=
$$
{
OUT_PWD
}
/
release
DEFINES
+=
DEBUG
DEFINES
+=
NDEBUG
DEFINES
+=
NDEBUG
}
}
...
...
src/Settings/Wima.SettingsGroup.json
View file @
fd49a04a
...
@@ -19,4 +19,10 @@
...
@@ -19,4 +19,10 @@
"units"
:
"s"
,
"units"
:
"s"
,
"defaultValue"
:
15
"defaultValue"
:
15
}
}
{
"name"
:
"rosbridgeConnectionString"
,
"shortDescription"
:
"Ros Bridge Connection String (host:port)."
,
"type"
:
"string"
,
"defaultValue"
:
"localhost:9090"
}
]
]
src/Settings/WimaSettings.cc
View file @
fd49a04a
...
@@ -11,3 +11,4 @@ DECLARE_SETTINGGROUP(Wima, "Wima")
...
@@ -11,3 +11,4 @@ DECLARE_SETTINGGROUP(Wima, "Wima")
DECLARE_SETTINGSFACT
(
WimaSettings
,
lowBatteryThreshold
)
DECLARE_SETTINGSFACT
(
WimaSettings
,
lowBatteryThreshold
)
DECLARE_SETTINGSFACT
(
WimaSettings
,
enableLowBatteryHandling
)
DECLARE_SETTINGSFACT
(
WimaSettings
,
enableLowBatteryHandling
)
DECLARE_SETTINGSFACT
(
WimaSettings
,
minimalRemainingMissionTime
)
DECLARE_SETTINGSFACT
(
WimaSettings
,
minimalRemainingMissionTime
)
DECLARE_SETTINGSFACT
(
WimaSettings
,
rosbridgeConnectionString
)
src/Settings/WimaSettings.h
View file @
fd49a04a
...
@@ -13,4 +13,5 @@ public:
...
@@ -13,4 +13,5 @@ public:
DEFINE_SETTINGFACT
(
lowBatteryThreshold
)
DEFINE_SETTINGFACT
(
lowBatteryThreshold
)
DEFINE_SETTINGFACT
(
enableLowBatteryHandling
)
DEFINE_SETTINGFACT
(
enableLowBatteryHandling
)
DEFINE_SETTINGFACT
(
minimalRemainingMissionTime
)
DEFINE_SETTINGFACT
(
minimalRemainingMissionTime
)
DEFINE_SETTINGFACT
(
rosbridgeConnectionString
)
};
};
src/Wima/WimaController.cc
View file @
fd49a04a
...
@@ -368,6 +368,7 @@ bool WimaController::setWimaPlanData(const WimaPlanData &planData)
...
@@ -368,6 +368,7 @@ bool WimaController::setWimaPlanData(const WimaPlanData &planData)
// reset visual items
// reset visual items
_areas
.
clear
();
_areas
.
clear
();
_defaultManager
.
clear
();
_defaultManager
.
clear
();
_snakeManager
.
clear
();
_snakeTiles
.
polygons
().
clear
();
_snakeTiles
.
polygons
().
clear
();
_snakeTilesLocal
.
polygons
().
clear
();
_snakeTilesLocal
.
polygons
().
clear
();
_snakeTileCenterPoints
.
clear
();
_snakeTileCenterPoints
.
clear
();
...
@@ -533,6 +534,15 @@ bool WimaController::setWimaPlanData(const WimaPlanData &planData)
...
@@ -533,6 +534,15 @@ bool WimaController::setWimaPlanData(const WimaPlanData &planData)
emit
snakeTilesChanged
();
emit
snakeTilesChanged
();
emit
snakeTileCenterPointsChanged
();
emit
snakeTileCenterPointsChanged
();
if
(
_enableSnake
.
rawValue
().
toBool
()
&&
_pRosBridge
->
isRunning
()
&&
_pRosBridge
->
connected
()
){
if
(
_snakeTilesLocal
.
polygons
().
size
()
>
0
){
_pRosBridge
->
publish
(
_snakeOrigin
,
"/snake/origin"
);
_pRosBridge
->
publish
(
_snakeTilesLocal
,
"/snake/tiles"
);
}
}
_localPlanDataValid
=
true
;
_localPlanDataValid
=
true
;
return
true
;
return
true
;
}
}
...
@@ -723,14 +733,21 @@ void WimaController::_eventTimerHandler()
...
@@ -723,14 +733,21 @@ void WimaController::_eventTimerHandler()
}
}
if
(
_snakeTicker
.
ready
()
)
{
if
(
_snakeTicker
.
ready
()
)
{
static
bool
setupDone
=
false
;
if
(
_enableSnake
.
rawValue
().
toBool
()
)
{
if
(
_enableSnake
.
rawValue
().
toBool
()
)
{
if
(
!
_pRosBridge
->
isRunning
()
&&
_pRosBridge
->
connected
()
)
{
if
(
!
_pRosBridge
->
isRunning
())
{
_pRosBridge
->
start
();
}
else
if
(
_pRosBridge
->
isRunning
()
&&
_pRosBridge
->
connected
()
&&
!
setupDone
)
{
_pRosBridge
->
reset
();
_pRosBridge
->
start
();
_setupTopicService
();
_setupTopicService
();
setupDone
=
true
;
}
else
if
(
_pRosBridge
->
isRunning
()
&&
!
_pRosBridge
->
connected
()
){
}
else
if
(
_pRosBridge
->
isRunning
()
&&
!
_pRosBridge
->
connected
()
){
_pRosBridge
->
reset
()
;
setupDone
=
false
;
}
}
}
else
if
(
_pRosBridge
->
isRunning
()
)
{
}
else
if
(
_pRosBridge
->
isRunning
()
)
{
_pRosBridge
->
reset
();
_pRosBridge
->
reset
();
setupDone
=
false
;
}
}
}
}
}
}
...
@@ -908,16 +925,23 @@ void WimaController::_switchSnakeManager(QVariant variant)
...
@@ -908,16 +925,23 @@ void WimaController::_switchSnakeManager(QVariant variant)
void
WimaController
::
_setupTopicService
()
void
WimaController
::
_setupTopicService
()
{
{
_pRosBridge
->
start
();
if
(
_snakeTilesLocal
.
polygons
().
size
()
>
0
){
_pRosBridge
->
publish
(
_snakeOrigin
,
"/snake/origin"
);
_pRosBridge
->
publish
(
_snakeOrigin
,
"/snake/origin"
);
_pRosBridge
->
publish
(
_snakeTilesLocal
,
"/snake/tiles"
);
_pRosBridge
->
publish
(
_snakeTilesLocal
,
"/snake/tiles"
);
}
_pRosBridge
->
subscribe
(
"/nemo/progress"
,
_pRosBridge
->
subscribe
(
"/nemo/progress"
,
/* callback */
[
this
](
JsonDocUPtr
pDoc
){
/* callback */
[
this
](
JsonDocUPtr
pDoc
){
int
requiredSize
=
this
->
_snakeTilesLocal
.
polygons
().
size
();
int
requiredSize
=
this
->
_snakeTilesLocal
.
polygons
().
size
();
auto
&
progress
=
this
->
_nemoProgress
;
auto
&
progress
=
this
->
_nemoProgress
;
if
(
!
this
->
_pRosBridge
->
casePacker
()
->
unpack
(
pDoc
,
progress
)
if
(
!
this
->
_pRosBridge
->
casePacker
()
->
unpack
(
pDoc
,
progress
)
||
progress
.
progress
().
size
()
!=
requiredSize
)
{
||
progress
.
progress
().
size
()
!=
requiredSize
)
{
// Some error occured.
// Set progress to default.
progress
.
progress
().
fill
(
0
,
requiredSize
);
progress
.
progress
().
fill
(
0
,
requiredSize
);
// Publish origin and tiles again.
if
(
this
->
_snakeTilesLocal
.
polygons
().
size
()
>
0
){
this
->
_pRosBridge
->
publish
(
this
->
_snakeOrigin
,
"/snake/origin"
);
this
->
_pRosBridge
->
publish
(
this
->
_snakeTilesLocal
,
"/snake/tiles"
);
}
}
}
emit
WimaController
::
nemoProgressChanged
();
emit
WimaController
::
nemoProgressChanged
();
...
@@ -936,32 +960,31 @@ void WimaController::_setupTopicService()
...
@@ -936,32 +960,31 @@ void WimaController::_setupTopicService()
emit
WimaController
::
nemoStatusStringChanged
();
emit
WimaController
::
nemoStatusStringChanged
();
});
});
auto
pOrigin
=
&
_snakeOrigin
;
auto
casePacker
=
_pCasePacker
;
_pRosBridge
->
advertiseService
(
"/snake/get_origin"
,
"snake_msgs/GetOrigin"
,
_pRosBridge
->
advertiseService
(
"/snake/get_origin"
,
"snake_msgs/GetOrigin"
,
[
casePacker
,
pOrigin
](
JsonDocUPtr
)
->
JsonDocUPtr
{
[
this
](
JsonDocUPtr
)
->
JsonDocUPtr
{
JsonDocUPtr
pDoc
=
casePacker
->
pack
(
*
pOrigin
,
""
);
JsonDocUPtr
pDoc
;
casePacker
->
removeTag
(
pDoc
);
if
(
this
->
_snakeTilesLocal
.
polygons
().
size
()
>
0
){
pDoc
=
this
->
_pCasePacker
->
pack
(
this
->
_snakeOrigin
,
""
);
}
else
{
pDoc
=
this
->
_pCasePacker
->
pack
(
::
GeoPoint3D
(
0
,
0
,
0
),
""
);
}
this
->
_pCasePacker
->
removeTag
(
pDoc
);
rapidjson
::
Document
value
(
rapidjson
::
kObjectType
);
rapidjson
::
Document
value
(
rapidjson
::
kObjectType
);
JsonDocUPtr
pReturn
(
new
rapidjson
::
Document
(
rapidjson
::
kObjectType
));
JsonDocUPtr
pReturn
(
new
rapidjson
::
Document
(
rapidjson
::
kObjectType
));
value
.
CopyFrom
(
*
pDoc
,
pReturn
->
GetAllocator
());
value
.
CopyFrom
(
*
pDoc
,
pReturn
->
GetAllocator
());
pReturn
->
AddMember
(
"origin"
,
value
,
pReturn
->
GetAllocator
());
pReturn
->
AddMember
(
"origin"
,
value
,
pReturn
->
GetAllocator
());
return
pReturn
;
return
pReturn
;
});
});
auto
pTiles
=
&
_snakeTilesLocal
;
_pRosBridge
->
advertiseService
(
"/snake/get_tiles"
,
"snake_msgs/GetTiles"
,
_pRosBridge
->
advertiseService
(
"/snake/get_tiles"
,
"snake_msgs/GetTiles"
,
[
casePacker
,
pTiles
](
JsonDocUPtr
)
->
JsonDocUPtr
{
[
this
](
JsonDocUPtr
)
->
JsonDocUPtr
{
JsonDocUPtr
pDoc
=
this
->
_pCasePacker
->
pack
(
this
->
_snakeTilesLocal
,
""
);
JsonDocUPtr
pDoc
=
casePacker
->
pack
(
*
pTiles
,
""
);
this
->
_pCasePacker
->
removeTag
(
pDoc
);
casePacker
->
removeTag
(
pDoc
);
rapidjson
::
Document
value
(
rapidjson
::
kObjectType
);
rapidjson
::
Document
value
(
rapidjson
::
kObjectType
);
JsonDocUPtr
pReturn
(
new
rapidjson
::
Document
(
rapidjson
::
kObjectType
));
JsonDocUPtr
pReturn
(
new
rapidjson
::
Document
(
rapidjson
::
kObjectType
));
value
.
CopyFrom
(
*
pDoc
,
pReturn
->
GetAllocator
());
value
.
CopyFrom
(
*
pDoc
,
pReturn
->
GetAllocator
());
pReturn
->
AddMember
(
"tiles"
,
value
,
pReturn
->
GetAllocator
());
pReturn
->
AddMember
(
"tiles"
,
value
,
pReturn
->
GetAllocator
());
return
pReturn
;
return
pReturn
;
});
});
}
}
src/comm/ros_bridge/include/RosBridgeClient.cpp
View file @
fd49a04a
...
@@ -74,7 +74,6 @@ void RosbridgeWsClient::start(const std::__cxx11::string &client_name, std::shar
...
@@ -74,7 +74,6 @@ void RosbridgeWsClient::start(const std::__cxx11::string &client_name, std::shar
client
->
on_close
=
NULL
;
client
->
on_close
=
NULL
;
client
->
on_error
=
NULL
;
client
->
on_error
=
NULL
;
#ifdef DEBUG
#ifdef DEBUG
std
::
cout
<<
"start"
<<
client_name
<<
" client reference count:"
<<
client
.
use_count
()
<<
std
::
endl
;
std
::
cout
<<
client_name
<<
" thread end"
<<
std
::
endl
;
std
::
cout
<<
client_name
<<
" thread end"
<<
std
::
endl
;
#endif
#endif
});
});
...
@@ -82,11 +81,34 @@ void RosbridgeWsClient::start(const std::__cxx11::string &client_name, std::shar
...
@@ -82,11 +81,34 @@ void RosbridgeWsClient::start(const std::__cxx11::string &client_name, std::shar
client_thread
.
detach
();
client_thread
.
detach
();
}
}
RosbridgeWsClient
::
RosbridgeWsClient
(
const
std
::
__cxx11
::
string
&
server_port_path
)
:
RosbridgeWsClient
::
RosbridgeWsClient
(
const
std
::
string
&
server_port_path
,
bool
run
)
:
server_port_path
(
server_port_path
)
server_port_path
(
server_port_path
)
,
isConnected
(
std
::
make_shared
<
std
::
atomic_bool
>
(
false
))
,
isConnected
(
std
::
make_shared
<
std
::
atomic_bool
>
(
false
))
,
stopped
(
std
::
make_shared
<
std
::
atomic_bool
>
(
false
))
,
stopped
(
std
::
make_shared
<
std
::
atomic_bool
>
(
true
))
{
if
(
run
)
this
->
run
();
}
RosbridgeWsClient
::
RosbridgeWsClient
(
const
std
::
__cxx11
::
string
&
server_port_path
)
:
RosbridgeWsClient
::
RosbridgeWsClient
(
server_port_path
,
true
)
{
}
RosbridgeWsClient
::~
RosbridgeWsClient
()
{
reset
();
}
bool
RosbridgeWsClient
::
connected
(){
return
isConnected
->
load
();
}
void
RosbridgeWsClient
::
run
()
{
{
if
(
!
stopped
->
load
()
)
return
;
stopped
->
store
(
false
);
// Start periodic thread to monitor connection status, advertised topics etc.
// Start periodic thread to monitor connection status, advertised topics etc.
periodic_thread
=
std
::
make_shared
<
std
::
thread
>
([
this
]
{
periodic_thread
=
std
::
make_shared
<
std
::
thread
>
([
this
]
{
std
::
list
<
Task
>
task_list
;
std
::
list
<
Task
>
task_list
;
...
@@ -165,7 +187,7 @@ RosbridgeWsClient::RosbridgeWsClient(const std::__cxx11::string &server_port_pat
...
@@ -165,7 +187,7 @@ RosbridgeWsClient::RosbridgeWsClient(const std::__cxx11::string &server_port_pat
this
->
callService
(
"/rosapi/topics"
,
[
topics_set
,
this
](
this
->
callService
(
"/rosapi/topics"
,
[
topics_set
,
this
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
,
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
,
std
::
shared_ptr
<
WsClient
::
InMessage
>
in_message
){
std
::
shared_ptr
<
WsClient
::
InMessage
>
in_message
){
std
::
cout
<<
"/rosapi/topics: "
<<
in_message
->
string
()
<<
std
::
endl
;
//
std::cout << "/rosapi/topics: " << in_message->string() << std::endl;
std
::
unique_lock
<
std
::
mutex
>
lk
(
this
->
mutex
);
std
::
unique_lock
<
std
::
mutex
>
lk
(
this
->
mutex
);
this
->
available_topics
=
in_message
->
string
();
this
->
available_topics
=
in_message
->
string
();
lk
.
unlock
();
lk
.
unlock
();
...
@@ -212,7 +234,7 @@ RosbridgeWsClient::RosbridgeWsClient(const std::__cxx11::string &server_port_pat
...
@@ -212,7 +234,7 @@ RosbridgeWsClient::RosbridgeWsClient(const std::__cxx11::string &server_port_pat
this
->
callService
(
"/rosapi/services"
,
[
this
,
services_set
](
this
->
callService
(
"/rosapi/services"
,
[
this
,
services_set
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
,
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
,
std
::
shared_ptr
<
WsClient
::
InMessage
>
in_message
){
std
::
shared_ptr
<
WsClient
::
InMessage
>
in_message
){
std
::
cout
<<
"/rosapi/services: "
<<
in_message
->
string
()
<<
std
::
endl
;
//
std::cout << "/rosapi/services: " << in_message->string() << std::endl;
std
::
unique_lock
<
std
::
mutex
>
lk
(
this
->
mutex
);
std
::
unique_lock
<
std
::
mutex
>
lk
(
this
->
mutex
);
this
->
available_services
=
in_message
->
string
();
this
->
available_services
=
in_message
->
string
();
lk
.
unlock
();
lk
.
unlock
();
...
@@ -257,26 +279,24 @@ RosbridgeWsClient::RosbridgeWsClient(const std::__cxx11::string &server_port_pat
...
@@ -257,26 +279,24 @@ RosbridgeWsClient::RosbridgeWsClient(const std::__cxx11::string &server_port_pat
// Process tasks.
// Process tasks.
// ====================================================================================
// ====================================================================================
for
(
auto
task_it
=
task_list
.
begin
();
task_it
!=
task_list
.
end
();
){
for
(
auto
task_it
=
task_list
.
begin
();
task_it
!=
task_list
.
end
();
){
std
::
cout
<<
"processing task: "
<<
task_it
->
name
<<
std
::
endl
;
//
std::cout << "processing task: " << task_it->name << std::endl;
if
(
!
task_it
->
expired
()
){
if
(
!
task_it
->
expired
()
){
if
(
task_it
->
ready
()
){
if
(
task_it
->
ready
()
){
std
::
cout
<<
"executing task: "
<<
task_it
->
name
<<
std
::
endl
;
//
std::cout << "executing task: " << task_it->name << std::endl;
task_it
->
execute
();
task_it
->
execute
();
task_it
=
task_list
.
erase
(
task_it
);
task_it
=
task_list
.
erase
(
task_it
);
}
else
{
}
else
{
std
::
cout
<<
"noting to do for task: "
<<
task_it
->
name
<<
std
::
endl
;
//
std::cout << "noting to do for task: " << task_it->name << std::endl;
++
task_it
;
++
task_it
;
}
}
}
else
{
}
else
{
std
::
cout
<<
"task expired: "
<<
task_it
->
name
<<
std
::
endl
;
//
std::cout << "task expired: " << task_it->name << std::endl;
task_it
->
clear_up
();
task_it
->
clear_up
();
task_it
=
task_list
.
erase
(
task_it
);
task_it
=
task_list
.
erase
(
task_it
);
}
}
std
::
cout
<<
std
::
endl
;
//
std::cout << std::endl;
}
}
std
::
cout
<<
"task list size: "
<<
task_list
.
size
()
<<
std
::
endl
;
std
::
this_thread
::
sleep_for
(
std
::
chrono
::
milliseconds
(
10
));
std
::
this_thread
::
sleep_for
(
std
::
chrono
::
milliseconds
(
10
));
}
}
...
@@ -287,11 +307,20 @@ RosbridgeWsClient::RosbridgeWsClient(const std::__cxx11::string &server_port_pat
...
@@ -287,11 +307,20 @@ RosbridgeWsClient::RosbridgeWsClient(const std::__cxx11::string &server_port_pat
task_list
.
clear
();
task_list
.
clear
();
std
::
cout
<<
"periodic thread end"
<<
std
::
endl
;
std
::
cout
<<
"periodic thread end"
<<
std
::
endl
;
});
});
}
}
RosbridgeWsClient
::~
RosbridgeWsClient
()
void
RosbridgeWsClient
::
stop
()
{
{
if
(
stopped
->
load
()
)
return
;
stopped
->
store
(
true
);
stopped
->
store
(
true
);
periodic_thread
->
join
();
}
void
RosbridgeWsClient
::
reset
()
{
stop
();
unsubscribeAll
();
unsubscribeAll
();
unadvertiseAll
();
unadvertiseAll
();
unadvertiseAllServices
();
unadvertiseAllServices
();
...
@@ -299,11 +328,6 @@ RosbridgeWsClient::~RosbridgeWsClient()
...
@@ -299,11 +328,6 @@ RosbridgeWsClient::~RosbridgeWsClient()
{
{
removeClient
(
client
.
first
);
removeClient
(
client
.
first
);
}
}
periodic_thread
->
join
();
}
bool
RosbridgeWsClient
::
connected
(){
return
isConnected
->
load
();
}
}
void
RosbridgeWsClient
::
addClient
(
const
std
::
string
&
client_name
)
void
RosbridgeWsClient
::
addClient
(
const
std
::
string
&
client_name
)
...
@@ -378,9 +402,6 @@ void RosbridgeWsClient::removeClient(const std::string &client_name)
...
@@ -378,9 +402,6 @@ void RosbridgeWsClient::removeClient(const std::string &client_name)
if
(
it
!=
client_map
.
end
())
if
(
it
!=
client_map
.
end
())
{
{
client_map
.
erase
(
it
);
client_map
.
erase
(
it
);
#ifdef DEBUG
std
::
cout
<<
"removeClient: "
<<
client_name
<<
" reference count: "
<<
client
.
use_count
()
<<
std
::
endl
;
#endif
}
}
#ifdef DEBUG
#ifdef DEBUG
else
else
...
@@ -468,7 +489,7 @@ void RosbridgeWsClient::unadvertise(const std::string &topic, const std::string
...
@@ -468,7 +489,7 @@ void RosbridgeWsClient::unadvertise(const std::string &topic, const std::string
std
::
lock_guard
<
std
::
mutex
>
lk
(
mutex
);
std
::
lock_guard
<
std
::
mutex
>
lk
(
mutex
);
auto
it_ser_top
=
std
::
find_if
(
service_topic_list
.
begin
(),
auto
it_ser_top
=
std
::
find_if
(
service_topic_list
.
begin
(),
service_topic_list
.
end
(),
service_topic_list
.
end
(),
[
topic
](
const
EntryData
&
td
){
[
&
topic
](
const
EntryData
&
td
){
return
topic
==
std
::
get
<
integral
(
EntryEnum
::
ServiceTopicName
)
>
(
td
);
return
topic
==
std
::
get
<
integral
(
EntryEnum
::
ServiceTopicName
)
>
(
td
);
});
});
if
(
it_ser_top
==
service_topic_list
.
end
()){
if
(
it_ser_top
==
service_topic_list
.
end
()){
...
@@ -723,7 +744,7 @@ void RosbridgeWsClient::unadvertiseService(const std::string &service){
...
@@ -723,7 +744,7 @@ void RosbridgeWsClient::unadvertiseService(const std::string &service){
message
+=
",
\"
service
\"
:
\"
"
+
service
+
"
\"
"
;
message
+=
",
\"
service
\"
:
\"
"
+
service
+
"
\"
"
;
message
=
"{"
+
message
+
"}"
;
message
=
"{"
+
message
+
"}"
;
std
::
string
client_name
=
"
topic_unsubscrib
er"
+
service
;
std
::
string
client_name
=
"
service_unadvertis
er"
+
service
;
auto
client
=
std
::
make_shared
<
WsClient
>
(
server_port_path
);
auto
client
=
std
::
make_shared
<
WsClient
>
(
server_port_path
);
#ifdef DEBUG
#ifdef DEBUG
client
->
on_open
=
[
client_name
,
message
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
)
{
client
->
on_open
=
[
client_name
,
message
](
std
::
shared_ptr
<
WsClient
::
Connection
>
connection
)
{
...
@@ -851,30 +872,32 @@ bool RosbridgeWsClient::serviceAvailable(const std::string &service)
...
@@ -851,30 +872,32 @@ bool RosbridgeWsClient::serviceAvailable(const std::string &service)
void
RosbridgeWsClient
::
waitForService
(
const
std
::
string
&
service
)
void
RosbridgeWsClient
::
waitForService
(
const
std
::
string
&
service
)
{
{
waitForService
(
service
,
stopped
);
waitForService
(
service
,
[]{
return
false
;
// never stop
});
}
}
void
RosbridgeWsClient
::
waitForService
(
const
std
::
string
&
service
,
const
std
::
shared_ptr
<
std
::
atomic_bool
>
stop
)
void
RosbridgeWsClient
::
waitForService
(
const
std
::
string
&
service
,
const
std
::
function
<
bool
(
void
)
>
stop
)
{
{
#ifdef DEBUG
#ifdef DEBUG
auto
s
=
std
::
chrono
::
high_resolution_clock
::
now
();
auto
s
=
std
::
chrono
::
high_resolution_clock
::
now
();
long
counter
=
0
;
long
counter
=
0
;
#endif
#endif
const
auto
poll_interval
=
std
::
chrono
::
milliseconds
(
1000
);
const
auto
poll_interval
=
std
::
chrono
::
milliseconds
(
1000
);
auto
end
=
std
::
chrono
::
high_resolution_clock
::
now
()
+
poll_interval
;
auto
poll_time_point
=
std
::
chrono
::
high_resolution_clock
::
now
()
+
poll_interval
;
while
(
!
stop
->
load
()
)
while
(
!
stop
()
)
{
{
if
(
std
::
chrono
::
high_resolution_clock
::
now
()
>
end
){
if
(
std
::
chrono
::
high_resolution_clock
::
now
()
>
poll_time_point
){
#ifdef DEBUG
#ifdef DEBUG
++
counter
;
++
counter
;
#endif
#endif
if
(
serviceAvailable
(
service
)
){
if
(
serviceAvailable
(
service
)
){
break
;
break
;
}
else
{
}
else
{
end
=
std
::
chrono
::
high_resolution_clock
::
now
()
+
poll_interval
;
poll_time_point
=
std
::
chrono
::
high_resolution_clock
::
now
()
+
poll_interval
;
}
}
}
else
{
}
else
{
std
::
this_thread
::
sleep_for
(
std
::
chrono
::
milliseconds
(
1
0
));
std
::
this_thread
::
sleep_for
(
std
::
chrono
::
milliseconds
(
1
));
}
}
};
};
#ifdef DEBUG
#ifdef DEBUG
...
@@ -889,31 +912,32 @@ void RosbridgeWsClient::waitForService(const std::string &service, const std::sh
...
@@ -889,31 +912,32 @@ void RosbridgeWsClient::waitForService(const std::string &service, const std::sh
void
RosbridgeWsClient
::
waitForTopic
(
const
std
::
string
&
topic
)
void
RosbridgeWsClient
::
waitForTopic
(
const
std
::
string
&
topic
)
{
{
auto
stop
=
std
::
make_shared
<
std
::
atomic_bool
>
(
false
);
waitForTopic
(
topic
,
[]{
waitForTopic
(
topic
,
stop
);
return
false
;
// never stop
});
}
}
void
RosbridgeWsClient
::
waitForTopic
(
const
std
::
string
&
topic
,
const
std
::
shared_ptr
<
std
::
atomic_bool
>
stop
)
void
RosbridgeWsClient
::
waitForTopic
(
const
std
::
string
&
topic
,
const
std
::
function
<
bool
(
void
)
>
stop
)
{
{
#ifdef DEBUG
#ifdef DEBUG
auto
s
=
std
::
chrono
::
high_resolution_clock
::
now
();
auto
s
=
std
::
chrono
::
high_resolution_clock
::
now
();
long
counter
=
0
;
long
counter
=
0
;
#endif
#endif
const
auto
poll_interval
=
std
::
chrono
::
milliseconds
(
1000
);
const
auto
poll_interval
=
std
::
chrono
::
milliseconds
(
1000
);
auto
end
=
std
::
chrono
::
high_resolution_clock
::
now
()
+
poll_interval
;
auto
poll_time_point
=
std
::
chrono
::
high_resolution_clock
::
now
()
+
poll_interval
;
while
(
!
stop
->
load
()
)
while
(
!
stop
()
)
{
{
if
(
std
::
chrono
::
high_resolution_clock
::
now
()
>
end
){
if
(
std
::
chrono
::
high_resolution_clock
::
now
()
>
poll_time_point
){
#ifdef DEBUG
#ifdef DEBUG
++
counter
;
++
counter
;
#endif
#endif
if
(
topicAvailable
(
topic
)
){
if
(
topicAvailable
(
topic
)
){
break
;
break
;
}
else
{
}
else
{
end
=
std
::
chrono
::
high_resolution_clock
::
now
()
+
poll_interval
;
poll_time_point
=
std
::
chrono
::
high_resolution_clock
::
now
()
+
poll_interval
;
}
}
}
else
{
}
else
{
std
::
this_thread
::
sleep_for
(
std
::
chrono
::
milliseconds
(
1
0
));
std
::
this_thread
::
sleep_for
(
std
::
chrono
::
milliseconds
(
1
));
}
}
};
};
#ifdef DEBUG
#ifdef DEBUG
...
...
src/comm/ros_bridge/include/RosBridgeClient.h
View file @
fd49a04a
...
@@ -63,11 +63,16 @@ class RosbridgeWsClient
...
@@ -63,11 +63,16 @@ class RosbridgeWsClient
public:
public:
RosbridgeWsClient
(
const
std
::
string
&
server_port_path
);
RosbridgeWsClient
(
const
std
::
string
&
server_port_path
);
RosbridgeWsClient
(
const
std
::
string
&
server_port_path
,
bool
run
);
~
RosbridgeWsClient
();
~
RosbridgeWsClient
();
bool
connected
();
bool
connected
();
void
run
();
void
stop
();
void
reset
();
// Adds a client to the client_map.
// Adds a client to the client_map.
void
addClient
(
const
std
::
string
&
client_name
);
void
addClient
(
const
std
::
string
&
client_name
);
...
@@ -173,7 +178,7 @@ public:
...
@@ -173,7 +178,7 @@ public:
//! \param stop Flag to stop waiting.
//! \param stop Flag to stop waiting.
//! @note This function will block as long as the service is not available or \p stop is false.
//! @note This function will block as long as the service is not available or \p stop is false.
//!
//!
void
waitForService
(
const
std
::
string
&
service
,
const
std
::
shared_ptr
<
std
::
atomic_bool
>
stop
);
void
waitForService
(
const
std
::
string
&
service
,
const
std
::
function
<
bool
(
void
)
>
stop
);
//!
//!
//! \brief Waits until the topic with the name \p topic is available.
//! \brief Waits until the topic with the name \p topic is available.
...
@@ -187,5 +192,5 @@ public:
...
@@ -187,5 +192,5 @@ public:
//! \param stop Flag to stop waiting.
//! \param stop Flag to stop waiting.
//! @note This function will block as long as the topic is not available or \p stop is false.
//! @note This function will block as long as the topic is not available or \p stop is false.
//!
//!
void
waitForTopic
(
const
std
::
string
&
topic
,
const
std
::
shared_ptr
<
std
::
atomic_bool
>
stop
);
void
waitForTopic
(
const
std
::
string
&
topic
,
const
std
::
function
<
bool
(
void
)
>
stop
);
};
};
src/comm/ros_bridge/include/TopicPublisher.cpp
View file @
fd49a04a
...
@@ -60,7 +60,9 @@ void ROSBridge::ComPrivate::TopicPublisher::start()
...
@@ -60,7 +60,9 @@ void ROSBridge::ComPrivate::TopicPublisher::start()
this
->
_rbc
.
advertise
(
clientName
,
this
->
_rbc
.
advertise
(
clientName
,
tag
.
topic
(),
tag
.
topic
(),
tag
.
messageType
()
);
tag
.
messageType
()
);
this
->
_rbc
.
waitForTopic
(
tag
.
topic
(),
this
->
_stopped
);
// Wait until topic is advertised.
this
->
_rbc
.
waitForTopic
(
tag
.
topic
(),
[
this
]{
return
this
->
_stopped
->
load
();
});
// Wait until topic is advertised.
}
}
// Publish Json message.
// Publish Json message.
...
...
src/comm/ros_bridge/include/TopicSubscriber.cpp
View file @
fd49a04a
...
@@ -85,7 +85,9 @@ void ROSBridge::ComPrivate::TopicSubscriber::subscribe(
...
@@ -85,7 +85,9 @@ void ROSBridge::ComPrivate::TopicSubscriber::subscribe(
// Wait until topic is available.
// Wait until topic is available.
std
::
cout
<<
"TopicSubscriber: Starting wait thread, "
<<
clientName
<<
std
::
endl
;
std
::
cout
<<
"TopicSubscriber: Starting wait thread, "
<<
clientName
<<
std
::
endl
;
std
::
thread
t
([
this
,
clientName
,
topic
,
callbackWrapper
]{
std
::
thread
t
([
this
,
clientName
,
topic
,
callbackWrapper
]{
this
->
_rbc
.
waitForTopic
(
topic
,
this
->
_stopped
);
this
->
_rbc
.
waitForTopic
(
topic
,
[
this
]{
return
this
->
_stopped
->
load
();
});
if
(
!
this
->
_stopped
->
load
()
){
if
(
!
this
->
_stopped
->
load
()
){
this
->
_rbc
.
addClient
(
clientName
);
this
->
_rbc
.
addClient
(
clientName
);
this
->
_rbc
.
subscribe
(
clientName
,
topic
,
callbackWrapper
);
this
->
_rbc
.
subscribe
(
clientName
,
topic
,
callbackWrapper
);
...
...
src/comm/ros_bridge/src/ROSBridge.cpp
View file @
fd49a04a
...
@@ -3,12 +3,11 @@
...
@@ -3,12 +3,11 @@
ROSBridge
::
ROSBridge
::
ROSBridge
()
:
ROSBridge
::
ROSBridge
::
ROSBridge
()
:
_stopped
(
std
::
make_shared
<
std
::
atomic_bool
>
(
true
))
_stopped
(
std
::
make_shared
<
std
::
atomic_bool
>
(
true
))
,
_casePacker
(
&
_typeFactory
,
&
_jsonFactory
)
,
_casePacker
(
&
_typeFactory
,
&
_jsonFactory
)
,
_rbc
(
"localhost:9090"
)
,
_rbc
(
"localhost:9090"
,
false
/*run*/
)
,
_topicPublisher
(
_casePacker
,
_rbc
)
,
_topicPublisher
(
_casePacker
,
_rbc
)
,
_topicSubscriber
(
_casePacker
,
_rbc
)
,
_topicSubscriber
(
_casePacker
,
_rbc
)
,
_server
(
_casePacker
,
_rbc
)
,
_server
(
_casePacker
,
_rbc
)
{
{
}
}
void
ROSBridge
::
ROSBridge
::
publish
(
ROSBridge
::
ROSBridge
::
JsonDocUPtr
doc
)
void
ROSBridge
::
ROSBridge
::
publish
(
ROSBridge
::
ROSBridge
::
JsonDocUPtr
doc
)
...
@@ -35,19 +34,21 @@ const ROSBridge::CasePacker *ROSBridge::ROSBridge::casePacker() const
...
@@ -35,19 +34,21 @@ const ROSBridge::CasePacker *ROSBridge::ROSBridge::casePacker() const
void
ROSBridge
::
ROSBridge
::
start
()
void
ROSBridge
::
ROSBridge
::
start
()
{
{
_stopped
->
store
(
false
);
_rbc
.
run
();
_topicPublisher
.
start
();
_topicPublisher
.
start
();
_topicSubscriber
.
start
();
_topicSubscriber
.
start
();
_server
.
start
();
_server
.
start
();
_stopped
->
store
(
false
);
}
}
void
ROSBridge
::
ROSBridge
::
reset
()
void
ROSBridge
::
ROSBridge
::
reset
()
{
{
auto
start
=
std
::
chrono
::
high_resolution_clock
::
now
();
auto
start
=
std
::
chrono
::
high_resolution_clock
::
now
();
_stopped
->
store
(
true
);
_topicPublisher
.
reset
();
_topicPublisher
.
reset
();
_topicSubscriber
.
reset
();
_topicSubscriber
.
reset
();
_server
.
reset
();
_server
.
reset
();
_
stopped
->
store
(
true
);
_
rbc
.
reset
(
);
auto
end
=
std
::
chrono
::
high_resolution_clock
::
now
();
auto
end
=
std
::
chrono
::
high_resolution_clock
::
now
();
auto
delta
=
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
end
-
start
).
count
();
auto
delta
=
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
end
-
start
).
count
();
std
::
cout
<<
"ROSBridge reset duration: "
<<
delta
<<
" ms"
<<
std
::
endl
;
std
::
cout
<<
"ROSBridge reset duration: "
<<
delta
<<
" ms"
<<
std
::
endl
;
...
...
src/ui/preferences/GeneralSettings.qml
View file @
fd49a04a
...
@@ -950,18 +950,18 @@ QGCView {
...
@@ -950,18 +950,18 @@ QGCView {
columns
:
4
columns
:
4
QGCLabel
{
QGCLabel
{
text
:
qsTr
(
"
Low
Battery Threshold
"
)
text
:
qsTr
(
"
Battery Threshold
"
)
visible
:
_userBrandImageIndoor
.
visible
visible
:
_userBrandImageIndoor
.
visible
}
}
FactTextField
{
FactTextField
{
Layout.preferredWidth
:
_valueFieldWidth
Layout.preferredWidth
:
_valueFieldWidth
fact
:
QGroundControl
.
settingsManager
.
wimaSettings
.
lowBatteryThreshold
fact
:
QGroundControl
.
settingsManager
.
wimaSettings
.
lowBatteryThreshold
Layout.columnSpan
:
2
}
}
FactCheckBox
{
FactCheckBox
{
text
:
"
Enable
low Battery handling
"
text
:
"
Enable
Smart RTL
"
fact
:
QGroundControl
.
settingsManager
.
wimaSettings
.
enableLowBatteryHandling
fact
:
QGroundControl
.
settingsManager
.
wimaSettings
.
enableLowBatteryHandling
Layout.columnSpan
:
2
}
}
QGCLabel
{
QGCLabel
{
...
@@ -971,6 +971,25 @@ QGCView {
...
@@ -971,6 +971,25 @@ QGCView {
FactTextField
{
FactTextField
{
Layout.preferredWidth
:
_valueFieldWidth
Layout.preferredWidth
:
_valueFieldWidth
fact
:
QGroundControl
.
settingsManager
.
wimaSettings
.
minimalRemainingMissionTime
fact
:
QGroundControl
.
settingsManager
.
wimaSettings
.
minimalRemainingMissionTime
Layout.columnSpan
:
2
}
Item
{
// dummy
width
:
1
}
QGCLabel
{
text
:
qsTr
(
"
ROS Bridge Connection String
"
)
visible
:
_userBrandImageIndoor
.
visible
}
FactTextField
{
Layout.preferredWidth
:
_valueFieldWidth
fact
:
QGroundControl
.
settingsManager
.
wimaSettings
.
rosbridgeConnectionString
Layout.columnSpan
:
2
}
Item
{
// dummy
width
:
1
}
}
}
}
}
}
...
...
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