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
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
ea39e714
Commit
ea39e714
authored
4 years ago
by
Johannes Mey
Browse files
Options
Downloads
Patches
Plain Diff
update state provider to use tf2 state
parent
cbe3cda5
Branches
Branches containing commit
Tags
Tags containing commit
No related merge requests found
Changes
2
Hide whitespace changes
Inline
Side-by-side
Showing
2 changed files
config/config.yaml
+10
-10
10 additions, 10 deletions
config/config.yaml
src/RobotStateProvider.cpp
+76
-82
76 additions, 82 deletions
src/RobotStateProvider.cpp
with
86 additions
and
92 deletions
config/config.yaml
+
10
−
10
View file @
ea39e714
...
...
@@ -15,18 +15,18 @@ panda_mqtt_connector:
-
"
-1
1"
parts
:
panda
:
Link0
:
"
panda
::panda
_link0"
Link1
:
"
panda
::panda
_link1"
Link2
:
"
panda
::panda
_link2"
Link3
:
"
panda
::panda
_link3"
Link4
:
"
panda
::panda
_link4"
Link5
:
"
panda
::panda
_link5"
Link6
:
"
panda
::panda
_link6"
EndEffector
:
"
panda::panda_link7
"
LeftFinger
:
"
panda
::panda
_leftfinger"
Link0
:
"
panda_link0"
Link1
:
"
panda_link1"
Link2
:
"
panda_link2"
Link3
:
"
panda_link3"
Link4
:
"
panda_link4"
Link5
:
"
panda_link5"
Link6
:
"
panda_link6"
RightFinger
:
"
panda_rightfinger
"
LeftFinger
:
"
panda_leftfinger"
end_effectors
:
panda
:
RightFinger
:
"
panda::panda_rightfinger
"
EndEffector
:
"
panda_hand
"
goal_poses
:
-
position
:
"
0.4
0.4
0.3"
orientation
:
"
1
1
0
0"
...
...
This diff is collapsed.
Click to expand it.
src/RobotStateProvider.cpp
+
76
−
82
View file @
ea39e714
/** @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>
#include
<tf2_ros/transform_listener.h>
#include
<geometry_msgs/TransformStamped.h>
#include
<geometry_msgs/PoseStamped.h>
#include
"robot_state.pb.h"
#include
"util/MqttUtil.h"
...
...
@@ -29,69 +31,18 @@ namespace robot_state_provider {
MqttUtil
*
mqttUtil
=
nullptr
;
std
::
vector
<
robot
::
RobotState
>
buildLinkStateMessages
(
const
gazebo_msgs
::
LinkStates
&
msg
)
{
std
::
vector
<
robot
::
RobotState
>
messages
{};
std
::
vector
<
std
::
string
>
names
{
msg
.
name
};
for
(
int
link_number
=
0
;
link_number
<
names
.
size
();
++
link_number
)
{
std
::
string
name
=
names
[
link_number
];
if
(
topics
.
find
(
name
)
!=
topics
.
end
())
{
robot
::
RobotState
pls_msg
;
// set the name to the MQTT topic provided by the topics map
pls_msg
.
set_name
(
topics
[
name
]);
auto
*
pos
=
new
robot
::
RobotState_Position
();
pos
->
set_x
(
msg
.
pose
.
at
(
link_number
).
position
.
x
);
pos
->
set_y
(
msg
.
pose
.
at
(
link_number
).
position
.
y
);
pos
->
set_z
(
msg
.
pose
.
at
(
link_number
).
position
.
z
);
pls_msg
.
set_allocated_position
(
pos
);
auto
*
orient
=
new
robot
::
RobotState_Orientation
();
orient
->
set_w
(
msg
.
pose
.
at
(
link_number
).
orientation
.
w
);
orient
->
set_x
(
msg
.
pose
.
at
(
link_number
).
orientation
.
x
);
orient
->
set_y
(
msg
.
pose
.
at
(
link_number
).
orientation
.
y
);
orient
->
set_z
(
msg
.
pose
.
at
(
link_number
).
orientation
.
z
);
pls_msg
.
set_allocated_orientation
(
orient
);
auto
*
tl
=
new
robot
::
RobotState_LinearTwist
();
tl
->
set_x
(
msg
.
twist
.
at
(
link_number
).
linear
.
x
);
tl
->
set_y
(
msg
.
twist
.
at
(
link_number
).
linear
.
y
);
tl
->
set_z
(
msg
.
twist
.
at
(
link_number
).
linear
.
z
);
pls_msg
.
set_allocated_linear_twist
(
tl
);
auto
*
ta
=
new
robot
::
RobotState_AngularTwist
();
ta
->
set_x
(
msg
.
twist
.
at
(
link_number
).
angular
.
x
);
ta
->
set_y
(
msg
.
twist
.
at
(
link_number
).
angular
.
y
);
ta
->
set_z
(
msg
.
twist
.
at
(
link_number
).
angular
.
z
);
pls_msg
.
set_allocated_angular_twist
(
ta
);
messages
.
push_back
(
pls_msg
);
}
else
{
ROS_DEBUG_STREAM
(
"Skipping the status of link "
<<
name
<<
"."
);
}
}
return
messages
;
}
void
sendMqttMessages
(
const
gazebo_msgs
::
LinkStates
&
msg
)
{
void
sendMqttMessages
(
const
robot
::
RobotState
&
pls_msg
)
{
if
(
mqttUtil
->
ensureConnection
())
{
try
{
for
(
const
robot
::
RobotState
&
pls_msg
:
buildLinkStateMessages
(
msg
))
{
std
::
string
mqtt_msg
;
if
(
!
pls_msg
.
SerializeToString
(
&
mqtt_msg
))
{
ROS_ERROR_STREAM
(
"Serialization of incoming MQTT message has failed."
);
}
auto
pubmsg
=
mqtt
::
make_message
(
pls_msg
.
name
(),
mqtt_msg
);
pubmsg
->
set_qos
(
QOS
);
mqttUtil
->
getClient
().
publish
(
pubmsg
);
std
::
string
mqtt_msg
;
if
(
!
pls_msg
.
SerializeToString
(
&
mqtt_msg
))
{
ROS_ERROR_STREAM
(
"Serialization of MQTT message has failed."
);
}
auto
pubmsg
=
mqtt
::
make_message
(
pls_msg
.
name
(),
mqtt_msg
);
pubmsg
->
set_qos
(
QOS
);
mqttUtil
->
getClient
().
publish
(
pubmsg
);
}
catch
(
const
mqtt
::
exception
&
exc
)
{
ROS_ERROR_STREAM
(
"Unable to publish robot state message. "
<<
exc
.
what
());
...
...
@@ -102,24 +53,6 @@ namespace robot_state_provider {
}
}
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"
);
int
message_redirect_rate
;
if
(
!
n
.
getParam
(
"data_publish_rate"
,
message_redirect_rate
))
{
message_redirect_rate
=
200
;
// fallback
}
if
(
current_redirect
==
0
)
{
sendMqttMessages
(
msg
);
}
current_redirect
=
(
current_redirect
+
1
)
%
message_redirect_rate
;
}
void
readTopicList
(
const
ros
::
NodeHandle
&
n
)
{
const
std
::
vector
<
std
::
string
>
elementTypes
{
"/parts/"
,
"/end_effectors/"
};
std
::
vector
<
std
::
string
>
parameterNames
;
...
...
@@ -164,7 +97,7 @@ int main(int argc, char **argv) {
std
::
string
server
;
if
(
!
n
.
getParam
(
"server"
,
server
))
{
ROS_ERROR
(
"Could not get string value for
panda_mqtt_connector
/server from param server"
);
ROS_ERROR
_STREAM
(
"Could not get string value for
"
<<
n
.
getNamespace
()
<<
"
/server from param server"
);
return
-
1
;
}
...
...
@@ -174,8 +107,69 @@ int main(int argc, char **argv) {
robot_state_provider
::
mqttUtil
->
connect
();
ros
::
Subscriber
sub
=
n
.
subscribe
(
"/gazebo/link_states"
,
10000
,
robot_state_provider
::
providerCallback
);
ros
::
spin
();
tf2_ros
::
Buffer
tfBuffer
;
tf2_ros
::
TransformListener
tfListener
(
tfBuffer
);
int
message_redirect_rate
;
if
(
!
n
.
getParam
(
"data_publish_rate"
,
message_redirect_rate
))
{
message_redirect_rate
=
200
;
// fallback
}
// map from ROS topic to ROS publisher
std
::
map
<
std
::
string
,
ros
::
Publisher
>
publishers
{};
// advertise the ROS messages
for
(
const
auto
&
topic
:
robot_state_provider
::
topics
)
{
publishers
[
topic
.
second
]
=
n
.
advertise
<
geometry_msgs
::
PoseStamped
>
(
topic
.
second
,
10
);
}
ros
::
Rate
rate
(
message_redirect_rate
);
while
(
n
.
ok
()){
for
(
const
auto
&
topic
:
robot_state_provider
::
topics
)
{
geometry_msgs
::
TransformStamped
transformStamped
;
try
{
transformStamped
=
tfBuffer
.
lookupTransform
(
"world"
,
topic
.
first
,
ros
::
Time
(
0
));
}
catch
(
tf2
::
TransformException
&
ex
)
{
ROS_WARN
(
"%s"
,
ex
.
what
());
ros
::
Duration
(
0.1
).
sleep
();
continue
;
}
// publish the ROS message
geometry_msgs
::
PoseStamped
ros_msg
;
ros_msg
.
header
.
stamp
=
transformStamped
.
header
.
stamp
;
ros_msg
.
pose
.
position
.
x
=
transformStamped
.
transform
.
translation
.
x
;
ros_msg
.
pose
.
position
.
y
=
transformStamped
.
transform
.
translation
.
y
;
ros_msg
.
pose
.
position
.
z
=
transformStamped
.
transform
.
translation
.
z
;
ros_msg
.
pose
.
orientation
.
w
=
transformStamped
.
transform
.
rotation
.
w
;
ros_msg
.
pose
.
orientation
.
x
=
transformStamped
.
transform
.
rotation
.
x
;
ros_msg
.
pose
.
orientation
.
y
=
transformStamped
.
transform
.
rotation
.
y
;
ros_msg
.
pose
.
orientation
.
z
=
transformStamped
.
transform
.
rotation
.
z
;
publishers
[
topic
.
second
].
publish
(
ros_msg
);
// publish the MQTT message
robot
::
RobotState
pls_msg
;
// set the name to the MQTT topic provided by the topics map
pls_msg
.
set_name
(
topic
.
second
);
auto
*
pos
=
new
robot
::
RobotState_Position
();
pos
->
set_x
(
transformStamped
.
transform
.
translation
.
x
);
pos
->
set_y
(
transformStamped
.
transform
.
translation
.
y
);
pos
->
set_z
(
transformStamped
.
transform
.
translation
.
z
);
pls_msg
.
set_allocated_position
(
pos
);
auto
*
orient
=
new
robot
::
RobotState_Orientation
();
orient
->
set_w
(
transformStamped
.
transform
.
rotation
.
w
);
orient
->
set_x
(
transformStamped
.
transform
.
rotation
.
x
);
orient
->
set_y
(
transformStamped
.
transform
.
rotation
.
y
);
orient
->
set_z
(
transformStamped
.
transform
.
rotation
.
z
);
pls_msg
.
set_allocated_orientation
(
orient
);
robot_state_provider
::
sendMqttMessages
(
pls_msg
);
}
rate
.
sleep
();
}
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