Python msg.LaserScan类代码示例

def merge_scans(rf, sg):
    rf.ranges = list(rf.ranges)
    for i in range(40):
        rf.ranges[len(rf.ranges)-i-1] = 0

    if not sg:
        rf.header.frame_id = 'laser'
        return rf
    else:
        global angle_min
        global angle_max
        global angle_increment
        global last_scan_time
        if not last_scan_time:
            last_scan_time = time.time()

        scan = LaserScan()
        scan.header.frame_id = 'laser'
        scan.header.stamp = get_most_recent_timestamp(rf, sg)    
        scan.angle_min = angle_min
        scan.angle_max = angle_max
        scan.angle_increment = angle_increment
        scan.scan_time = time.time() - last_scan_time
        scan.time_increment = scan.scan_time / 541
        scan.range_min = rf.range_min
        scan.range_max = rf.range_max
        scan.ranges = rf.ranges
        for i in range(180*2):
            if sg.ranges[i] < scan.ranges[90 + i] or scan.ranges[90 + i] == 0:
                scan.ranges[90 + i] = sg.ranges[i]

    return scan

    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

def callback(data):
    n = int(rospy.get_param('~reduction', '2'))
    pub=rospy.Publisher('/scan_reduced', LaserScan)
    newScan=LaserScan()
    newScan=data
    newScan.angle_increment=n*data.angle_increment
    newScan.ranges=data.ranges[1::n]    
    pub.publish(newScan)

	def sonarsCallback(self, event):
		sonars = self.rh.sensors.getSonarsMeasurements()['sonars']
		laser_msg = LaserScan()
		laser_msg.ranges.append(sonars['front_right'])
		laser_msg.ranges.append(sonars['front_left'])
		laser_msg.range_max = 1.00
		laser_msg.angle_increment = 0.785398185253
		laser_msg.angle_min = -0.392699092627
		self.pub.publish(laser_msg)

    def update(self):
        #############################################################################
        now = rospy.Time.now()
        if now > self.t_next:
            elapsed = now - self.then
            self.then = now
            elapsed = elapsed.to_sec()

            # this approximation works (in radians) for small angles
            th = self.th - self.th_pre

            self.dr = th / elapsed

            # publish the odom information
            quaternion = Quaternion()
            quaternion.x = self.qx
            quaternion.y = self.qy
            quaternion.z = self.qz
            quaternion.w = self.qw
            self.odomBroadcaster.sendTransform(
                (self.x, self.y, 0),
                (0, 0, quaternion.z, quaternion.w),
                rospy.Time.now(),
                self.base_frame_id,
                self.odom_frame_id,
            )

            self.laserBroadcaster.sendTransform(
                (0, 0, 0), (0, 0, 0, 1), rospy.Time.now(), self.laser_frame_id, self.base_frame_id
            )

            odom = Odometry()
            odom.header.stamp = now
            odom.header.frame_id = self.odom_frame_id
            odom.pose.pose.position.x = self.x
            odom.pose.pose.position.y = self.y
            odom.pose.pose.position.z = 0
            odom.pose.pose.orientation = quaternion
            odom.child_frame_id = self.base_frame_id
            odom.twist.twist.linear.x = self.dx
            odom.twist.twist.linear.y = 0
            odom.twist.twist.angular.z = self.dr
            self.odomPub.publish(odom)
            laser = LaserScan()
            laser.header.stamp = now
            laser.header.frame_id = self.laser_frame_id
            laser.angle_min = self.laser.angle_min
            laser.angle_max = self.laser.angle_max
            laser.angle_increment = self.laser.angle_increment
            laser.time_increment = self.laser.time_increment
            laser.scan_time = self.laser.scan_time
            laser.range_min = self.laser.range_min
            laser.range_max = self.laser.range_max
            laser.ranges = self.laser.ranges
            laser.intensities = self.laser.intensities
            self.laserPub.publish(laser)

def getLaserScan(frame_id, laser_scan_line):
    # Timestamp [seconds.microseconds]
    # # of ranges [unitless]
    # Angular offset [1/4 degree]
    # R1..R181 Ranges (zero padded to 181 ranges) [m]
    #
    # 1235603336.30835, 181, 0, 11.360, 11.360, 11.390, 11.410, 81.910, 81.910, 11.380, 11.400, 11.430, 6.450, 6.170, 6.030, 5.880, 5.740, 5.600, 5.470, 5.360, 5.370, 5.390, 5.430, 5.470, 5.500, 5.530, 5.580, 5.610, 5.410, 5.230, 5.130, 5.180, 5.230, 5.280, 5.350, 6.040, 6.110, 6.180, 6.250, 6.330, 6.400, 6.490, 5.950, 5.750, 5.640, 5.520, 5.440, 5.330, 5.220, 5.280, 5.040, 5.490, 5.590, 5.690, 5.810, 5.930, 6.080, 6.210, 6.360, 6.530, 6.690, 6.870, 13.930, 13.770, 13.650, 13.650, 13.530, 13.430, 13.300, 13.190, 13.040, 12.870, 12.780, 12.700, 12.630, 12.550, 12.480, 12.410, 12.360, 12.310, 12.240, 12.200, 12.150, 12.110, 12.070, 12.040, 12.010, 11.990, 11.970, 11.560, 11.930, 11.920, 11.920, 11.910, 11.930, 11.920, 11.920, 11.940, 11.930, 12.830, 12.840, 12.300, 12.130, 12.120, 13.000, 12.250, 12.230, 12.270, 12.330, 12.390, 12.440, 12.520, 12.580, 12.810, 13.640, 13.740, 13.830, 13.940, 13.640, 6.410, 6.220, 6.010, 5.810, 5.640, 5.080, 4.180, 4.090, 4.250, 4.070, 4.050, 3.700, 3.560, 3.510, 3.510, 3.570, 3.430, 3.520, 3.590, 4.940, 4.650, 4.630, 5.050, 5.040, 5.080, 4.890, 2.790, 2.710, 2.660, 2.620, 2.590, 2.600, 2.660, 2.650, 2.630, 2.690, 2.790, 2.900, 4.250, 4.150, 2.510, 2.480, 2.390, 2.360, 2.330, 2.320, 2.300, 2.410, 2.270, 3.930, 2.290, 2.390, 3.850, 3.830, 3.830, 3.710, 4.060, 4.050, 4.040, 4.030, 4.020, 4.010, 4.010, 4.010, 4.010
    str_timestamp, str_num_readings, str_angular_offset, str_ranges = laser_scan_line.split(', ', 3)
    num_readings = int(str_num_readings)
    angular_offset = float(str_angular_offset)
    ranges = map(float, str_ranges.split(', ')) #convert array of readings
    laser_frequency = 50
    angle_range_rad = 180 * np.pi / 180

    #populate the LaserScan message
    msg = LaserScan()
    msg.header.stamp = rospy.Time.now()
    msg.header.frame_id = frame_id
    msg.angle_min = - (angle_range_rad / 2)
    msg.angle_max = (angle_range_rad / 2)
    msg.angle_increment = angle_range_rad / num_readings
    msg.time_increment = (1 / laser_frequency) / (num_readings)
    msg.range_min = 0.0
    msg.range_max = 40.0
    msg.ranges = ranges
    msg.intensities = [0.0] * len(ranges)

    return msg

    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

def checker(fake_laser_param, realtime_lasers, nonrealtime_lasers):
    r = rospy.Rate(RATE)
    seq = 0
    laser_scan = LaserScan()
    laser_scan.header.seq = seq
    laser_scan.header.frame_id = fake_laser_param['frame_name']
    laser_scan.angle_min = fake_laser_param['angle_min']
    laser_scan.angle_max = fake_laser_param['angle_max']
    laser_scan.angle_increment = fake_laser_param['angle_increment']
    laser_scan.range_min = fake_laser_param['range_min']
    laser_scan.range_max = fake_laser_param['range_max']
    laser_scan.scan_time = 0
    laser_scan.time_increment = 0
    pub = rospy.Publisher('/scan', LaserScan, queue_size=10)
    while not rospy.is_shutdown():
        fake_laser_data = realtime_lasers[0].get_range_data()
        for laser_scanner in realtime_lasers[1:]:
            new_laser_data = laser_scanner.get_range_data()
            fake_laser_data = [min(r1, r2) for r1, r2 in zip(fake_laser_data, new_laser_data)]
        for laser_scanner in nonrealtime_lasers:
            laser_data = laser_scanner.get_range_data()
            #fake_laser_data = [r1 if r1 < 1000 else min(r1, r2) for r1, r2 in zip(fake_laser_data, laser_data)]
            fake_laser_data = [min(r1, r2) for r1, r2 in zip(fake_laser_data, laser_data)]
        laser_scan.ranges = fake_laser_data
        laser_scan.header.stamp = rospy.Time.now()
        pub.publish(laser_scan)
        seq += 1
        r.sleep()

    def prepare_laserscan_msg(self):
        '''
        Fill laser scan message
        '''
        laser_scan_msg = LaserScan()

        #Step 1: 
        num_readings = 100
        laser_frequency = 40
        ranges = []
        intensities = []
        count = 0
        i = 0
        
        #generate some fake data for laser scan
        while (i < num_readings):
            ranges.append(count)
            intensities.append(4 + count)
            i = i + 1

        #Step 2
        now = rospy.get_rostime()
        laser_scan_msg.header.stamp = now
        laser_scan_msg.header.frame_id = "laser_frame"
        laser_scan_msg.angle_min = -1.57
        laser_scan_msg.angle_max = 1.57
        laser_scan_msg.angle_increment = 3.14 / num_readings
        laser_scan_msg.time_increment = (1 / laser_frequency) / (num_readings)
        laser_scan_msg.range_min = 0.0
        laser_scan_msg.range_max = 4.0

        laser_scan_msg.ranges = ranges
        laser_scan_msg.intensities = intensities

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

 def inputCallback(self, msg):
 #########################################################################
     # rospy.loginfo("-D- range_filter inputCallback")
     cur_val = msg.value
 
     if cur_val <= self.max_valid and cur_val >= self.min_valid:
         self.prev.append(cur_val)
         del self.prev[0]
     
         p = array(self.prev)
         self.rolling_ave = p.mean()
         self.rolling_std = p.std()
     
         self.rolling_meters = ((self.b * self.rolling_ave) ** self.m) / 100
     
         self.filtered_pub.publish( self.rolling_meters )
         self.std_pub.publish( self.rolling_std )
         
         rng = Range()
         rng.radiation_type = 1
         rng.min_range = self.min_range
         rng.max_range = self.max_range
         rng.range = self.rolling_meters
         rng.header.frame_id = self.frame
         rng.field_of_view = 0.1
         rng.header.stamp = rospy.Time.now()
         
         self.range_pub.publish( rng )
        
         ranges = [] 
         intensities = []
         angle_start = 0.0 - self.field_of_view
         angle_stop = self.field_of_view
         for angle in linspace( angle_start, angle_stop, 10):
             ranges.append( self.rolling_meters )
             intensities.append( 1.0 )
         scan = LaserScan()
         scan.ranges = ranges
         scan.header.frame_id = self.frame
         scan.time_increment = 0;
         scan.range_min = self.min_range
         scan.range_max = self.max_range
         scan.angle_min = angle_start
         scan.angle_max = angle_stop
         scan.angle_increment = (angle_stop - angle_start) / 10
         scan.intensities = intensities
         scan.scan_time = self.scan_time
         scan.header.stamp = rospy.Time.now()
         self.scan_pub.publish( scan )

 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)

    def handle_scanner_msg(self, args):
        # Broadcast scanner transform first
        time_now = rospy.Time.now()        
        pos = self.laser_tf.transform.translation
        rot = self.laser_tf.transform.rotation        
        self.tf_broadcaster.sendTransform((pos.x, pos.y, pos.z),
                                      (rot.x, rot.y, rot.z, rot.w),
                                      time_now,
                                      self.laser_tf.child_frame_id,
                                      self.laser_tf.header.frame_id)
        
        scan = LaserScan()
        scan.header.stamp = time_now
        scan.header.frame_id = self.base_laser_frame                            
        for (name, par) in args.items():
            if name == 'Range':
                scan.ranges = [float(s) for s in par.split(',')]
            if name == 'FOV':
                fov = float(par)
                scan.angle_min = -fov/2.0
                scan.angle_max = fov/2.0
            if name == 'Resolution':
                scan.angle_increment = float(par)
        scan.range_min = 0.0
        scan.range_max = 20.0

        self.scan_pub.publish(scan)

    def Cb(self, msg):
        n_ranges = list()
        pv = msg.data[0]
        for i in range(50):
            if pv>0.0 and msg.data[i]>0.0:
                n_ranges.append((pv+msg.data[i])/2)
            else:
                n_ranges.append(0.0)
            n_ranges.append(msg.data[i])
            pv = msg.data[i]

        laserMsg = LaserScan()
        laserMsg.ranges = n_ranges
        laserMsg.angle_increment = 0.2; 
        laserMsg.time_increment = 1/50
        laserMsg.header.frame_id = '/ultra'
        laserMsg.header.stamp = rospy.Time.now()
        self.scanPub.publish(laserMsg)

 def publish_laser_msg(self, ranges):
     msg = LaserScan()
     msg.angle_min = self.robot.laser.min_angle
     msg.angle_max = self.robot.laser.max_angle
     msg.angle_increment = self.robot.laser.resolution
     msg.range_min = 0.0
     msg.range_max = self.robot.laser.range
     msg.ranges = ranges
     msg.header.stamp = rospy.Time.now()
     self.laser_publisher.publish(msg)