Main Page
Namespaces
Classes
Files
File List
File Members
scripts
ordered_hand_arm_joint_states_merger.py
Go to the documentation of this file.
1
#!/usr/bin/env python
2
import
rospy
3
from
sensor_msgs.msg
import
JointState
4
from
sr_utilities.srv
import
getJointState
5
import
thread
6
7
RATE = 100
8
9
10
class
MergeMessages
(object):
11
def
__init__
(self):
12
rospy.init_node(
'arm_and_hand_joint_state_merger'
, anonymous=
True
)
13
14
self.
msg_1_received
=
False
15
self.
msg_2_received
=
False
16
17
self.
subs_1
= rospy.Subscriber(
"/sh/joint_states"
, JointState, self.
callback1
)
18
self.
subs_2
= rospy.Subscriber(
"/sa/joint_states"
, JointState, self.
callback2
)
19
self.
serv
= rospy.Service(
'/getJointState'
, getJointState, self.
getJointStateCB
)
20
21
self.
pub
= rospy.Publisher(
"/joint_states"
, JointState)
22
23
self.
joint_state_msg
= JointState()
24
self.
joint_state_msg_1
= JointState()
25
self.
joint_state_msg_2
= JointState()
26
27
self.
mutex
= thread.allocate_lock()
28
29
r = rospy.Rate(RATE)
30
while
not
rospy.is_shutdown():
31
self.
publish
()
32
r.sleep()
33
34
def
callback1
(self, data):
35
self.
msg_1_received
=
True
36
self.
joint_state_msg_1
= data
37
38
def
callback2
(self, data):
39
self.
msg_2_received
=
True
40
self.
joint_state_msg_2
= data
41
42
def
publish
(self):
43
if
self.
msg_1_received
and
self.
msg_2_received
:
44
self.joint_state_msg.header.stamp = rospy.Time.now()
45
self.joint_state_msg.name = self.joint_state_msg_1.name + self.joint_state_msg_2.name
46
self.joint_state_msg.position = self.joint_state_msg_1.position + self.joint_state_msg_2.position
47
self.joint_state_msg.effort = self.joint_state_msg_1.effort + self.joint_state_msg_2.effort
48
self.joint_state_msg.velocity = self.joint_state_msg_1.velocity + self.joint_state_msg_2.velocity
49
50
self.pub.publish(self.
joint_state_msg
)
51
self.
msg_1_received
=
False
52
self.
msg_2_received
=
False
53
54
def
getJointStateCB
(self, req):
55
res = self.
joint_state_msg
56
return
res
57
58
59
if
__name__ ==
'__main__'
:
60
merger =
MergeMessages
()
sr_utilities.scripts.ordered_hand_arm_joint_states_merger.MergeMessages.__init__
def __init__(self)
Definition:
ordered_hand_arm_joint_states_merger.py:11
sr_utilities.scripts.ordered_hand_arm_joint_states_merger.MergeMessages.joint_state_msg
joint_state_msg
Definition:
ordered_hand_arm_joint_states_merger.py:23
sr_utilities.scripts.ordered_hand_arm_joint_states_merger.MergeMessages.callback2
def callback2(self, data)
Definition:
ordered_hand_arm_joint_states_merger.py:38
sr_utilities.scripts.ordered_hand_arm_joint_states_merger.MergeMessages
Definition:
ordered_hand_arm_joint_states_merger.py:10
sr_utilities.scripts.ordered_hand_arm_joint_states_merger.MergeMessages.callback1
def callback1(self, data)
Definition:
ordered_hand_arm_joint_states_merger.py:34
sr_utilities.scripts.ordered_hand_arm_joint_states_merger.MergeMessages.publish
def publish(self)
Definition:
ordered_hand_arm_joint_states_merger.py:42
sr_utilities.scripts.ordered_hand_arm_joint_states_merger.MergeMessages.getJointStateCB
def getJointStateCB(self, req)
Definition:
ordered_hand_arm_joint_states_merger.py:54
sr_utilities.scripts.ordered_hand_arm_joint_states_merger.MergeMessages.msg_1_received
msg_1_received
Definition:
ordered_hand_arm_joint_states_merger.py:14
sr_utilities.scripts.ordered_hand_arm_joint_states_merger.MergeMessages.msg_2_received
msg_2_received
Definition:
ordered_hand_arm_joint_states_merger.py:15
sr_utilities.scripts.ordered_hand_arm_joint_states_merger.MergeMessages.mutex
mutex
Definition:
ordered_hand_arm_joint_states_merger.py:27
sr_utilities.scripts.ordered_hand_arm_joint_states_merger.MergeMessages.joint_state_msg_1
joint_state_msg_1
Definition:
ordered_hand_arm_joint_states_merger.py:24
sr_utilities.scripts.ordered_hand_arm_joint_states_merger.MergeMessages.subs_1
subs_1
Definition:
ordered_hand_arm_joint_states_merger.py:17
sr_utilities.scripts.ordered_hand_arm_joint_states_merger.MergeMessages.pub
pub
Definition:
ordered_hand_arm_joint_states_merger.py:21
sr_utilities.scripts.ordered_hand_arm_joint_states_merger.MergeMessages.serv
serv
Definition:
ordered_hand_arm_joint_states_merger.py:19
sr_utilities.scripts.ordered_hand_arm_joint_states_merger.MergeMessages.subs_2
subs_2
Definition:
ordered_hand_arm_joint_states_merger.py:18
sr_utilities.scripts.ordered_hand_arm_joint_states_merger.MergeMessages.joint_state_msg_2
joint_state_msg_2
Definition:
ordered_hand_arm_joint_states_merger.py:25
sr_utilities
Author(s): Ugo Cupcic
autogenerated on Tue Oct 13 2020 03:55:49