Python LaserScan.header方法代码示例

# 需要导入模块: from sensor_msgs.msg import LaserScan [as 别名]
# 或者: from sensor_msgs.msg.LaserScan import header [as 别名]
    def _processLaserscan(self, readingType, remappedTimestamp, content):
        # FIXME: For type ROBOTLASER, we should publish the robot/sensor pose using TF and use the correct TF frame
        laserscan = LaserScan()
        laserscan.header = Header(stamp=remappedTimestamp, frame_id="odom", seq=self._laserscanCounter)

        if readingType.startswith("RAWLASER") or readingType.startswith("ROBOTLASER"):
            laserscan.angle_min = float(content[1])
            laserscan.angle_max = laserscan.angle_min + float(content[2])
            laserscan.angle_increment = float(content[3])
            laserscan.time_increment = 0
            laserscan.scan_time = 0.0  # FIXME
            laserscan.range_min = 0
            laserscan.range_max = float(content[4])

            numRanges = int(content[7])
            for i in xrange(0, numRanges):
                laserscan.ranges.append( float(content[8 + i]) )

            numRemissions = int(content[8 + numRanges])
            for i in xrange(0, numRemissions):
                laserscan.intensities.append( float(content[9 + numRanges + i]) )

        else:
            rospy.logwarn("Unsupported laser of type %s in line %d" % (readingType, self._lineCounter) )

        publisher = self._laserscanPublishers[ self._getLaserID(readingType, content) ]
        publisher.publish(laserscan)
        self._laserscanCounter += 1

# 需要导入模块: from sensor_msgs.msg import LaserScan [as 别名]
# 或者: from sensor_msgs.msg.LaserScan import header [as 别名]
    def create_lidar_msg(lidar_string):
        lidar_msg = LaserScan()    
        data = lidar_string.split(";")
        #num_readings = 1440 --------------------------------
        #data[0] = min angle (degrees)
        #data[1] = max angle (degrees)
        #data[2] = timestep (ms)
        #data[3] = lidar scan array
        #data[4] = min range
        #data[5] = max range	

        #print data

        lidar_msg.header = create_header() #self?
        lidar_msg.angle_min = math.radians(float(data[0]))
        lidar_msg.angle_max = math.radians(float(data[1]))
        lidar_msg.angle_increment = math.radians(.25) #get from lidar
        lidar_msg.time_increment = float(25. / self.num_readings) #time in ms / measurements YOYOYOYO CHECK THIS
        lidar_msg.scan_time = float(data[2])
        lidar_msg.range_min = float(data[4]) / 1000 #sent in mm, should be meters
        lidar_msg.range_max = float(data[5]) / 1000 #sent in mm, should be meters


        array_string = data[3].translate(None, '[]')
        string_array = array_string.split(",")
        lidar_msg.ranges = [float(r) / 1000 for r in string_array] #better way?
    #    string_array = data[3].strip("[").strip("]").split(",")
        # string_array = data[3].split(",")
        # try:
        #     lidar_msg.ranges = [float(r) for r in string_array]
        #     lidar_msg.intensities = []
        # except ValueError:
        #     print "range vals failed"

        return lidar_msg

# 需要导入模块: from sensor_msgs.msg import LaserScan [as 别名]
# 或者: from sensor_msgs.msg.LaserScan import header [as 别名]
 def publish_scan(self, angles, ranges):
     ls = LaserScan()
     ls.header = Utils.make_header("laser", stamp=self.last_stamp)
     ls.angle_min = np.min(angles)
     ls.angle_max = np.max(angles)
     ls.angle_increment = np.abs(angles[0] - angles[1])
     ls.range_min = 0
     ls.range_max = np.max(ranges)
     ls.ranges = ranges
     self.pub_fake_scan.publish(ls)

# 需要导入模块: from sensor_msgs.msg import LaserScan [as 别名]
# 或者: from sensor_msgs.msg.LaserScan import header [as 别名]
def create_laser_msg(range_data_array):
    ls = LaserScan()
    ls.angle_increment = 0.006283185307179586 # 0.36 deg
    ls.angle_max = 2.0943951023931953 # 120.0 deg
    ls.angle_min = -2.0943951023931953 # -120.0 deg
    ls.range_max = 4.0
    ls.range_min = 0.02
    ls.scan_time = 0.001 # No idea
    ls.time_increment = 1.73611115315e-05 # No idea, took from http://comments.gmane.org/gmane.science.robotics.ros.user/5192
    ls.header = Header()
    ls.header.frame_id = 'laser_link'
    ls.ranges = range_data_array
    return ls

# 需要导入模块: from sensor_msgs.msg import LaserScan [as 别名]
# 或者: from sensor_msgs.msg.LaserScan import header [as 别名]
    def convert_array_to_laser_scan(self, vision_raw_scan):

        if vision_raw_scan.size < 100:
            return None

        header = Header()
        header.frame_id = "vision_scan"
        #header.stamp = time()

        laser_scan = LaserScan()
        laser_scan.angle_min = 0.0
        laser_scan.angle_max = self.angle_max
        laser_scan.angle_increment = self.angle_increment
        laser_scan.range_min = 0.0
        laser_scan.range_max = self.range_max
        #laser_scan.ranges = [0]*360

        image_size = vision_raw_scan.shape

        if len(image_size) == 3:
            vision_raw_scan = cv2.cvtColor(vision_raw_scan, cv2.COLOR_BGR2GRAY)
            image_size = vision_raw_scan.shape

        if self.init is False:
            self._init_lines(image_size)
            self.init = True


        tasks = list()
        for line in range(self.number_lines):
            tasks.append((vision_raw_scan, self.lines[line]))

        laser_scan.ranges = self.pool.map(_getObstacle, tasks)

        #pool.close()
        laser_scan.header = header
        #laser_scan.scan_time = 1.0/5.0
        #laser_scan.time_increment = 1.0/5.0
        return laser_scan

# 需要导入模块: from sensor_msgs.msg import LaserScan [as 别名]
# 或者: from sensor_msgs.msg.LaserScan import header [as 别名]
    def create_lidar_msg(self, L):
 
        raw_lidar = L.data
        #stripped_lidar = raw_lidar.translate(None, '[]').translate(None, '"').translate(None, '\'')
        array_lidar = raw_lidar.split(";")
        
        lidar_msg = LaserScan()
        lidar_msg.header = self.create_header() #self?
        lidar_msg.angle_min = math.radians(float(array_lidar[0]))
        lidar_msg.angle_max = math.radians(float(array_lidar[1]))
        lidar_msg.angle_increment = math.radians(0.25) #MAKE PARAM
        lidar_msg.time_increment = 0.025/(270*4) #time in ms / measurements YOYOYOYO CHECK THIS
        lidar_msg.scan_time = float(array_lidar[2]) / 1000 #time in ms
        lidar_msg.range_min = float(array_lidar[4]) / 1000 #sent in mm, should be meters
        lidar_msg.range_max = float(array_lidar[5]) / 1000 #sent in mm, should be meters


        array_string = array_lidar[3]   #.translate(None, '[]')
        string_array = array_string.split(",")
        lidar_msg.ranges = [float(r) / 1000 for r in string_array] #better way?

        self.scanPub.publish(lidar_msg)

# 需要导入模块: from sensor_msgs.msg import LaserScan [as 别名]
# 或者: from sensor_msgs.msg.LaserScan import header [as 别名]
    def default(self, ci='unused'):
        laserscan = LaserScan()
        laserscan.header = self.get_ros_header()

        # Note: Scan time and laser frequency are chosen as standard values
        laser_frequency = 40 # TODO ? component_instance.frequency()
        scan_window = self.component_instance.bge_object['scan_window']
        num_readings = scan_window / self.component_instance.bge_object['resolution']

        laserscan.angle_max = scan_window * math.pi / 360
        laserscan.angle_min = laserscan.angle_max * -1
        laserscan.angle_increment = scan_window / num_readings * math.pi / 180
        laserscan.time_increment = 1 / laser_frequency / num_readings
        laserscan.scan_time = 1.0
        laserscan.range_min = 0.3
        laserscan.range_max = self.component_instance.bge_object['laser_range']
        # ROS expect the ranges to be sorted clockwise.
        # see morse.builder.sensor.LaserSensorWithArc.create_laser_arc
        # where we create the ray from -window / 2.0 to +window / 2.0
        laserscan.ranges = self.data['range_list']

        self.publish(laserscan)