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         tmp_array = []
00167         for i in xrange(scanNum, 0, -1):
00168             keyX = keyPrefix + 'Seg' + '%02d' % (i,) + '/X/Sensor/Value'
00169             keyY = keyPrefix + 'Seg' + '%02d' % (i,) + '/Y/Sensor/Value'
00170             tmp_array.append(keyX)
00171             tmp_array.append(keyY)
00172         memData = self.memProxy.getListData(tmp_array)
00173         for i in xrange(0, len(tmp_array), 2):
00174             x = memData[i]
00175             y = memData[i+1]
00176             ranges.append(math.sqrt(math.pow(x, 2.0) + math.pow(y, 2.0)))
00177         return ranges
00178 
00179     def fetchPCValues(self, keyPrefix, scanNum):
00180         scans = []
00181         for i in xrange(scanNum,0,-1):
00182             keyX = keyPrefix+'Seg'+'%02d'%(i,)+'/X/Sensor/Value'
00183             keyY = keyPrefix+'Seg'+'%02d'%(i,)+'/Y/Sensor/Value'
00184             x = self.memProxy.getData(keyX)
00185             y = self.memProxy.getData(keyY)
00186             scans.append(x)
00187             scans.append(y)
00188             scans.append(0.0)
00189         ba = struct.pack('%sf' %len(scans), *scans)
00190         return ba
00191 
00192     def createPointCloudMessage(self, frameID, keyPrefix, scanNum):
00193         pointCloudMsg = PointCloud2()
00194         pointCloudMsg.header.frame_id = frameID
00195         pointCloudMsg.header.stamp = rospy.Time.now()
00196         pointCloudMsg.height = 1
00197         pointCloudMsg.width = scanNum
00198         pointCloudMsg.is_dense = False
00199         pointCloudMsg.is_bigendian = False
00200         pointCloudMsg.fields = [PointField('x', 0, PointField.FLOAT32, 1),
00201                                 PointField('y', 4, PointField.FLOAT32, 1),
00202                                 PointField('z', 8, PointField.FLOAT32, 1)]
00203         pointCloudMsg.point_step = 4*3
00204         pointCloudMsg.row_step = pointCloudMsg.point_step* pointCloudMsg.width
00205         pointCloudMsg.data = self.fetchLaserValues(keyPrefix, scanNum)
00206         return pointCloudMsg
00207 
00208     def createLaserMessage(self, frameID, keyPrefix, scanNum):
00209         laserScanMsg = LaserScan()
00210         laserScanMsg.header.frame_id = frameID
00211         laserScanMsg.angle_min = self.PEPPER_LASER_MIN_ANGLE
00212         laserScanMsg.angle_max = self.PEPPER_LASER_MAX_ANGLE
00213         laserScanMsg.angle_increment = self.PEPPER_LASER_FOV/scanNum
00214         laserScanMsg.range_min = self.PEPPER_LASER_MIN_RANGE
00215         laserScanMsg.range_max = self.PEPPER_LASER_MAX_RANGE
00216         return laserScanMsg
00217 
00218     # do it!
00219     def run(self):
00220         # start subscriber to laser sensor
00221         #self.laserProxy.subscribe(self.PEPPER_LASER_SUB_NAME)
00222 
00223         # we publish 6 laser messages in total
00224         # 1. shovel, 2. ground_left, 3. ground_right
00225         # 4. srd_front 5. srd_left 6. srd_right
00226         if self.pointcloud:
00227             shovelPC = self.createPointCloudMessage(
00228                                 self.laserShovelFrame,
00229                                 self.PEPPER_MEM_KEY_GROUND_SHOVEL,
00230                                 self.PEPPER_LASER_GROUND_SHOVEL_POINTS )
00231             groundLeftPC = self.createPointCloudMessage(
00232                                 self.laserGroundLeftFrame,
00233                                 self.PEPPER_MEM_KEY_GROUND_LEFT,
00234                                 self.PEPPER_LASER_GROUND_LEFT_POINTS )
00235             groundRightPC = self.createPointCloudMessage(
00236                                 self.laserGroundRightFrame,
00237                                 self.PEPPER_MEM_KEY_GROUND_RIGHT,
00238                                 self.PEPPER_LASER_GROUND_RIGHT_POINTS )
00239             srdFrontPC = self.createPointCloudMessage(
00240                                 self.laserSRDFrontFrame,
00241                                 self.PEPPER_MEM_KEY_SRD_FRONT,
00242                                 self.PEPPER_LASER_SRD_POINTS )
00243             srdLeftPC = self.createPointCloudMessage(
00244                                 self.laserSRDLeftFrame,
00245                                 self.PEPPER_MEM_KEY_SRD_LEFT,
00246                                 self.PEPPER_LASER_SRD_POINTS )
00247             srdRightPC = self.createPointCloudMessage(
00248                                 self.laserSRDRightFrame,
00249                                 self.PEPPER_MEM_KEY_SRD_RIGHT,
00250                                 self.PEPPER_LASER_SRD_POINTS )
00251 
00252         if self.laserscan:
00253             shovelScan = self.createLaserMessage(
00254                                 self.laserShovelFrame,
00255                                 self.PEPPER_MEM_KEY_GROUND_SHOVEL,
00256                                 self.PEPPER_LASER_GROUND_SHOVEL_POINTS )
00257             groundLeftScan = self.createLaserMessage(
00258                                 self.laserGroundLeftFrame,
00259                                 self.PEPPER_MEM_KEY_GROUND_LEFT,
00260                                 self.PEPPER_LASER_GROUND_LEFT_POINTS )
00261             groundRightScan = self.createLaserMessage(
00262                                 self.laserGroundRightFrame,
00263                                 self.PEPPER_MEM_KEY_GROUND_RIGHT,
00264                                 self.PEPPER_LASER_GROUND_RIGHT_POINTS )
00265             srdFrontScan = self.createLaserMessage(
00266                                 self.laserSRDFrontFrame,
00267                                 self.PEPPER_MEM_KEY_SRD_FRONT,
00268                                 self.PEPPER_LASER_SRD_POINTS )
00269             srdLeftScan = self.createLaserMessage(
00270                                 self.laserSRDLeftFrame,
00271                                 self.PEPPER_MEM_KEY_SRD_LEFT,
00272                                 self.PEPPER_LASER_SRD_POINTS )
00273             srdRightScan = self.createLaserMessage(
00274                                 self.laserSRDRightFrame,
00275                                 self.PEPPER_MEM_KEY_SRD_RIGHT,
00276                                 self.PEPPER_LASER_SRD_POINTS )
00277 
00278         while(self.is_looping()):
00279 
00280             if self.laserscan:
00281                 # fetch values
00282                 shovelScan.header.stamp = rospy.Time.now()
00283                 shovelScan.ranges = self.fetchLaserValues(
00284                                             self.PEPPER_MEM_KEY_GROUND_SHOVEL,
00285                                             self.PEPPER_LASER_GROUND_SHOVEL_POINTS
00286                                             )
00287 
00288                 groundLeftScan.header.stamp = rospy.Time.now()
00289                 groundLeftScan.ranges = self.fetchLaserValues(
00290                                             self.PEPPER_MEM_KEY_GROUND_LEFT,
00291                                             self.PEPPER_LASER_GROUND_LEFT_POINTS
00292                                             )
00293 
00294                 groundRightScan.header.stamp = rospy.Time.now()
00295                 groundRightScan.ranges = self.fetchLaserValues(
00296                                             self.PEPPER_MEM_KEY_GROUND_RIGHT,
00297                                             self.PEPPER_LASER_GROUND_RIGHT_POINTS
00298                                             )
00299 
00300                 srdFrontScan.header.stamp = rospy.Time.now()
00301                 srdFrontScan.ranges = self.fetchLaserValues(
00302                                             self.PEPPER_MEM_KEY_SRD_FRONT,
00303                                             self.PEPPER_LASER_SRD_POINTS
00304                                             )
00305 
00306                 srdLeftScan.header.stamp = rospy.Time.now()
00307                 srdLeftScan.ranges = self.fetchLaserValues(
00308                                             self.PEPPER_MEM_KEY_SRD_LEFT,
00309                                             self.PEPPER_LASER_SRD_POINTS
00310                                             )
00311 
00312                 srdRightScan.header.stamp = rospy.Time.now()
00313                 srdRightScan.ranges = self.fetchLaserValues(
00314                                             self.PEPPER_MEM_KEY_SRD_RIGHT,
00315                                             self.PEPPER_LASER_SRD_POINTS
00316                                             )
00317 
00318                 # publish messages
00319                 self.laserShovelPublisher.publish(shovelScan)
00320                 self.laserGroundLeftPublisher.publish(groundLeftScan)
00321                 self.laserGroundRightPublisher.publish(groundRightScan)
00322                 self.laserSRDFrontPublisher.publish(srdFrontScan)
00323                 self.laserSRDLeftPublisher.publish(srdLeftScan)
00324                 self.laserSRDRightPublisher.publish(srdRightScan)
00325 
00326 
00327             if self.pointcloud:
00328                 # fetch values
00329                 shovelPC.header.stamp = rospy.Time.now()
00330                 shovelPC.data = self.fetchPCValues(
00331                                             self.PEPPER_MEM_KEY_GROUND_SHOVEL,
00332                                             self.PEPPER_LASER_GROUND_SHOVEL_POINTS
00333                                             )
00334 
00335                 groundLeftPC.header.stamp = rospy.Time.now()
00336                 groundLeftPC.data = self.fetchPCValues(
00337                                             self.PEPPER_MEM_KEY_GROUND_LEFT,
00338                                             self.PEPPER_LASER_GROUND_LEFT_POINTS
00339                                             )
00340 
00341                 groundRightPC.header.stamp = rospy.Time.now()
00342                 groundRightPC.data = self.fetchPCValues(
00343                                             self.PEPPER_MEM_KEY_GROUND_RIGHT,
00344                                             self.PEPPER_LASER_GROUND_RIGHT_POINTS
00345                                             )
00346 
00347                 srdFrontPC.header.stamp = rospy.Time.now()
00348                 srdFrontPC.data = self.fetchPCValues(
00349                                             self.PEPPER_MEM_KEY_SRD_FRONT,
00350                                             self.PEPPER_LASER_SRD_POINTS
00351                                             )
00352 
00353                 srdLeftPC.header.stamp = rospy.Time.now()
00354                 srdLeftPC.data = self.fetchPCValues(
00355                                             self.PEPPER_MEM_KEY_SRD_LEFT,
00356                                             self.PEPPER_LASER_SRD_POINTS
00357                                             )
00358 
00359                 srdRightPC.header.stamp = rospy.Time.now()
00360                 srdRightPC.data = self.fetchPCValues(
00361                                             self.PEPPER_MEM_KEY_SRD_RIGHT,
00362                                             self.PEPPER_LASER_SRD_POINTS
00363                                             )
00364 
00365                 # publish messages
00366                 self.pcShovelPublisher.publish(shovelPC)
00367                 self.pcGroundLeftPublisher.publish(groundLeftPC)
00368                 self.pcGroundRightPublisher.publish(groundRightPC)
00369                 self.pcSRDFrontPublisher.publish(srdFrontPC)
00370                 self.pcSRDLeftPublisher.publish(srdLeftPC)
00371                 self.pcSRDRightPublisher.publish(srdRightPC)
00372 
00373             #sleep
00374             self.laserRate.sleep()
00375 
00376 
00377 if __name__ == '__main__':
00378 #    from optparse import OptionParser
00379 #    parser = OptionParser()
00380 #    parser.add_option("--ppointcloud", dest="ppointcloud", default=True)
00381 #    parser.add_option("--plaser", dest="plaser", default=False)
00382 #
00383 #    (options, args) = parser.parse_args()
00384 #    with_pc = options.ppointcloud
00385 #    with_laser = options.plaser
00386 
00387     with_pc = True
00388     with_laser = True
00389     laser = NaoqiLaser(with_pc, with_laser)
00390     laser.start()
00391 
00392     rospy.spin()
00393     exit(0)


pepper_sensors_py
Author(s): Karsten Knese
autogenerated on Thu Jun 6 2019 20:15:11