Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010 from StringIO import StringIO
00011
00012 import rospy
00013 from sensor_msgs.msg import LaserScan
00014 from lama_msgs.msg import Crossing
00015 from lama_msgs.msg import Frontier
00016
00017 from laser_crossing_detector_wrapper_cpp import LaserCrossingDetectorWrapper
00018 from wrapper import WrapperPy
00019
00020
00021 class LaserCrossingDetector(WrapperPy):
00022 def __init__(self, frontier_width, max_frontier_angle=0.785):
00023 self._detector = LaserCrossingDetectorWrapper(frontier_width,
00024 max_frontier_angle)
00025
00026 def crossingDescriptor(self, scan, normalize=False):
00027 """Return a Crossing message from analysis of a LaserScan
00028
00029 Return a lama_msgs/Crossing message from analysis of a
00030 sensor_msgs/LaserScan.
00031
00032 Parameters
00033 ----------
00034 - scan: a sensor_msgs/LaserScan message
00035 - normalize: true if scan should be normalized in a first step.
00036 """
00037 if not isinstance(scan, LaserScan):
00038 err = 'Argument 1 is not a sensor_msgs/LaserScan instance'
00039 rospy.logerr(err)
00040 rospy.ROSException(err)
00041 str_scan = self._to_cpp(scan)
00042 str_crossing = self._detector.crossingDescriptor(str_scan, normalize)
00043 return self._from_cpp(str_crossing, Crossing)
00044
00045 def frontiers(self, scan, normalize=False):
00046 """Return a list of Frontier messages from analysis of a LaserScan
00047
00048 Return a list of lama_msgs/Frontier messages from analysis of a
00049 sensor_msgs/LaserScan.
00050
00051 Parameters
00052 ----------
00053 - scan: a sensor_msgs/LaserScan message
00054 - normalize: true if scan should be normalized in a first step.
00055 """
00056 if not isinstance(scan, LaserScan):
00057 err = 'Argument 1 is not a sensor_msgs/LaserScan instance'
00058 rospy.logerr(err)
00059 rospy.ROSException(err)
00060 str_scan = self._to_cpp(scan)
00061 str_frontiers = self._detector.frontiers(str_scan, normalize)
00062 frontiers = []
00063 for str_frontier in str_frontiers:
00064 frontiers.append(self._from_cpp(str_frontier, Frontier))
00065 return frontiers