38 from sensor_msgs.msg
import JointState
39 from sensor_msgs.msg
import Imu
40 from nav_msgs.msg
import Odometry
44 from tf
import transformations
52 NaoqiNode.__init__(self,
'naoqi_joint_states')
57 self.
sensorRate = rospy.Rate(rospy.get_param(
'~sensor_rate', 25.0))
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"]
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)
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
90 self.jointState.name = self.motionProxy.getJointNames(
'Body')
92 self.jointStiffness.name = self.motionProxy.getJointNames(
'Body')
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")
101 msg =
"Nao joints found: "+ str(self.jointState.name)
106 self.
jointStatePub = rospy.Publisher(
"joint_states", JointState, queue_size=1)
109 self.
tf_br = tf.TransformBroadcaster()
111 rospy.loginfo(
"nao_joint_states initialized")
115 rospy.loginfo(
"Connecting to NaoQi at %s:%d", self.
pip, self.
pport)
122 """ Odometry thread code - collects and sends out odometry esimate. """ 127 timestamp = rospy.Time.now()
131 odomData = self.motionProxy.getPosition(
'Torso', motion.SPACE_WORLD,
True)
132 positionData = self.motionProxy.getAngles(
'Body', self.
useJointSensors)
134 referenceData = self.motionProxy.getAngles(
'Body',
False)
136 referenceData = positionData
137 stiffnessData = self.motionProxy.getStiffnesses(
'Body')
138 except RuntimeError, e:
139 print "Error accessing ALMemory, exiting...\n" 141 rospy.signal_shutdown(
"No NaoQI available anymore")
143 self.torsoOdom.header.stamp = timestamp
145 odomData = odomData[1]
146 elif len(odomData)!=6:
147 print "Error getting odom data" 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]
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)
165 self.torsoOdomPub.publish(self.
torsoOdom)
170 for i, m
in enumerate(memData):
175 print "memData length does not match expected length" 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]
188 self.torsoIMU.angular_velocity.x = memData[4]
189 self.torsoIMU.angular_velocity.y = memData[5]
190 self.torsoIMU.angular_velocity.z = memData[6]
192 self.torsoIMU.linear_acceleration.x = memData[7]
193 self.torsoIMU.linear_acceleration.y = memData[8]
194 self.torsoIMU.linear_acceleration.z = memData[9]
198 self.torsoIMU.orientation_covariance[0] = -1
199 self.torsoIMU.angular_velocity_covariance[0] = -1
200 self.torsoIMU.linear_acceleration_covariance[0] = -1
202 self.torsoIMUPub.publish(self.
torsoIMU)
205 self.jointState.header.stamp = timestamp
207 self.jointState.position = positionData
208 self.jointState.effort = map(
lambda x, y: x - y, positionData, referenceData)
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)
220 self.jointStiffness.header.stamp = timestamp
222 self.jointStiffness.effort = stiffnessData
226 self.sensorRate.sleep()
228 if __name__ ==
'__main__':
def get_proxy(self, name, warn=True)