scripts
ordered_target_joint_states_merger.py
Go to the documentation of this file.
1
#!/usr/bin/env python
2
3
# Copyright 2011 Shadow Robot Company Ltd.
4
#
5
# This program is free software: you can redistribute it and/or modify it
6
# under the terms of the GNU General Public License as published by the Free
7
# Software Foundation version 2 of the License.
8
#
9
# This program is distributed in the hope that it will be useful, but WITHOUT
10
# ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
11
# FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
12
# more details.
13
#
14
# You should have received a copy of the GNU General Public License along
15
# with this program. If not, see <http://www.gnu.org/licenses/>.
16
17
import
rospy
18
from
sensor_msgs.msg
import
JointState
19
import
thread
20
21
22
class
MergeMessages
:
23
def
__init__
(self):
24
rospy.init_node(
'target_joint_state_merger'
, anonymous=
True
)
25
self.
subs_1
= rospy.Subscriber(
"/srh/target/joint_states"
, JointState, self.
callback1
)
26
self.
subs_2
= rospy.Subscriber(
"/sr_arm/target/joint_states"
, JointState, self.
callback2
)
27
28
self.
pub
= rospy.Publisher(
"/targets/joint_states"
, JointState)
29
30
self.
msg_1_received
=
False
31
self.
msg_2_received
=
False
32
33
self.
joint_state_msg
= JointState()
34
35
self.
mutex
= thread.allocate_lock()
36
37
rospy.spin()
38
39
def
callback1
(self, data):
40
self.
mutex
.acquire()
41
if
self.
msg_1_received
:
42
self.
mutex
.release()
43
return
44
45
self.
msg_1_received
=
True
46
47
self.
joint_state_msg
.header.stamp = rospy.Time.now()
48
49
tmp = data.name
50
self.
joint_state_msg
.name = tmp
51
52
tmp = data.position
53
self.
joint_state_msg
.position = tmp
54
55
tmp = data.velocity
56
self.
joint_state_msg
.velocity = tmp
57
58
tmp = data.effort
59
self.
joint_state_msg
.effort = tmp
60
61
self.
mutex
.release()
62
63
def
callback2
(self, data):
64
self.
mutex
.acquire()
65
66
self.
msg_2_received
=
True
67
68
self.
joint_state_msg
.header.stamp = rospy.Time.now()
69
70
tmp = self.
joint_state_msg
.name
71
tmp += data.name
72
self.
joint_state_msg
.name = tmp
73
74
tmp = self.
joint_state_msg
.position
75
tmp += data.position
76
self.
joint_state_msg
.position = tmp
77
78
tmp = self.
joint_state_msg
.velocity
79
tmp += data.velocity
80
self.
joint_state_msg
.velocity = tmp
81
82
tmp = self.
joint_state_msg
.effort
83
tmp += data.effort
84
self.
joint_state_msg
.effort = tmp
85
86
if
self.
msg_1_received
:
87
self.
pub
.publish(self.
joint_state_msg
)
88
self.
msg_1_received
=
False
89
self.
msg_2_received
=
False
90
self.
joint_state_msg
= JointState()
91
92
self.
mutex
.release()
93
94
95
if
__name__ ==
'__main__'
:
96
merger =
MergeMessages
()
sr_utilities.scripts.ordered_target_joint_states_merger.MergeMessages.joint_state_msg
joint_state_msg
Definition:
ordered_target_joint_states_merger.py:33
sr_utilities.scripts.ordered_target_joint_states_merger.MergeMessages
Definition:
ordered_target_joint_states_merger.py:22
sr_utilities.scripts.ordered_target_joint_states_merger.MergeMessages.msg_1_received
msg_1_received
Definition:
ordered_target_joint_states_merger.py:30
sr_utilities.scripts.ordered_target_joint_states_merger.MergeMessages.__init__
def __init__(self)
Definition:
ordered_target_joint_states_merger.py:23
sr_utilities.scripts.ordered_target_joint_states_merger.MergeMessages.subs_1
subs_1
Definition:
ordered_target_joint_states_merger.py:25
sr_utilities.scripts.ordered_target_joint_states_merger.MergeMessages.callback1
def callback1(self, data)
Definition:
ordered_target_joint_states_merger.py:39
sr_utilities.scripts.ordered_target_joint_states_merger.MergeMessages.msg_2_received
msg_2_received
Definition:
ordered_target_joint_states_merger.py:31
sr_utilities.scripts.ordered_target_joint_states_merger.MergeMessages.mutex
mutex
Definition:
ordered_target_joint_states_merger.py:35
sr_utilities.scripts.ordered_target_joint_states_merger.MergeMessages.subs_2
subs_2
Definition:
ordered_target_joint_states_merger.py:26
sr_utilities.scripts.ordered_target_joint_states_merger.MergeMessages.pub
pub
Definition:
ordered_target_joint_states_merger.py:28
sr_utilities.scripts.ordered_target_joint_states_merger.MergeMessages.callback2
def callback2(self, data)
Definition:
ordered_target_joint_states_merger.py:63
sr_utilities
Author(s): Ugo Cupcic
autogenerated on Mon Feb 28 2022 23:52:19