00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020 import rospy
00021 from sensor_msgs.msg import PointCloud2, PointField
00022
00023
00024 from sensor_msgs.msg import LaserScan
00025
00026
00027 from naoqi_driver.naoqi_node import NaoqiNode
00028
00029
00030 import copy
00031 import math
00032 import struct
00033
00034
00035
00036 class NaoqiLaser(NaoqiNode):
00037
00038
00039
00040 PEPPER_LASER_FREQ = 6
00041 PEPPER_LASER_MIN_ANGLE = -0.523598776
00042 PEPPER_LASER_MAX_ANGLE = 0.523598776
00043 PEPPER_LASER_FOV = math.fabs(PEPPER_LASER_MIN_ANGLE)+math.fabs(PEPPER_LASER_MAX_ANGLE)
00044
00045 PEPPER_LASER_MIN_RANGE = 0.1
00046 PEPPER_LASER_MAX_RANGE = 5.0
00047
00048
00049 PEPPER_LASER_GROUND_SHOVEL_POINTS = 3
00050 PEPPER_LASER_GROUND_LEFT_POINTS = 1
00051 PEPPER_LASER_GROUND_RIGHT_POINTS = 1
00052
00053 PEPPER_LASER_SRD_POINTS = 15
00054
00055
00056
00057
00058 PEPPER_MEM_KEY_GROUND_SHOVEL = 'Device/SubDeviceList/Platform/LaserSensor/Front/Shovel/'
00059 PEPPER_MEM_KEY_GROUND_LEFT = 'Device/SubDeviceList/Platform/LaserSensor/Front/Vertical/Left/'
00060 PEPPER_MEM_KEY_GROUND_RIGHT = 'Device/SubDeviceList/Platform/LaserSensor/Front/Vertical/Right/'
00061 PEPPER_MEM_KEY_SRD_FRONT = 'Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/'
00062 PEPPER_MEM_KEY_SRD_LEFT = 'Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/'
00063 PEPPER_MEM_KEY_SRD_RIGHT = 'Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/'
00064
00065
00066
00067 PARAM_LASER_RATE = '~laser_rate'
00068 PARAM_LASER_RATE_DEFAULT = PEPPER_LASER_FREQ
00069
00070
00071 PARAM_LASER_SHOVEL_FRAME = '~laser_shovel_frame_id'
00072 PARAM_LASER_SHOVEL_FRAME_DEFAULT = 'ShovelLaser_frame'
00073
00074 PARAM_LASER_GROUND_LEFT_FRAME = '~laser_ground_left_frame_id'
00075 PARAM_LASER_GROUND_LEFT_FRAME_DEFAULT = 'VerticalLeftLaser_frame'
00076
00077 PARAM_LASER_GROUND_RIGHT_FRAME = '~laser_ground_right_frame_id'
00078 PARAM_LASER_GROUND_RIGHT_FRAME_DEFAULT = 'VerticalRightLaser_frame'
00079
00080 PARAM_LASER_SRD_FRONT_FRAME = '~laser_srd_front_frame_id'
00081 PARAM_LASER_SRD_FRONT_FRAME_DEFAULT = 'SurroundingFrontLaser_frame'
00082
00083 PARAM_LASER_SRD_LEFT_FRAME = '~laser_srd_left_frame_id'
00084 PARAM_LASER_SRD_LEFT_FRAME_DEFAULT = 'SurroundingLeftLaser_frame'
00085
00086 PARAM_LASER_SRD_RIGHT_FRAME = '~laser_srd_right_frame_id'
00087 PARAM_LASER_SRD_RIGHT_FRAME_DEFAULT = 'SurroundingRightLaser_frame'
00088
00089 TOPIC_LASER_SHOVEL = '~/pepper/laser/shovel/'
00090 TOPIC_LASER_GROUND_LEFT = '~/pepper/laser/ground_left/'
00091 TOPIC_LASER_GROUND_RIGHT = '~/pepper/laser/ground_right/'
00092 TOPIC_LASER_SRD_FRONT = '~/pepper/laser/srd_front/'
00093 TOPIC_LASER_SRD_LEFT = '~/pepper/laser/srd_left/'
00094 TOPIC_LASER_SRD_RIGHT = '~/pepper/laser/srd_right/'
00095
00096 PEPPER_LASER_SUB_NAME = 'pepper_ros_laser'
00097
00098 def __init__(self, pointcloud=True, laserscan=False):
00099 self.pointcloud = pointcloud
00100 self.laserscan = laserscan
00101
00102 NaoqiNode.__init__(self, 'pepper_node')
00103
00104
00105 self.connectNaoQi()
00106
00107
00108 self.laserRate = rospy.Rate(rospy.get_param(
00109 self.PARAM_LASER_RATE,
00110 self.PARAM_LASER_RATE_DEFAULT))
00111
00112 self.laserShovelFrame = rospy.get_param(
00113 self.PARAM_LASER_SHOVEL_FRAME,
00114 self.PARAM_LASER_SHOVEL_FRAME_DEFAULT)
00115 self.laserGroundLeftFrame = rospy.get_param(
00116 self.PARAM_LASER_GROUND_LEFT_FRAME,
00117 self.PARAM_LASER_GROUND_LEFT_FRAME_DEFAULT)
00118 self.laserGroundRightFrame = rospy.get_param(
00119 self.PARAM_LASER_GROUND_RIGHT_FRAME,
00120 self.PARAM_LASER_GROUND_RIGHT_FRAME_DEFAULT)
00121 self.laserSRDFrontFrame = rospy.get_param(
00122 self.PARAM_LASER_SRD_FRONT_FRAME,
00123 self.PARAM_LASER_SRD_FRONT_FRAME_DEFAULT)
00124 self.laserSRDLeftFrame = rospy.get_param(
00125 self.PARAM_LASER_SRD_LEFT_FRAME,
00126 self.PARAM_LASER_SRD_LEFT_FRAME_DEFAULT)
00127 self.laserSRDRightFrame = rospy.get_param(
00128 self.PARAM_LASER_SRD_RIGHT_FRAME,
00129 self.PARAM_LASER_SRD_RIGHT_FRAME_DEFAULT)
00130
00131 if self.pointcloud:
00132 self.pcShovelPublisher = rospy.Publisher(self.TOPIC_LASER_SHOVEL+'pointcloud', PointCloud2, queue_size=1)
00133 self.pcGroundLeftPublisher = rospy.Publisher(self.TOPIC_LASER_GROUND_LEFT+'pointcloud', PointCloud2, queue_size=1)
00134 self.pcGroundRightPublisher = rospy.Publisher(self.TOPIC_LASER_GROUND_RIGHT+'pointcloud', PointCloud2, queue_size=1)
00135 self.pcSRDFrontPublisher = rospy.Publisher(self.TOPIC_LASER_SRD_FRONT+'pointcloud', PointCloud2, queue_size=1)
00136 self.pcSRDLeftPublisher = rospy.Publisher(self.TOPIC_LASER_SRD_LEFT+'pointcloud', PointCloud2, queue_size=1)
00137 self.pcSRDRightPublisher = rospy.Publisher(self.TOPIC_LASER_SRD_RIGHT+'pointcloud', PointCloud2, queue_size=1)
00138
00139 if self.laserscan:
00140 self.laserShovelPublisher = rospy.Publisher(self.TOPIC_LASER_SHOVEL+'scan', LaserScan, queue_size=1)
00141 self.laserGroundLeftPublisher = rospy.Publisher(self.TOPIC_LASER_GROUND_LEFT+'scan', LaserScan, queue_size=1)
00142 self.laserGroundRightPublisher = rospy.Publisher(self.TOPIC_LASER_GROUND_RIGHT+'scan', LaserScan, queue_size=1)
00143 self.laserSRDFrontPublisher = rospy.Publisher(self.TOPIC_LASER_SRD_FRONT+'scan', LaserScan, queue_size=1)
00144 self.laserSRDLeftPublisher = rospy.Publisher(self.TOPIC_LASER_SRD_LEFT+'scan', LaserScan, queue_size=1)
00145 self.laserSRDRightPublisher = rospy.Publisher(self.TOPIC_LASER_SRD_RIGHT+'scan', LaserScan, queue_size=1)
00146
00147 self.laserSRDFrontPublisher_test = rospy.Publisher("~/pepper_navigation/front", LaserScan, queue_size=1)
00148
00149
00150 def connectNaoQi(self):
00151 rospy.loginfo("Connecting to NaoQi at %s:%d", self.pip, self.pport)
00152 self.laserProxy = self.get_proxy("ALLaser")
00153 self.memProxy = self.get_proxy("ALMemory")
00154 if self.laserProxy is None or self.memProxy is None:
00155 print "could not start either laser or memory proxy"
00156 exit(1)
00157
00158
00159 ''' the idea here is to get the point xy
00160 calculate the length from the origin (range)
00161 assumption: field of view spanning from very min to very max
00162 '''
00163 def fetchLaserValues(self, keyPrefix, scanNum):
00164 ranges = []
00165
00166 for i in xrange(scanNum,0,-1):
00167 keyX = keyPrefix+'Seg'+'%02d'%(i,)+'/X/Sensor/Value'
00168 keyY = keyPrefix+'Seg'+'%02d'%(i,)+'/Y/Sensor/Value'
00169 x = self.memProxy.getData(keyX)
00170 y = self.memProxy.getData(keyY)
00171 ranges.append(math.sqrt(math.pow(x,2)+math.pow(y,2)))
00172 return ranges
00173
00174 def fetchPCValues(self, keyPrefix, scanNum):
00175 scans = []
00176 for i in xrange(scanNum,0,-1):
00177 keyX = keyPrefix+'Seg'+'%02d'%(i,)+'/X/Sensor/Value'
00178 keyY = keyPrefix+'Seg'+'%02d'%(i,)+'/Y/Sensor/Value'
00179 x = self.memProxy.getData(keyX)
00180 y = self.memProxy.getData(keyY)
00181 scans.append(x)
00182 scans.append(y)
00183 scans.append(0.0)
00184 ba = struct.pack('%sf' %len(scans), *scans)
00185 return ba
00186
00187 def createPointCloudMessage(self, frameID, keyPrefix, scanNum):
00188 pointCloudMsg = PointCloud2()
00189 pointCloudMsg.header.frame_id = frameID
00190 pointCloudMsg.header.stamp = rospy.Time.now()
00191 pointCloudMsg.height = 1
00192 pointCloudMsg.width = scanNum
00193 pointCloudMsg.is_dense = False
00194 pointCloudMsg.is_bigendian = False
00195 pointCloudMsg.fields = [PointField('x', 0, PointField.FLOAT32, 1),
00196 PointField('y', 4, PointField.FLOAT32, 1),
00197 PointField('z', 8, PointField.FLOAT32, 1)]
00198 pointCloudMsg.point_step = 4*3
00199 pointCloudMsg.row_step = pointCloudMsg.point_step* pointCloudMsg.width
00200 pointCloudMsg.data = self.fetchLaserValues(keyPrefix, scanNum)
00201 return pointCloudMsg
00202
00203 def createLaserMessage(self, frameID, keyPrefix, scanNum):
00204 laserScanMsg = LaserScan()
00205 laserScanMsg.header.frame_id = frameID
00206 laserScanMsg.angle_min = self.PEPPER_LASER_MIN_ANGLE
00207 laserScanMsg.angle_max = self.PEPPER_LASER_MAX_ANGLE
00208 laserScanMsg.angle_increment = self.PEPPER_LASER_FOV/scanNum
00209 laserScanMsg.range_min = self.PEPPER_LASER_MIN_RANGE
00210 laserScanMsg.range_max = self.PEPPER_LASER_MAX_RANGE
00211 return laserScanMsg
00212
00213
00214 def run(self):
00215
00216
00217
00218
00219
00220
00221 if self.pointcloud:
00222 shovelPC = self.createPointCloudMessage(
00223 self.laserShovelFrame,
00224 self.PEPPER_MEM_KEY_GROUND_SHOVEL,
00225 self.PEPPER_LASER_GROUND_SHOVEL_POINTS )
00226 groundLeftPC = self.createPointCloudMessage(
00227 self.laserGroundLeftFrame,
00228 self.PEPPER_MEM_KEY_GROUND_LEFT,
00229 self.PEPPER_LASER_GROUND_LEFT_POINTS )
00230 groundRightPC = self.createPointCloudMessage(
00231 self.laserGroundRightFrame,
00232 self.PEPPER_MEM_KEY_GROUND_RIGHT,
00233 self.PEPPER_LASER_GROUND_RIGHT_POINTS )
00234 srdFrontPC = self.createPointCloudMessage(
00235 self.laserSRDFrontFrame,
00236 self.PEPPER_MEM_KEY_SRD_FRONT,
00237 self.PEPPER_LASER_SRD_POINTS )
00238 srdLeftPC = self.createPointCloudMessage(
00239 self.laserSRDLeftFrame,
00240 self.PEPPER_MEM_KEY_SRD_LEFT,
00241 self.PEPPER_LASER_SRD_POINTS )
00242 srdRightPC = self.createPointCloudMessage(
00243 self.laserSRDRightFrame,
00244 self.PEPPER_MEM_KEY_SRD_RIGHT,
00245 self.PEPPER_LASER_SRD_POINTS )
00246
00247 if self.laserscan:
00248 shovelScan = self.createLaserMessage(
00249 self.laserShovelFrame,
00250 self.PEPPER_MEM_KEY_GROUND_SHOVEL,
00251 self.PEPPER_LASER_GROUND_SHOVEL_POINTS )
00252 groundLeftScan = self.createLaserMessage(
00253 self.laserGroundLeftFrame,
00254 self.PEPPER_MEM_KEY_GROUND_LEFT,
00255 self.PEPPER_LASER_GROUND_LEFT_POINTS )
00256 groundRightScan = self.createLaserMessage(
00257 self.laserGroundRightFrame,
00258 self.PEPPER_MEM_KEY_GROUND_RIGHT,
00259 self.PEPPER_LASER_GROUND_RIGHT_POINTS )
00260 srdFrontScan = self.createLaserMessage(
00261 self.laserSRDFrontFrame,
00262 self.PEPPER_MEM_KEY_SRD_FRONT,
00263 self.PEPPER_LASER_SRD_POINTS )
00264 srdLeftScan = self.createLaserMessage(
00265 self.laserSRDLeftFrame,
00266 self.PEPPER_MEM_KEY_SRD_LEFT,
00267 self.PEPPER_LASER_SRD_POINTS )
00268 srdRightScan = self.createLaserMessage(
00269 self.laserSRDRightFrame,
00270 self.PEPPER_MEM_KEY_SRD_RIGHT,
00271 self.PEPPER_LASER_SRD_POINTS )
00272
00273 while(self.is_looping()):
00274
00275 if self.laserscan:
00276
00277 shovelScan.header.stamp = rospy.Time.now()
00278 shovelScan.ranges = self.fetchLaserValues(
00279 self.PEPPER_MEM_KEY_GROUND_SHOVEL,
00280 self.PEPPER_LASER_GROUND_SHOVEL_POINTS
00281 )
00282
00283 groundLeftScan.header.stamp = rospy.Time.now()
00284 groundLeftScan.ranges = self.fetchLaserValues(
00285 self.PEPPER_MEM_KEY_GROUND_LEFT,
00286 self.PEPPER_LASER_GROUND_LEFT_POINTS
00287 )
00288
00289 groundRightScan.header.stamp = rospy.Time.now()
00290 groundRightScan.ranges = self.fetchLaserValues(
00291 self.PEPPER_MEM_KEY_GROUND_RIGHT,
00292 self.PEPPER_LASER_GROUND_RIGHT_POINTS
00293 )
00294
00295 srdFrontScan.header.stamp = rospy.Time.now()
00296 srdFrontScan.ranges = self.fetchLaserValues(
00297 self.PEPPER_MEM_KEY_SRD_FRONT,
00298 self.PEPPER_LASER_SRD_POINTS
00299 )
00300
00301 srdLeftScan.header.stamp = rospy.Time.now()
00302 srdLeftScan.ranges = self.fetchLaserValues(
00303 self.PEPPER_MEM_KEY_SRD_LEFT,
00304 self.PEPPER_LASER_SRD_POINTS
00305 )
00306
00307 srdRightScan.header.stamp = rospy.Time.now()
00308 srdRightScan.ranges = self.fetchLaserValues(
00309 self.PEPPER_MEM_KEY_SRD_RIGHT,
00310 self.PEPPER_LASER_SRD_POINTS
00311 )
00312
00313
00314 self.laserShovelPublisher.publish(shovelScan)
00315 self.laserGroundLeftPublisher.publish(groundLeftScan)
00316 self.laserGroundRightPublisher.publish(groundRightScan)
00317 self.laserSRDFrontPublisher.publish(srdFrontScan)
00318 self.laserSRDLeftPublisher.publish(srdLeftScan)
00319 self.laserSRDRightPublisher.publish(srdRightScan)
00320
00321
00322 if self.pointcloud:
00323
00324 shovelPC.header.stamp = rospy.Time.now()
00325 shovelPC.data = self.fetchPCValues(
00326 self.PEPPER_MEM_KEY_GROUND_SHOVEL,
00327 self.PEPPER_LASER_GROUND_SHOVEL_POINTS
00328 )
00329
00330 groundLeftPC.header.stamp = rospy.Time.now()
00331 groundLeftPC.data = self.fetchPCValues(
00332 self.PEPPER_MEM_KEY_GROUND_LEFT,
00333 self.PEPPER_LASER_GROUND_LEFT_POINTS
00334 )
00335
00336 groundRightPC.header.stamp = rospy.Time.now()
00337 groundRightPC.data = self.fetchPCValues(
00338 self.PEPPER_MEM_KEY_GROUND_RIGHT,
00339 self.PEPPER_LASER_GROUND_RIGHT_POINTS
00340 )
00341
00342 srdFrontPC.header.stamp = rospy.Time.now()
00343 srdFrontPC.data = self.fetchPCValues(
00344 self.PEPPER_MEM_KEY_SRD_FRONT,
00345 self.PEPPER_LASER_SRD_POINTS
00346 )
00347
00348 srdLeftPC.header.stamp = rospy.Time.now()
00349 srdLeftPC.data = self.fetchPCValues(
00350 self.PEPPER_MEM_KEY_SRD_LEFT,
00351 self.PEPPER_LASER_SRD_POINTS
00352 )
00353
00354 srdRightPC.header.stamp = rospy.Time.now()
00355 srdRightPC.data = self.fetchPCValues(
00356 self.PEPPER_MEM_KEY_SRD_RIGHT,
00357 self.PEPPER_LASER_SRD_POINTS
00358 )
00359
00360
00361 self.pcShovelPublisher.publish(shovelPC)
00362 self.pcGroundLeftPublisher.publish(groundLeftPC)
00363 self.pcGroundRightPublisher.publish(groundRightPC)
00364 self.pcSRDFrontPublisher.publish(srdFrontPC)
00365 self.pcSRDLeftPublisher.publish(srdLeftPC)
00366 self.pcSRDRightPublisher.publish(srdRightPC)
00367
00368
00369 self.laserRate.sleep()
00370
00371
00372 if __name__ == '__main__':
00373
00374
00375
00376
00377
00378
00379
00380
00381
00382 with_pc = True
00383 with_laser = True
00384 laser = NaoqiLaser(with_pc, with_laser)
00385 laser.start()
00386
00387 rospy.spin()
00388 exit(0)