Skip to content
GitLab
Projects
Groups
Snippets
Help
Loading...
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
S
sot-hpp
Project overview
Project overview
Details
Activity
Releases
Repository
Repository
Files
Commits
Branches
Tags
Contributors
Graph
Compare
Issues
0
Issues
0
List
Boards
Labels
Service Desk
Milestones
Merge Requests
0
Merge Requests
0
CI / CD
CI / CD
Pipelines
Jobs
Schedules
Operations
Operations
Incidents
Environments
Packages & Registries
Packages & Registries
Container Registry
Analytics
Analytics
CI / CD
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
Stack Of Tasks
sot-hpp
Commits
f5072c56
Commit
f5072c56
authored
Nov 29, 2017
by
Joseph Mirabel
Committed by
Joseph Mirabel
Nov 29, 2017
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
Add command readQueue to RosQueuedSubscribe
parent
df878ec7
Changes
3
Hide whitespace changes
Inline
Side-by-side
Showing
3 changed files
with
72 additions
and
11 deletions
+72
-11
src/ros_subscribe.cpp
src/ros_subscribe.cpp
+43
-1
src/ros_subscribe.hh
src/ros_subscribe.hh
+13
-2
src/ros_subscribe.hxx
src/ros_subscribe.hxx
+16
-8
No files found.
src/ros_subscribe.cpp
View file @
f5072c56
...
...
@@ -147,6 +147,26 @@ namespace dynamicgraph
return
Value
((
unsigned
)
entity
.
queueSize
(
signal
));
}
ReadQueue
::
ReadQueue
(
RosQueuedSubscribe
&
entity
,
const
std
::
string
&
docstring
)
:
Command
(
entity
,
boost
::
assign
::
list_of
(
Value
::
BOOL
),
docstring
)
{}
Value
ReadQueue
::
doExecute
()
{
RosQueuedSubscribe
&
entity
=
static_cast
<
RosQueuedSubscribe
&>
(
owner
());
std
::
vector
<
Value
>
values
=
getParameterValues
();
bool
read
=
values
[
0
].
value
();
entity
.
readQueue
(
read
);
return
Value
();
}
}
// end of errorEstimator.
}
// end of namespace command.
...
...
@@ -158,7 +178,8 @@ namespace dynamicgraph
RosQueuedSubscribe
::
RosQueuedSubscribe
(
const
std
::
string
&
n
)
:
dynamicgraph
::
Entity
(
n
),
nh_
(
rosInit
(
true
)),
bindedSignal_
()
bindedSignal_
(),
readQueue_
(
false
)
{
std
::
string
docstring
=
"
\n
"
...
...
@@ -221,6 +242,16 @@ namespace dynamicgraph
addCommand
(
"queueSize"
,
new
command
::
rosSubscribe
::
QueueSize
(
*
this
,
docstring
));
docstring
=
"
\n
"
" Whether signals should read values from the queues.
\n
"
"
\n
"
" Input is:
\n
"
" - boolean.
\n
"
"
\n
"
;
addCommand
(
"readQueue"
,
new
command
::
rosSubscribe
::
ReadQueue
(
*
this
,
docstring
));
}
RosQueuedSubscribe
::~
RosQueuedSubscribe
()
...
...
@@ -287,6 +318,17 @@ namespace dynamicgraph
return
-
1
;
}
void
RosQueuedSubscribe
::
readQueue
(
bool
read
)
{
for
(
std
::
map
<
std
::
string
,
bindedSignal_t
>::
const_iterator
it
=
bindedSignal_
.
begin
();
it
!=
bindedSignal_
.
end
();
it
++
)
{
std
::
cout
<<
it
->
first
<<
" : "
<<
it
->
second
->
size
()
<<
'\n'
;
}
std
::
cout
<<
std
::
endl
;
// TODO ensure that the signal are synchronised
readQueue_
=
read
;
}
std
::
string
RosQueuedSubscribe
::
getDocString
()
const
{
return
docstring_
;
...
...
src/ros_subscribe.hh
View file @
f5072c56
...
...
@@ -43,12 +43,14 @@ namespace dynamicgraph
ROS_SUBSCRIBE_MAKE_COMMAND
(
Rm
);
ROS_SUBSCRIBE_MAKE_COMMAND
(
ClearQueue
);
ROS_SUBSCRIBE_MAKE_COMMAND
(
QueueSize
);
ROS_SUBSCRIBE_MAKE_COMMAND
(
ReadQueue
);
#undef ROS_SUBSCRIBE_MAKE_COMMAND
}
// end of namespace errorEstimator.
}
// end of namespace command.
class
RosQueuedSubscribe
;
namespace
internal
{
...
...
@@ -58,13 +60,14 @@ namespace dynamicgraph
struct
BindedSignalBase
{
typedef
boost
::
shared_ptr
<
ros
::
Subscriber
>
Subscriber_t
;
BindedSignalBase
()
{}
BindedSignalBase
(
RosQueuedSubscribe
*
e
)
:
entity
(
e
)
{}
virtual
~
BindedSignalBase
()
{}
virtual
void
clear
()
=
0
;
virtual
std
::
size_t
size
()
const
=
0
;
Subscriber_t
subscriber
;
RosQueuedSubscribe
*
entity
;
};
template
<
typename
T
>
...
...
@@ -73,7 +76,7 @@ namespace dynamicgraph
typedef
boost
::
shared_ptr
<
Signal_t
>
SignalPtr_t
;
typedef
std
::
queue
<
T
>
Queue_t
;
BindedSignal
(
)
:
BindedSignalBase
(
)
{}
BindedSignal
(
RosQueuedSubscribe
*
e
)
:
BindedSignalBase
(
e
),
init
(
false
)
{}
~
BindedSignal
()
{
std
::
cout
<<
signal
->
getName
()
<<
": Delete"
<<
std
::
endl
;
...
...
@@ -97,6 +100,7 @@ namespace dynamicgraph
Queue_t
queue
;
boost
::
mutex
qmutex
;
T
last
;
bool
init
;
template
<
typename
R
>
void
writer
(
const
R
&
data
);
T
&
reader
(
T
&
val
,
int
time
);
...
...
@@ -123,6 +127,7 @@ namespace dynamicgraph
std
::
string
list
();
void
clear
();
void
clearQueue
(
const
std
::
string
&
signal
);
void
readQueue
(
bool
read
);
std
::
size_t
queueSize
(
const
std
::
string
&
signal
)
const
;
template
<
typename
T
>
...
...
@@ -155,6 +160,12 @@ namespace dynamicgraph
static
const
std
::
string
docstring_
;
ros
::
NodeHandle
&
nh_
;
std
::
map
<
std
::
string
,
bindedSignal_t
>
bindedSignal_
;
bool
readQueue_
;
// Signal<bool, int> readQueue_;
template
<
typename
T
>
friend
class
internal
::
BindedSignal
;
};
}
// end of namespace dynamicgraph.
...
...
src/ros_subscribe.hxx
View file @
f5072c56
...
...
@@ -29,7 +29,7 @@ namespace dynamicgraph
typedef
typename
BindedSignal_t
::
Signal_t
Signal_t
;
// Initialize the bindedSignal object.
BindedSignal_t
*
bs
=
new
BindedSignal_t
();
BindedSignal_t
*
bs
=
new
BindedSignal_t
(
&
rosSubscribe
);
SotToRos
<
T
>::
setDefault
(
bs
->
last
);
// Initialize the signal.
...
...
@@ -61,6 +61,10 @@ namespace dynamicgraph
{
T
value
;
converter
(
value
,
data
);
if
(
!
init
)
{
last
=
value
;
init
=
true
;
}
qmutex
.
lock
();
queue
.
push
(
value
);
qmutex
.
unlock
();
...
...
@@ -69,15 +73,19 @@ namespace dynamicgraph
template
<
typename
T
>
T
&
BindedSignal
<
T
>::
reader
(
T
&
data
,
int
)
{
qmutex
.
lock
();
if
(
queue
.
empty
())
if
(
!
entity
->
readQueue_
)
{
data
=
last
;
else
{
data
=
queue
.
front
();
queue
.
pop
();
last
=
data
;
}
else
{
qmutex
.
lock
();
if
(
queue
.
empty
())
data
=
last
;
else
{
data
=
queue
.
front
();
queue
.
pop
();
last
=
data
;
}
qmutex
.
unlock
();
}
qmutex
.
unlock
();
return
data
;
}
}
// end of namespace internal.
...
...
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