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 
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 
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
58  self.rate.sleep()
59 
60 if __name__ == '__main__':
61  rospy.init_node('joint_states_to_float', anonymous=False)
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