laser.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # Copyright (C) 2014 Aldebaran Robotics
00004 #
00005 # Licensed under the Apache License, Version 2.0 (the "License");
00006 # you may not use this file except in compliance with the License.
00007 # You may obtain a copy of the License at
00008 #
00009 #     http://www.apache.org/licenses/LICENSE-2.0
00010 #
00011 # Unless required by applicable law or agreed to in writing, software
00012 # distributed under the License is distributed on an "AS IS" BASIS,
00013 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00014 # See the License for the specific language governing permissions and
00015 # limitations under the License.
00016 #
00017 
00018 
00019 #import ROS dependencies
00020 import rospy
00021 from sensor_msgs.msg import PointCloud2, PointField
00022 
00023 # navigation test
00024 from sensor_msgs.msg import LaserScan
00025 
00026 #import PEPPER dependencies
00027 from naoqi_driver.naoqi_node import NaoqiNode
00028 
00029 #general
00030 import copy
00031 import math
00032 import struct
00033 
00034 #PEPPER specifications:
00035 #https://community.aldebaran.com/doc/1-14/family/robots/laser_robot.html
00036 class NaoqiLaser(NaoqiNode):
00037 
00038     # PEPPER laser specs
00039     # see https://community.aldebaran.com/doc/2-1/family/juliette_technical/laser_juliette.html#juliette-laser
00040     PEPPER_LASER_FREQ = 6                       # [hZ] --> check on that
00041     PEPPER_LASER_MIN_ANGLE = -0.523598776          # [rad]
00042     PEPPER_LASER_MAX_ANGLE = 0.523598776           # [radi]
00043     PEPPER_LASER_FOV = math.fabs(PEPPER_LASER_MIN_ANGLE)+math.fabs(PEPPER_LASER_MAX_ANGLE)
00044 
00045     PEPPER_LASER_MIN_RANGE = 0.1                   # [m] --> no spec given here
00046     PEPPER_LASER_MAX_RANGE = 5.0                   # [m] --> same here, 5m as quality guess
00047 
00048     # FRONT GROUND LASERS
00049     PEPPER_LASER_GROUND_SHOVEL_POINTS = 3
00050     PEPPER_LASER_GROUND_LEFT_POINTS = 1
00051     PEPPER_LASER_GROUND_RIGHT_POINTS = 1
00052     # SURROUNDING LASER
00053     PEPPER_LASER_SRD_POINTS = 15
00054 
00055     # memory key to fetch laser readings from
00056     # see memory key listing https://community.aldebaran.com/doc/2-1/family/juliette_technical/juliette_dcm/actuator_sensor_names.html#lasers
00057     # iterate over all segments e.g./SubDeviceList/Platform/LaserSensor/Front/Shovel/Seg01/X/Value
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     # ROS params to check
00066     # acc. to spec: 40 kHz
00067     PARAM_LASER_RATE = '~laser_rate'
00068     PARAM_LASER_RATE_DEFAULT = PEPPER_LASER_FREQ
00069 
00070     # frame id to publish
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 = '~/laser/shovel/'
00090     TOPIC_LASER_GROUND_LEFT = '~/laser/ground_left/'
00091     TOPIC_LASER_GROUND_RIGHT = '~/laser/ground_right/'
00092     TOPIC_LASER_SRD_FRONT = '~/laser/srd_front/'
00093     TOPIC_LASER_SRD_LEFT = '~/laser/srd_left/'
00094     TOPIC_LASER_SRD_RIGHT = '~/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         # ROS initialization;
00105         self.connectNaoQi()
00106 
00107         # default sensor rate: 25 Hz (50 is max, stresses Nao's CPU)
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     # (re-) connect to NaoQI:
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     # fetch laser values
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         # traverse backwards
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     # do it!
00214     def run(self):
00215         # start subscriber to laser sensor
00216         #self.laserProxy.subscribe(self.PEPPER_LASER_SUB_NAME)
00217 
00218         # we publish 6 laser messages in total
00219         # 1. shovel, 2. ground_left, 3. ground_right
00220         # 4. srd_front 5. srd_left 6. srd_right
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                 # fetch values
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                 # publish messages
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                 # fetch values
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                 # publish messages
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             #sleep
00369             self.laserRate.sleep()
00370 
00371 
00372 if __name__ == '__main__':
00373 #    from optparse import OptionParser
00374 #    parser = OptionParser()
00375 #    parser.add_option("--ppointcloud", dest="ppointcloud", default=True)
00376 #    parser.add_option("--plaser", dest="plaser", default=False)
00377 #
00378 #    (options, args) = parser.parse_args()
00379 #    with_pc = options.ppointcloud
00380 #    with_laser = options.plaser
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)


pepper_sensors_py
Author(s):
autogenerated on Fri Aug 28 2015 11:59:35