Skip to content
GitLab
Explore
Sign in
Register
Primary navigation
Search or go to…
Project
P
panda_mqtt_connector
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Snippets
Build
Pipelines
Jobs
Pipeline schedules
Artifacts
Deploy
Releases
Package registry
Container registry
Model registry
Operate
Environments
Terraform modules
Monitor
Incidents
Analyze
Value stream analytics
Contributor analytics
CI/CD analytics
Repository analytics
Model experiments
Help
Help
Support
GitLab documentation
Compare GitLab plans
GitLab community forum
Contribute to GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
This is an archived project. Repository and other project resources are read-only.
Show more breadcrumbs
CeTI
ROS
ROS Packages
panda_mqtt_connector
Commits
902a67c6
Commit
902a67c6
authored
5 years ago
by
Johannes Mey
Browse files
Options
Downloads
Patches
Plain Diff
read topics to be published from config file
parent
75917759
Branches
Branches containing commit
No related tags found
No related merge requests found
Changes
2
Show whitespace changes
Inline
Side-by-side
Showing
2 changed files
config/config.yaml
+14
-12
14 additions, 12 deletions
config/config.yaml
src/RobotStateProvider.cpp
+192
-151
192 additions, 151 deletions
src/RobotStateProvider.cpp
with
206 additions
and
163 deletions
config/config.yaml
+
14
−
12
View file @
902a67c6
...
...
@@ -8,14 +8,16 @@ panda_mqtt_connector:
-
"
1
1"
-
"
-1
-1
1"
joints
:
-
panda/Joint0
:
"
panda::panda_link0"
-
panda/Joint1
:
"
panda::panda_link1"
-
panda/Joint2
:
"
panda::panda_link2"
-
panda/Joint3
:
"
panda::panda_link3"
-
panda/Joint4
:
"
panda::panda_link4"
-
panda/Joint5
:
"
panda::panda_link5"
-
panda/Joint6
:
"
panda::panda_link6"
-
panda/LeftFinger
:
"
panda::panda_leftfinger"
-
panda/RightFinger
:
"
panda::panda_rightfinger"
panda
:
Joint0
:
"
panda::panda_link0"
Joint1
:
"
panda::panda_link1"
Joint2
:
"
panda::panda_link2"
Joint3
:
"
panda::panda_link3"
Joint4
:
"
panda::panda_link4"
Joint5
:
"
panda::panda_link5"
Joint6
:
"
panda::panda_link6"
LeftFinger
:
"
panda::panda_leftfinger"
RightFinger
:
"
panda::panda_rightfinger"
end_effectors
:
-
panda/EndEffector
:
"
panda::panda_link7"
\ No newline at end of file
panda
:
EndEffector
:
"
panda::panda_link7"
This diff is collapsed.
Click to expand it.
src/RobotStateProvider.cpp
+
192
−
151
View file @
902a67c6
//
// Created by sebastian on 31.03.20.
//
/** @file RobotStateProvider.cpp
*
* @author Sebastian Ebert
* @author Johannes Mey
* @date 31.03.20
*/
#include
"ros/ros.h"
#include
<gazebo_msgs/LinkStates.h>
#include
<mqtt/client.h>
...
...
@@ -9,23 +13,18 @@
#include
"util/MqttUtil.h"
namespace
robot_state_provider
{
bool
export_to_log
=
false
;
/*
* mqtt-topics for all links
* ground, link0, link1, link2, link3, link4, link5, link6, link7 (end-effector), left finger, right finder
/**
* A map from the ROS element name to an MQTT topic
*/
std
::
set
<
std
::
string
>
topics
=
{
"ground_plane::link"
,
"panda::panda_link0"
,
"panda::panda_link1"
,
"panda::panda_link2"
,
"panda::panda_link3"
,
"panda::panda_link4"
,
"panda::panda_link5"
,
"panda::panda_link6"
,
"panda::panda_link7"
,
"panda::panda_leftfinger"
,
"panda::panda_rightfinger"
};
std
::
map
<
std
::
string
,
std
::
string
>
topics
{};
bool
isInitialized
=
false
;
const
int
QOS
=
1
;
int
current_redirect
=
0
;
constexpr
int
QOS
=
0
;
const
std
::
string
CLIENT_ID
{
"robot_state_provider_mqtt"
}
;
const
std
::
string
CLIENT_ID
=
"robot_state_provider_mqtt"
;
MqttUtil
*
mqttUtil
=
nullptr
;
...
...
@@ -125,10 +124,11 @@ std::vector <panda::PandaLinkState> buildLinkStateMessages(const gazebo_msgs::Li
panda
::
PandaLinkState
pls_msg
;
pls_msg
.
set_name
(
msg
.
name
.
at
(
link_number
));
// set the name to the MQTT topic provided by the topics map
pls_msg
.
set_name
(
topics
[
name
]);
if
(
export_position
)
{
panda
::
PandaLinkState_Position
*
pos
=
new
panda
::
PandaLinkState_Position
();
auto
*
pos
=
new
panda
::
PandaLinkState_Position
();
pos
->
set_positionx
(
msg
.
pose
.
at
(
link_number
).
position
.
x
);
pos
->
set_positiony
(
msg
.
pose
.
at
(
link_number
).
position
.
y
);
pos
->
set_positionz
(
msg
.
pose
.
at
(
link_number
).
position
.
z
);
...
...
@@ -136,7 +136,7 @@ std::vector <panda::PandaLinkState> buildLinkStateMessages(const gazebo_msgs::Li
}
if
(
export_orientation
)
{
panda
::
PandaLinkState_Orientation
*
orient
=
new
panda
::
PandaLinkState_Orientation
();
auto
*
orient
=
new
panda
::
PandaLinkState_Orientation
();
orient
->
set_orientationw
(
msg
.
pose
.
at
(
link_number
).
orientation
.
w
);
orient
->
set_orientationx
(
msg
.
pose
.
at
(
link_number
).
orientation
.
x
);
orient
->
set_orientationy
(
msg
.
pose
.
at
(
link_number
).
orientation
.
y
);
...
...
@@ -145,7 +145,7 @@ std::vector <panda::PandaLinkState> buildLinkStateMessages(const gazebo_msgs::Li
}
if
(
export_twist_linear
)
{
panda
::
PandaLinkState_TwistLinear
*
tl
=
new
panda
::
PandaLinkState_TwistLinear
();
auto
*
tl
=
new
panda
::
PandaLinkState_TwistLinear
();
tl
->
set_twistlinearx
(
msg
.
twist
.
at
(
link_number
).
linear
.
x
);
tl
->
set_twistlineary
(
msg
.
twist
.
at
(
link_number
).
linear
.
y
);
tl
->
set_twistlinearz
(
msg
.
twist
.
at
(
link_number
).
linear
.
z
);
...
...
@@ -153,7 +153,7 @@ std::vector <panda::PandaLinkState> buildLinkStateMessages(const gazebo_msgs::Li
}
if
(
export_twist_angular
)
{
panda
::
PandaLinkState_TwistAngular
*
ta
=
new
panda
::
PandaLinkState_TwistAngular
();
auto
*
ta
=
new
panda
::
PandaLinkState_TwistAngular
();
ta
->
set_twistangularx
(
msg
.
twist
.
at
(
link_number
).
angular
.
x
);
ta
->
set_twistangulary
(
msg
.
twist
.
at
(
link_number
).
angular
.
y
);
ta
->
set_twistangularz
(
msg
.
twist
.
at
(
link_number
).
angular
.
z
);
...
...
@@ -173,7 +173,7 @@ void sendMqttMessages(const gazebo_msgs::LinkStates &msg) {
if
(
mqttUtil
->
ensureConnection
())
{
try
{
for
(
panda
::
PandaLinkState
pls_msg
:
buildLinkStateMessages
(
msg
))
{
for
(
const
panda
::
PandaLinkState
&
pls_msg
:
buildLinkStateMessages
(
msg
))
{
std
::
string
mqtt_msg
;
if
(
!
pls_msg
.
SerializeToString
(
&
mqtt_msg
))
{
...
...
@@ -185,9 +185,8 @@ void sendMqttMessages(const gazebo_msgs::LinkStates &msg) {
mqttUtil
->
getClient
().
publish
(
pubmsg
);
}
}
// happens at lib paho sometimes when multiple instances of an mqtt-client are publishing at the same time
catch
(
const
mqtt
::
exception
&
exc
)
{
ROS_ERROR_STREAM
(
"
RSP:
"
<<
exc
.
what
());
ROS_ERROR_STREAM
(
"
Unable to publish robot state message.
"
<<
exc
.
what
());
return
;
}
}
else
{
...
...
@@ -197,6 +196,8 @@ void sendMqttMessages(const gazebo_msgs::LinkStates &msg) {
void
providerCallback
(
const
gazebo_msgs
::
LinkStates
&
msg
)
{
static
int
current_redirect
=
0
;
// TODO the rate should not be depending on the link state rate, but rather be a time or frequency!!!!
ros
::
NodeHandle
n
(
"panda_mqtt_connector"
);
...
...
@@ -214,6 +215,44 @@ void providerCallback(const gazebo_msgs::LinkStates &msg) {
current_redirect
=
(
current_redirect
+
1
)
%
message_redirect_rate
;
}
void
readTopicList
(
const
ros
::
NodeHandle
&
n
)
{
const
std
::
vector
<
std
::
string
>
elementTypes
{
"/joints/"
,
"/end_effectors/"
};
std
::
vector
<
std
::
string
>
parameterNames
;
n
.
getParamNames
(
parameterNames
);
std
::
set
<
std
::
string
>
groups
{};
for
(
const
auto
&
elementType
:
elementTypes
)
{
// get the groups
for
(
const
auto
&
param
:
parameterNames
)
{
std
::
string
prefix
{
n
.
getNamespace
()
+
elementType
};
if
(
param
.
rfind
(
prefix
,
0
)
==
0
)
{
std
::
string
rest
{
param
.
substr
(
prefix
.
size
())};
std
::
string
element
{
rest
.
substr
(
0
,
rest
.
rfind
(
'/'
))};
groups
.
insert
(
element
);
}
}
// get the elements in the group
for
(
const
auto
&
group
:
groups
)
{
std
::
map
<
std
::
string
,
std
::
string
>
element_topics
{};
std
::
string
key
{
"/panda_mqtt_connector"
+
elementType
+
group
};
if
(
!
n
.
getParam
(
key
,
element_topics
))
{
ROS_ERROR_STREAM
(
"Unable to retrieve value for "
<<
key
);
}
for
(
const
auto
&
pair
:
element_topics
)
{
std
::
string
mqttTopic
{
group
+
"/"
+
pair
.
first
};
topics
[
pair
.
second
]
=
mqttTopic
;
}
}
}
ROS_INFO_STREAM
(
"Publishing the state of "
<<
topics
.
size
()
<<
" ROS elements to MQTT."
);
for
(
const
auto
&
pair
:
topics
)
{
ROS_INFO_STREAM
(
"ROS element: '"
<<
pair
.
first
<<
"'
\t\t
MQTT topic: '"
<<
pair
.
second
<<
"'"
);
}
}
}
int
main
(
int
argc
,
char
**
argv
)
{
ros
::
init
(
argc
,
argv
,
"robot_state_provider"
);
ros
::
NodeHandle
n
(
"panda_mqtt_connector"
);
...
...
@@ -224,11 +263,13 @@ int main(int argc, char **argv) {
return
-
1
;
}
mqttUtil
=
new
MqttUtil
(
CLIENT_ID
,
server
);
robot_state_provider
::
readTopicList
(
n
);
robot_state_provider
::
mqttUtil
=
new
MqttUtil
(
robot_state_provider
::
CLIENT_ID
,
server
);
mqttUtil
->
connect
();
robot_state_provider
::
mqttUtil
->
connect
();
ros
::
Subscriber
sub
=
n
.
subscribe
(
"/gazebo/link_states"
,
10000
,
providerCallback
);
ros
::
Subscriber
sub
=
n
.
subscribe
(
"/gazebo/link_states"
,
10000
,
robot_state_provider
::
providerCallback
);
ros
::
spin
();
return
0
;
...
...
This diff is collapsed.
Click to expand it.
Preview
0%
Loading
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Save comment
Cancel
Please
register
or
sign in
to comment