naoqi_joint_states.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 #
00004 # ROS node to read Nao's sensors and torso odometry through the Aldebaran API.
00005 # This code is currently compatible to NaoQI version 1.6
00006 #
00007 # Copyright 2009 Armin Hornung, University of Freiburg
00008 # http://www.ros.org/wiki/nao
00009 #
00010 # Redistribution and use in source and binary forms, with or without
00011 # modification, are permitted provided that the following conditions are met:
00012 #
00013 #    # Redistributions of source code must retain the above copyright
00014 #       notice, this list of conditions and the following disclaimer.
00015 #    # Redistributions in binary form must reproduce the above copyright
00016 #       notice, this list of conditions and the following disclaimer in the
00017 #       documentation and/or other materials provided with the distribution.
00018 #    # Neither the name of the University of Freiburg nor the names of its
00019 #       contributors may be used to endorse or promote products derived from
00020 #       this software without specific prior written permission.
00021 #
00022 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00023 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00024 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00025 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00026 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00027 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00028 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00029 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00030 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00031 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 # POSSIBILITY OF SUCH DAMAGE.
00033 #
00034 
00035 
00036 import rospy
00037 
00038 from sensor_msgs.msg import JointState
00039 from sensor_msgs.msg import Imu
00040 from nav_msgs.msg import Odometry
00041 
00042 from naoqi_driver.naoqi_node import NaoqiNode
00043 
00044 from tf import transformations
00045 import tf
00046 
00047 # NAOqi specific
00048 import motion
00049 
00050 class NaoqiJointStates(NaoqiNode):
00051     def __init__(self):
00052         NaoqiNode.__init__(self, 'naoqi_joint_states')
00053 
00054         self.connectNaoQi()
00055 
00056         # default sensor rate: 25 Hz (50 is max, stresses Nao's CPU)
00057         self.sensorRate = rospy.Rate(rospy.get_param('~sensor_rate', 25.0))
00058 
00059         self.dataNamesList = ["DCM/Time",
00060                                 "Device/SubDeviceList/InertialSensor/AngleX/Sensor/Value","Device/SubDeviceList/InertialSensor/AngleY/Sensor/Value",
00061                                 "Device/SubDeviceList/InertialSensor/AngleZ/Sensor/Value",
00062                                 "Device/SubDeviceList/InertialSensor/GyroscopeX/Sensor/Value", "Device/SubDeviceList/InertialSensor/GyroscopeY/Sensor/Value",
00063                                 "Device/SubDeviceList/InertialSensor/GyroscopeZ/Sensor/Value",
00064                                 "Device/SubDeviceList/InertialSensor/AccelerometerX/Sensor/Value", "Device/SubDeviceList/InertialSensor/AccelerometerY/Sensor/Value",
00065                                 "Device/SubDeviceList/InertialSensor/AccelerometerZ/Sensor/Value"]
00066 
00067 
00068         tf_prefix_param_name = rospy.search_param('tf_prefix')
00069         if tf_prefix_param_name:
00070             self.tf_prefix = rospy.get_param(tf_prefix_param_name)
00071         else:
00072             self.tf_prefix = ""
00073 
00074         self.base_frameID = rospy.get_param('~base_frame_id', "base_link")
00075         if not(self.base_frameID[0] == '/'):
00076             self.base_frameID = self.tf_prefix + '/' + self.base_frameID
00077 
00078         # use sensor values or commanded (open-loop) values for joint angles
00079         self.useJointSensors = rospy.get_param('~use_joint_sensors', True) # (set to False in simulation!)
00080         # init. messages:
00081         self.torsoOdom = Odometry()
00082         self.torsoOdom.header.frame_id = rospy.get_param('~odom_frame_id', "odom")
00083         if not(self.torsoOdom.header.frame_id[0] == '/'):
00084             self.torsoOdom.header.frame_id = self.tf_prefix + '/' + self.torsoOdom.header.frame_id
00085 
00086         self.torsoIMU = Imu()
00087         self.torsoIMU.header.frame_id = self.base_frameID
00088         self.jointState = JointState()
00089         self.jointState.name = self.motionProxy.getJointNames('Body')
00090         self.jointStiffness = JointState()
00091         self.jointStiffness.name = self.motionProxy.getJointNames('Body')
00092 
00093         # simluated model misses some joints, we need to fill:
00094         if (len(self.jointState.name) == 22):
00095             self.jointState.name.insert(6,"LWristYaw")
00096             self.jointState.name.insert(7,"LHand")
00097             self.jointState.name.append("RWristYaw")
00098             self.jointState.name.append("RHand")
00099 
00100         msg = "Nao joints found: "+ str(self.jointState.name)
00101         rospy.logdebug(msg)
00102 
00103         self.torsoOdomPub = rospy.Publisher("odom", Odometry, queue_size=1)
00104         self.torsoIMUPub = rospy.Publisher("imu", Imu, queue_size=1)
00105         self.jointStatePub = rospy.Publisher("joint_states", JointState, queue_size=1)
00106         self.jointStiffnessPub = rospy.Publisher("joint_stiffness", JointState, queue_size=1)
00107 
00108         self.tf_br = tf.TransformBroadcaster()
00109 
00110         rospy.loginfo("nao_joint_states initialized")
00111 
00112     # (re-) connect to NaoQI:
00113     def connectNaoQi(self):
00114         rospy.loginfo("Connecting to NaoQi at %s:%d", self.pip, self.pport)
00115         self.motionProxy = self.get_proxy("ALMotion")
00116         self.memProxy = self.get_proxy("ALMemory")
00117         if self.motionProxy is None or self.memProxy is None:
00118             exit(1)
00119 
00120     def run(self):
00121         """ Odometry thread code - collects and sends out odometry esimate. """
00122         while self.is_looping():
00123                 #
00124                 # Build odometry:
00125                 #
00126             timestamp = rospy.Time.now()
00127             try:
00128                 memData = self.memProxy.getListData(self.dataNamesList)
00129                  # odometry data:
00130                 odomData = self.motionProxy.getPosition('Torso', motion.SPACE_WORLD, True)
00131                 positionData = self.motionProxy.getAngles('Body', self.useJointSensors)
00132                 stiffnessData = self.motionProxy.getStiffnesses('Body')
00133             except RuntimeError, e:
00134                 print "Error accessing ALMemory, exiting...\n"
00135                 print e
00136                 rospy.signal_shutdown("No NaoQI available anymore")
00137 
00138             self.torsoOdom.header.stamp = timestamp
00139             if len(odomData)==2:
00140                 odomData = odomData[1]
00141             elif len(odomData)!=6:
00142                 print "Error getting odom data"
00143                 continue
00144 
00145             self.torsoOdom.pose.pose.position.x = odomData[0]
00146             self.torsoOdom.pose.pose.position.y = odomData[1]
00147             self.torsoOdom.pose.pose.position.z = odomData[2]
00148             q = transformations.quaternion_from_euler(odomData[3], odomData[4], odomData[5])
00149             self.torsoOdom.pose.pose.orientation.x = q[0]
00150             self.torsoOdom.pose.pose.orientation.y = q[1]
00151             self.torsoOdom.pose.pose.orientation.z = q[2]
00152             self.torsoOdom.pose.pose.orientation.w = q[3]
00153 
00154             t = self.torsoOdom.pose.pose.position
00155             q = self.torsoOdom.pose.pose.orientation
00156             self.tf_br.sendTransform((t.x, t.y, t.z), (q.x, q.y, q.z, q.w),
00157                                      timestamp, self.base_frameID, self.torsoOdom.header.frame_id)
00158 
00159             self.torsoOdomPub.publish(self.torsoOdom)
00160 
00161             # Replace 'None' values with 0
00162             # (=> consistent behavior in 1.8 / 1.10 with 1.6)
00163             # TODO: still required with 1.12 / 1.14?
00164             for i, m in enumerate(memData):
00165                 if m is None:
00166                     memData[i] = 0
00167 
00168             if len(memData) != len(self.dataNamesList):
00169                 print "memData length does not match expected length"
00170                 print memData
00171                 continue
00172 
00173 
00174             # IMU data:
00175             self.torsoIMU.header.stamp = timestamp
00176             q = transformations.quaternion_from_euler(memData[1], memData[2], memData[3])
00177             self.torsoIMU.orientation.x = q[0]
00178             self.torsoIMU.orientation.y = q[1]
00179             self.torsoIMU.orientation.z = q[2]
00180             self.torsoIMU.orientation.w = q[3]
00181 
00182             self.torsoIMU.angular_velocity.x = memData[4]
00183             self.torsoIMU.angular_velocity.y = memData[5]
00184             self.torsoIMU.angular_velocity.z = memData[6] # currently always 0
00185 
00186             self.torsoIMU.linear_acceleration.x = memData[7]
00187             self.torsoIMU.linear_acceleration.y = memData[8]
00188             self.torsoIMU.linear_acceleration.z = memData[9]
00189 
00190             # covariances unknown
00191             # cf http://www.ros.org/doc/api/sensor_msgs/html/msg/Imu.html
00192             self.torsoIMU.orientation_covariance[0] = -1
00193             self.torsoIMU.angular_velocity_covariance[0] = -1
00194             self.torsoIMU.linear_acceleration_covariance[0] = -1
00195 
00196             self.torsoIMUPub.publish(self.torsoIMU)
00197 
00198             # Send JointState:
00199             self.jointState.header.stamp = timestamp
00200             self.jointState.header.frame_id = self.base_frameID
00201             self.jointState.position = positionData
00202 
00203             # simulated model misses some joints, we need to fill:
00204             if (len(self.jointState.position) == 22):
00205                 self.jointState.position.insert(6, 0.0)
00206                 self.jointState.position.insert(7, 0.0)
00207                 self.jointState.position.append(0.0)
00208                 self.jointState.position.append(0.0)
00209 
00210             self.jointStatePub.publish(self.jointState)
00211 
00212             # send jointStiffness
00213             self.jointStiffness.header.stamp = timestamp
00214             self.jointStiffness.header.frame_id = self.base_frameID
00215             self.jointStiffness.effort = stiffnessData
00216 
00217             self.jointStiffnessPub.publish(self.jointStiffness)
00218 
00219             self.sensorRate.sleep()
00220 
00221 if __name__ == '__main__':
00222 
00223     joint_states = NaoqiJointStates()
00224     joint_states.start()
00225 
00226     rospy.spin()
00227     exit(0)


naoqi_driver
Author(s): Armin Hornung, Armin Hornung, Stefan Osswald, Daniel Maier, Miguel Sarabia, Severin Lemaignan
autogenerated on Fri Jul 3 2015 12:51:45