naoqi_joint_states.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 #
4 # ROS node to read Nao's sensors and torso odometry through the Aldebaran API.
5 # This code is currently compatible to NaoQI version 1.6
6 #
7 # Copyright 2009 Armin Hornung, University of Freiburg
8 # http://www.ros.org/wiki/nao
9 #
10 # Redistribution and use in source and binary forms, with or without
11 # modification, are permitted provided that the following conditions are met:
12 #
13 # # Redistributions of source code must retain the above copyright
14 # notice, this list of conditions and the following disclaimer.
15 # # Redistributions in binary form must reproduce the above copyright
16 # notice, this list of conditions and the following disclaimer in the
17 # documentation and/or other materials provided with the distribution.
18 # # Neither the name of the University of Freiburg nor the names of its
19 # contributors may be used to endorse or promote products derived from
20 # this software without specific prior written permission.
21 #
22 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
23 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
24 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
25 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
26 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
27 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
28 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
29 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
30 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
31 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 # POSSIBILITY OF SUCH DAMAGE.
33 #
34 
35 
36 import rospy
37 
38 from sensor_msgs.msg import JointState
39 from sensor_msgs.msg import Imu
40 from nav_msgs.msg import Odometry
41 
42 from naoqi_driver.naoqi_node import NaoqiNode
43 
44 from tf import transformations
45 import tf
46 
47 # NAOqi specific
48 import motion
49 
51  def __init__(self):
52  NaoqiNode.__init__(self, 'naoqi_joint_states')
53 
54  self.connectNaoQi()
55 
56  # default sensor rate: 25 Hz (50 is max, stresses Nao's CPU)
57  self.sensorRate = rospy.Rate(rospy.get_param('~sensor_rate', 25.0))
58 
59  self.dataNamesList = ["DCM/Time",
60  "Device/SubDeviceList/InertialSensor/AngleX/Sensor/Value","Device/SubDeviceList/InertialSensor/AngleY/Sensor/Value",
61  "Device/SubDeviceList/InertialSensor/AngleZ/Sensor/Value",
62  "Device/SubDeviceList/InertialSensor/GyroscopeX/Sensor/Value", "Device/SubDeviceList/InertialSensor/GyroscopeY/Sensor/Value",
63  "Device/SubDeviceList/InertialSensor/GyroscopeZ/Sensor/Value",
64  "Device/SubDeviceList/InertialSensor/AccelerometerX/Sensor/Value", "Device/SubDeviceList/InertialSensor/AccelerometerY/Sensor/Value",
65  "Device/SubDeviceList/InertialSensor/AccelerometerZ/Sensor/Value"]
66 
67 
68  tf_prefix_param_name = rospy.search_param('tf_prefix')
69  if tf_prefix_param_name:
70  self.tf_prefix = rospy.get_param(tf_prefix_param_name)
71  else:
72  self.tf_prefix = ""
73 
74  self.base_frameID = rospy.get_param('~base_frame_id', "base_link")
75  if not(self.base_frameID[0] == '/'):
76  self.base_frameID = self.tf_prefix + '/' + self.base_frameID
77 
78  # use sensor values or commanded (open-loop) values for joint angles
79  self.useJointSensors = rospy.get_param('~use_joint_sensors', True) # (set to False in simulation!)
80  self.useOdometry = rospy.get_param('~use_odometry', True)
81  # init. messages:
82  self.torsoOdom = Odometry()
83  self.torsoOdom.header.frame_id = rospy.get_param('~odom_frame_id', "odom")
84  if not(self.torsoOdom.header.frame_id[0] == '/'):
85  self.torsoOdom.header.frame_id = self.tf_prefix + '/' + self.torsoOdom.header.frame_id
86 
87  self.torsoIMU = Imu()
88  self.torsoIMU.header.frame_id = self.base_frameID
89  self.jointState = JointState()
90  self.jointState.name = self.motionProxy.getJointNames('Body')
91  self.jointStiffness = JointState()
92  self.jointStiffness.name = self.motionProxy.getJointNames('Body')
93 
94  # simluated model misses some joints, we need to fill:
95  if (len(self.jointState.name) == 22):
96  self.jointState.name.insert(6,"LWristYaw")
97  self.jointState.name.insert(7,"LHand")
98  self.jointState.name.append("RWristYaw")
99  self.jointState.name.append("RHand")
100 
101  msg = "Nao joints found: "+ str(self.jointState.name)
102  rospy.logdebug(msg)
103 
104  self.torsoOdomPub = rospy.Publisher("odom", Odometry, queue_size=1)
105  self.torsoIMUPub = rospy.Publisher("imu", Imu, queue_size=1)
106  self.jointStatePub = rospy.Publisher("joint_states", JointState, queue_size=1)
107  self.jointStiffnessPub = rospy.Publisher("joint_stiffness", JointState, queue_size=1)
108 
109  self.tf_br = tf.TransformBroadcaster()
110 
111  rospy.loginfo("nao_joint_states initialized")
112 
113  # (re-) connect to NaoQI:
114  def connectNaoQi(self):
115  rospy.loginfo("Connecting to NaoQi at %s:%d", self.pip, self.pport)
116  self.motionProxy = self.get_proxy("ALMotion")
117  self.memProxy = self.get_proxy("ALMemory")
118  if self.motionProxy is None or self.memProxy is None:
119  exit(1)
120 
121  def run(self):
122  """ Odometry thread code - collects and sends out odometry esimate. """
123  while self.is_looping():
124  #
125  # Build odometry:
126  #
127  timestamp = rospy.Time.now()
128  try:
129  memData = self.memProxy.getListData(self.dataNamesList)
130  # odometry data:
131  odomData = self.motionProxy.getPosition('Torso', motion.SPACE_WORLD, True)
132  positionData = self.motionProxy.getAngles('Body', self.useJointSensors)
133  if self.useJointSensors: # get reference data when available
134  referenceData = self.motionProxy.getAngles('Body', False)
135  else:
136  referenceData = positionData
137  stiffnessData = self.motionProxy.getStiffnesses('Body')
138  except RuntimeError, e:
139  print "Error accessing ALMemory, exiting...\n"
140  print e
141  rospy.signal_shutdown("No NaoQI available anymore")
142 
143  self.torsoOdom.header.stamp = timestamp
144  if len(odomData)==2:
145  odomData = odomData[1]
146  elif len(odomData)!=6:
147  print "Error getting odom data"
148  continue
149 
150  self.torsoOdom.pose.pose.position.x = odomData[0]
151  self.torsoOdom.pose.pose.position.y = odomData[1]
152  self.torsoOdom.pose.pose.position.z = odomData[2]
153  q = transformations.quaternion_from_euler(odomData[3], odomData[4], odomData[5])
154  self.torsoOdom.pose.pose.orientation.x = q[0]
155  self.torsoOdom.pose.pose.orientation.y = q[1]
156  self.torsoOdom.pose.pose.orientation.z = q[2]
157  self.torsoOdom.pose.pose.orientation.w = q[3]
158 
159  if self.useOdometry:
160  t = self.torsoOdom.pose.pose.position
161  q = self.torsoOdom.pose.pose.orientation
162  self.tf_br.sendTransform((t.x, t.y, t.z), (q.x, q.y, q.z, q.w),
163  timestamp, self.base_frameID, self.torsoOdom.header.frame_id)
164 
165  self.torsoOdomPub.publish(self.torsoOdom)
166 
167  # Replace 'None' values with 0
168  # (=> consistent behavior in 1.8 / 1.10 with 1.6)
169  # TODO: still required with 1.12 / 1.14?
170  for i, m in enumerate(memData):
171  if m is None:
172  memData[i] = 0
173 
174  if len(memData) != len(self.dataNamesList):
175  print "memData length does not match expected length"
176  print memData
177  continue
178 
179 
180  # IMU data:
181  self.torsoIMU.header.stamp = timestamp
182  q = transformations.quaternion_from_euler(memData[1], memData[2], memData[3])
183  self.torsoIMU.orientation.x = q[0]
184  self.torsoIMU.orientation.y = q[1]
185  self.torsoIMU.orientation.z = q[2]
186  self.torsoIMU.orientation.w = q[3]
187 
188  self.torsoIMU.angular_velocity.x = memData[4]
189  self.torsoIMU.angular_velocity.y = memData[5]
190  self.torsoIMU.angular_velocity.z = memData[6] # currently always 0
191 
192  self.torsoIMU.linear_acceleration.x = memData[7]
193  self.torsoIMU.linear_acceleration.y = memData[8]
194  self.torsoIMU.linear_acceleration.z = memData[9]
195 
196  # covariances unknown
197  # cf http://www.ros.org/doc/api/sensor_msgs/html/msg/Imu.html
198  self.torsoIMU.orientation_covariance[0] = -1
199  self.torsoIMU.angular_velocity_covariance[0] = -1
200  self.torsoIMU.linear_acceleration_covariance[0] = -1
201 
202  self.torsoIMUPub.publish(self.torsoIMU)
203 
204  # Send JointState:
205  self.jointState.header.stamp = timestamp
206  self.jointState.header.frame_id = self.base_frameID
207  self.jointState.position = positionData
208  self.jointState.effort = map(lambda x, y: x - y, positionData, referenceData)
209 
210  # simulated model misses some joints, we need to fill:
211  if (len(self.jointState.position) == 22):
212  self.jointState.position.insert(6, 0.0)
213  self.jointState.position.insert(7, 0.0)
214  self.jointState.position.append(0.0)
215  self.jointState.position.append(0.0)
216 
217  self.jointStatePub.publish(self.jointState)
218 
219  # send jointStiffness
220  self.jointStiffness.header.stamp = timestamp
221  self.jointStiffness.header.frame_id = self.base_frameID
222  self.jointStiffness.effort = stiffnessData
223 
224  self.jointStiffnessPub.publish(self.jointStiffness)
225 
226  self.sensorRate.sleep()
227 
228 if __name__ == '__main__':
229 
230  joint_states = NaoqiJointStates()
231  joint_states.start()
232 
233  rospy.spin()
234  exit(0)
def get_proxy(self, name, warn=True)
Definition: naoqi_node.py:128


naoqi_driver_py
Author(s): Armin Hornung, Armin Hornung, Stefan Osswald, Daniel Maier, Miguel Sarabia, Severin Lemaignan
autogenerated on Thu Jul 16 2020 03:18:30