mia_hand_ros_control
rel 1.0.0
joint_states_to_float.py
Go to the documentation of this file.
1
#!/usr/bin/env python3
2
3
import
rospy
4
from
std_msgs.msg
import
Float64
5
from
sensor_msgs.msg
import
JointState
6
7
class
JointStatesToFloat
:
8
def
__init__
(self):
9
self.
rate
= rospy.Rate(10)
10
self.
msg_received
=
False
11
self.
joint_state_msg
=
None
12
self.
previous_angles
= []
13
rospy.Subscriber(
'fake_joint_states'
, JointState, self.
JointStatesCB
)
14
rospy.loginfo(
'waiting for first joint state msg to be published'
)
15
self.
wait_for_first_msg
()
16
rospy.loginfo(
'joint state msg received, proceeding'
)
17
# init previous angle list
18
for
i
in
range(len(self.
joint_state_msg
.position)):
19
self.
previous_angles
.append(0.0)
20
# create one publisher per joint name
21
self.
pub_list
= []
22
for
name
in
self.
joint_state_msg
.name:
23
self.
pub_list
.append(rospy.Publisher(name +
'_position_controller/command'
, Float64, queue_size=1))
24
25
def
wait_for_first_msg
(self):
26
while
not
self.
msg_received
:
27
# wait until at least one joint state msg is received
28
self.
rate
.sleep()
29
30
def
JointStatesCB
(self, msg):
31
self.
joint_state_msg
= msg
32
self.
msg_received
=
True
33
34
def
publishJointsAsFloats
(self):
35
# extract desired angles from joint state msg
36
angles = []
37
for
angle
in
self.
joint_state_msg
.position:
38
angles.append(angle)
39
# compare previous angles to see if there was any change
40
changes = []
41
for
i, previous_angle
in
enumerate(self.
previous_angles
):
42
changes.append(previous_angle != angles[i])
43
# publish desired angles to float topics
44
for
i, pub
in
enumerate(self.
pub_list
):
45
if
changes[i]:
# compute only if angle changed
46
float_msg = Float64()
47
float_msg.data = angles[i]
48
pub.publish(float_msg)
49
# to keep track if the angle changed or not
50
self.
previous_angles
= angles
51
52
def
start
(self):
53
while
not
rospy.is_shutdown():
54
if
self.
msg_received
:
55
# lower flag
56
self.
msg_received
=
False
57
self.
publishJointsAsFloats
()
58
self.
rate
.sleep()
59
60
if
__name__ ==
'__main__'
:
61
rospy.init_node(
'joint_states_to_float'
, anonymous=
False
)
62
jstf =
JointStatesToFloat
()
63
jstf.start()
joint_states_to_float.JointStatesToFloat.joint_state_msg
joint_state_msg
Definition:
joint_states_to_float.py:11
joint_states_to_float.JointStatesToFloat.JointStatesCB
def JointStatesCB(self, msg)
Definition:
joint_states_to_float.py:30
joint_states_to_float.JointStatesToFloat.previous_angles
previous_angles
Definition:
joint_states_to_float.py:12
joint_states_to_float.JointStatesToFloat.__init__
def __init__(self)
Definition:
joint_states_to_float.py:8
joint_states_to_float.JointStatesToFloat.msg_received
msg_received
Definition:
joint_states_to_float.py:10
joint_states_to_float.JointStatesToFloat.pub_list
pub_list
Definition:
joint_states_to_float.py:21
joint_states_to_float.JointStatesToFloat.publishJointsAsFloats
def publishJointsAsFloats(self)
Definition:
joint_states_to_float.py:34
joint_states_to_float.JointStatesToFloat
Definition:
joint_states_to_float.py:7
joint_states_to_float.JointStatesToFloat.rate
rate
Definition:
joint_states_to_float.py:9
joint_states_to_float.JointStatesToFloat.start
def start(self)
Definition:
joint_states_to_float.py:52
joint_states_to_float.JointStatesToFloat.wait_for_first_msg
def wait_for_first_msg(self)
Definition:
joint_states_to_float.py:25
mia_hand_ros_control
src
joint_states_to_float.py
Generated by
1.8.17