nao_sensors.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 nao_driver import NaoNode
00043 
00044 from tf import transformations
00045 import tf
00046 
00047 # NAOqi specific
00048 import motion
00049 
00050 class NaoSensors(NaoNode):
00051     def __init__(self):
00052         NaoNode.__init__(self, 'nao_sensors')
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         # To stop odometry tf being broadcast
00075         self.broadcast_odometry = rospy.get_param('~broadcast_odometry', True)
00076 
00077         self.base_frameID = rospy.get_param('~base_frame_id', "base_link")
00078         if not(self.base_frameID[0] == '/'):
00079             self.base_frameID = self.tf_prefix + '/' + self.base_frameID
00080 
00081         # use sensor values or commanded (open-loop) values for joint angles
00082         self.useJointSensors = rospy.get_param('~use_joint_sensors', True) # (set to False in simulation!)
00083         # init. messages:
00084         self.torsoOdom = Odometry()
00085         self.torsoOdom.header.frame_id = rospy.get_param('~odom_frame_id', "odom")
00086         if not(self.torsoOdom.header.frame_id[0] == '/'):
00087             self.torsoOdom.header.frame_id = self.tf_prefix + '/' + self.torsoOdom.header.frame_id
00088 
00089         self.torsoIMU = Imu()
00090         self.torsoIMU.header.frame_id = self.base_frameID
00091         self.jointState = JointState()
00092         self.jointState.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=10)
00105         self.torsoIMUPub = rospy.Publisher("imu", Imu, queue_size=10)
00106         self.jointStatePub = rospy.Publisher("joint_states", JointState, queue_size=10)
00107 
00108         self.tf_br = tf.TransformBroadcaster()
00109 
00110         rospy.loginfo("nao_sensors 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             except RuntimeError, e:
00133                 print "Error accessing ALMemory, exiting...\n"
00134                 print e
00135                 rospy.signal_shutdown("No NaoQI available anymore")
00136 
00137             self.torsoOdom.header.stamp = timestamp
00138             if len(odomData)==2:
00139                 odomData = odomData[1]
00140             elif len(odomData)!=6:
00141                 print "Error getting odom data"
00142                 continue
00143             
00144             self.torsoOdom.pose.pose.position.x = odomData[0]
00145             self.torsoOdom.pose.pose.position.y = odomData[1]
00146             self.torsoOdom.pose.pose.position.z = odomData[2]
00147             q = transformations.quaternion_from_euler(odomData[3], odomData[4], odomData[5])
00148             self.torsoOdom.pose.pose.orientation.x = q[0]
00149             self.torsoOdom.pose.pose.orientation.y = q[1]
00150             self.torsoOdom.pose.pose.orientation.z = q[2]
00151             self.torsoOdom.pose.pose.orientation.w = q[3]
00152 
00153             t = self.torsoOdom.pose.pose.position
00154             q = self.torsoOdom.pose.pose.orientation
00155             
00156             if self.broadcast_odometry:
00157                 self.tf_br.sendTransform((t.x, t.y, t.z), (q.x, q.y, q.z, q.w),
00158                                          timestamp, self.base_frameID, self.torsoOdom.header.frame_id)
00159 
00160             self.torsoOdomPub.publish(self.torsoOdom)
00161 
00162             # Replace 'None' values with 0
00163             # (=> consistent behavior in 1.8 / 1.10 with 1.6)
00164             # TODO: still required with 1.12 / 1.14?
00165             for i, m in enumerate(memData):
00166                 if m is None:
00167                     memData[i] = 0
00168 
00169             if len(memData) != len(self.dataNamesList):
00170                 print "memData length does not match expected length"
00171                 print memData
00172                 continue
00173 
00174 
00175             # IMU data:
00176             self.torsoIMU.header.stamp = timestamp
00177             q = transformations.quaternion_from_euler(memData[1], memData[2], memData[3])
00178             self.torsoIMU.orientation.x = q[0]
00179             self.torsoIMU.orientation.y = q[1]
00180             self.torsoIMU.orientation.z = q[2]
00181             self.torsoIMU.orientation.w = q[3]
00182 
00183             self.torsoIMU.angular_velocity.x = memData[4]
00184             self.torsoIMU.angular_velocity.y = memData[5]
00185             self.torsoIMU.angular_velocity.z = memData[6] # currently always 0
00186 
00187             self.torsoIMU.linear_acceleration.x = memData[7]
00188             self.torsoIMU.linear_acceleration.y = memData[8]
00189             self.torsoIMU.linear_acceleration.z = memData[9]
00190 
00191             # covariances unknown
00192             # cf http://www.ros.org/doc/api/sensor_msgs/html/msg/Imu.html
00193             self.torsoIMU.orientation_covariance[0] = -1
00194             self.torsoIMU.angular_velocity_covariance[0] = -1
00195             self.torsoIMU.linear_acceleration_covariance[0] = -1
00196 
00197             self.torsoIMUPub.publish(self.torsoIMU)
00198 
00199             # Send JointState:
00200             self.jointState.header.stamp = timestamp
00201             self.jointState.header.frame_id = self.base_frameID
00202             self.jointState.position = positionData
00203 
00204             # simulated model misses some joints, we need to fill:
00205             if (len(self.jointState.position) == 22):
00206                 self.jointState.position.insert(6, 0.0)
00207                 self.jointState.position.insert(7, 0.0)
00208                 self.jointState.position.append(0.0)
00209                 self.jointState.position.append(0.0)
00210 
00211             self.jointStatePub.publish(self.jointState)
00212 
00213             self.sensorRate.sleep()
00214 
00215 if __name__ == '__main__':
00216 
00217     sensors = NaoSensors()
00218     sensors.start()
00219 
00220     rospy.spin()
00221     exit(0)


nao_driver
Author(s): Armin Hornung, Armin Hornung, Stefan Osswald, Daniel Maier, Miguel Sarabia, Séverin Lemaignan
autogenerated on Thu Oct 30 2014 09:20:04