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         self.useOdometry = rospy.get_param('~use_odometry', True)
00081         # init. messages:
00082         self.torsoOdom = Odometry()
00083         self.torsoOdom.header.frame_id = rospy.get_param('~odom_frame_id', "odom")
00084         if not(self.torsoOdom.header.frame_id[0] == '/'):
00085             self.torsoOdom.header.frame_id = self.tf_prefix + '/' + self.torsoOdom.header.frame_id
00086 
00087         self.torsoIMU = Imu()
00088         self.torsoIMU.header.frame_id = self.base_frameID
00089         self.jointState = JointState()
00090         self.jointState.name = self.motionProxy.getJointNames('Body')
00091         self.jointStiffness = JointState()
00092         self.jointStiffness.name = self.motionProxy.getJointNames('Body')
00093 
00094         # simluated model misses some joints, we need to fill:
00095         if (len(self.jointState.name) == 22):
00096             self.jointState.name.insert(6,"LWristYaw")
00097             self.jointState.name.insert(7,"LHand")
00098             self.jointState.name.append("RWristYaw")
00099             self.jointState.name.append("RHand")
00100 
00101         msg = "Nao joints found: "+ str(self.jointState.name)
00102         rospy.logdebug(msg)
00103 
00104         self.torsoOdomPub = rospy.Publisher("odom", Odometry, queue_size=1)
00105         self.torsoIMUPub = rospy.Publisher("imu", Imu, queue_size=1)
00106         self.jointStatePub = rospy.Publisher("joint_states", JointState, queue_size=1)
00107         self.jointStiffnessPub = rospy.Publisher("joint_stiffness", JointState, queue_size=1)
00108 
00109         self.tf_br = tf.TransformBroadcaster()
00110 
00111         rospy.loginfo("nao_joint_states initialized")
00112 
00113     # (re-) connect to NaoQI:
00114     def connectNaoQi(self):
00115         rospy.loginfo("Connecting to NaoQi at %s:%d", self.pip, self.pport)
00116         self.motionProxy = self.get_proxy("ALMotion")
00117         self.memProxy = self.get_proxy("ALMemory")
00118         if self.motionProxy is None or self.memProxy is None:
00119             exit(1)
00120 
00121     def run(self):
00122         """ Odometry thread code - collects and sends out odometry esimate. """
00123         while self.is_looping():
00124                 #
00125                 # Build odometry:
00126                 #
00127             timestamp = rospy.Time.now()
00128             try:
00129                 memData = self.memProxy.getListData(self.dataNamesList)
00130                  # odometry data:
00131                 odomData = self.motionProxy.getPosition('Torso', motion.SPACE_WORLD, True)
00132                 positionData = self.motionProxy.getAngles('Body', self.useJointSensors)
00133                 if self.useJointSensors: # get reference data when available
00134                     referenceData = self.motionProxy.getAngles('Body', False)
00135                 else:
00136                     referenceData = positionData
00137                 stiffnessData = self.motionProxy.getStiffnesses('Body')
00138             except RuntimeError, e:
00139                 print "Error accessing ALMemory, exiting...\n"
00140                 print e
00141                 rospy.signal_shutdown("No NaoQI available anymore")
00142 
00143             self.torsoOdom.header.stamp = timestamp
00144             if len(odomData)==2:
00145                 odomData = odomData[1]
00146             elif len(odomData)!=6:
00147                 print "Error getting odom data"
00148                 continue
00149 
00150             self.torsoOdom.pose.pose.position.x = odomData[0]
00151             self.torsoOdom.pose.pose.position.y = odomData[1]
00152             self.torsoOdom.pose.pose.position.z = odomData[2]
00153             q = transformations.quaternion_from_euler(odomData[3], odomData[4], odomData[5])
00154             self.torsoOdom.pose.pose.orientation.x = q[0]
00155             self.torsoOdom.pose.pose.orientation.y = q[1]
00156             self.torsoOdom.pose.pose.orientation.z = q[2]
00157             self.torsoOdom.pose.pose.orientation.w = q[3]
00158 
00159             if self.useOdometry:
00160                 t = self.torsoOdom.pose.pose.position
00161                 q = self.torsoOdom.pose.pose.orientation
00162                 self.tf_br.sendTransform((t.x, t.y, t.z), (q.x, q.y, q.z, q.w),
00163                                      timestamp, self.base_frameID, self.torsoOdom.header.frame_id)
00164 
00165             self.torsoOdomPub.publish(self.torsoOdom)
00166 
00167             # Replace 'None' values with 0
00168             # (=> consistent behavior in 1.8 / 1.10 with 1.6)
00169             # TODO: still required with 1.12 / 1.14?
00170             for i, m in enumerate(memData):
00171                 if m is None:
00172                     memData[i] = 0
00173 
00174             if len(memData) != len(self.dataNamesList):
00175                 print "memData length does not match expected length"
00176                 print memData
00177                 continue
00178 
00179 
00180             # IMU data:
00181             self.torsoIMU.header.stamp = timestamp
00182             q = transformations.quaternion_from_euler(memData[1], memData[2], memData[3])
00183             self.torsoIMU.orientation.x = q[0]
00184             self.torsoIMU.orientation.y = q[1]
00185             self.torsoIMU.orientation.z = q[2]
00186             self.torsoIMU.orientation.w = q[3]
00187 
00188             self.torsoIMU.angular_velocity.x = memData[4]
00189             self.torsoIMU.angular_velocity.y = memData[5]
00190             self.torsoIMU.angular_velocity.z = memData[6] # currently always 0
00191 
00192             self.torsoIMU.linear_acceleration.x = memData[7]
00193             self.torsoIMU.linear_acceleration.y = memData[8]
00194             self.torsoIMU.linear_acceleration.z = memData[9]
00195 
00196             # covariances unknown
00197             # cf http://www.ros.org/doc/api/sensor_msgs/html/msg/Imu.html
00198             self.torsoIMU.orientation_covariance[0] = -1
00199             self.torsoIMU.angular_velocity_covariance[0] = -1
00200             self.torsoIMU.linear_acceleration_covariance[0] = -1
00201 
00202             self.torsoIMUPub.publish(self.torsoIMU)
00203 
00204             # Send JointState:
00205             self.jointState.header.stamp = timestamp
00206             self.jointState.header.frame_id = self.base_frameID
00207             self.jointState.position = positionData
00208             self.jointState.effort = map(lambda x, y: x - y, positionData, referenceData)
00209 
00210             # simulated model misses some joints, we need to fill:
00211             if (len(self.jointState.position) == 22):
00212                 self.jointState.position.insert(6, 0.0)
00213                 self.jointState.position.insert(7, 0.0)
00214                 self.jointState.position.append(0.0)
00215                 self.jointState.position.append(0.0)
00216 
00217             self.jointStatePub.publish(self.jointState)
00218 
00219             # send jointStiffness
00220             self.jointStiffness.header.stamp = timestamp
00221             self.jointStiffness.header.frame_id = self.base_frameID
00222             self.jointStiffness.effort = stiffnessData
00223 
00224             self.jointStiffnessPub.publish(self.jointStiffness)
00225 
00226             self.sensorRate.sleep()
00227 
00228 if __name__ == '__main__':
00229 
00230     joint_states = NaoqiJointStates()
00231     joint_states.start()
00232 
00233     rospy.spin()
00234     exit(0)


naoqi_driver_py
Author(s): Armin Hornung, Armin Hornung, Stefan Osswald, Daniel Maier, Miguel Sarabia, Severin Lemaignan
autogenerated on Sat Jun 8 2019 20:30:13