Python LaserScan.scan_time方法代码示例

# 需要导入模块: from sensor_msgs.msg import LaserScan [as 别名]
# 或者: from sensor_msgs.msg.LaserScan import scan_time [as 别名]
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()

# 需要导入模块: from sensor_msgs.msg import LaserScan [as 别名]
# 或者: from sensor_msgs.msg.LaserScan import scan_time [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 scan_time [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 scan_time [as 别名]
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

# 需要导入模块: from sensor_msgs.msg import LaserScan [as 别名]
# 或者: from sensor_msgs.msg.LaserScan import scan_time [as 别名]
    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)

# 需要导入模块: from sensor_msgs.msg import LaserScan [as 别名]
# 或者: from sensor_msgs.msg.LaserScan import scan_time [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 scan_time [as 别名]
 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 )

# 需要导入模块: from sensor_msgs.msg import LaserScan [as 别名]
# 或者: from sensor_msgs.msg.LaserScan import scan_time [as 别名]
def lidar_listener():
	port = 5560
	kill = False
	
	#init
	rospy.loginfo("Initializing LIDAR Listener...");
	sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
	sock.bind(('', port))
	laserpub = rospy.Publisher('/scan', LaserScan)
	rospy.init_node('lidar_listener')
	
	message = LaserScan();
	message.angle_max = 4.1887
	message.angle_increment = .0062832
	message.scan_time = .1
	message.range_min = .02
	message.range_max = 4.0
	message.header.frame_id = "/base_laser"
	
	#Loop
	rospy.loginfo("LIDAR Listener initialized")
	while not rospy.is_shutdown():
		try:
			raw_data =sock.recv(4096, socket.MSG_DONTWAIT)
			message.header.stamp=rospy.Time.now()
			if not raw_data:
				rospy.logwarn("No Raw Data")
			else:
				message.ranges=decode(raw_data)
				print message.ranges
				rospy.logdebug(message.ranges)
				laserpub.publish(message)
		except socket.error as ex:
			if (ex[0] != 11):
				rospy.logwarn( "Lidar Listener Socket Exception: ",ex)
			else:
				rospy.logdebug("Lidar Listener Timed Out");
		except Exception as ex:
			raise ex
	rospy.loginfo("Shutting down LIDAR listener...")
	sock.close()		

# 需要导入模块: from sensor_msgs.msg import LaserScan [as 别名]
# 或者: from sensor_msgs.msg.LaserScan import scan_time [as 别名]
def imu_serial():
    pub = rospy.Publisher('laser_scan', LaserScan)
    rospy.init_node('imu_serial')
    laser_msg = LaserScan()

    laser_msg.header.frame_id = 'laser'
    laser_msg.angle_min = -1.5
    laser_msg.angle_max = 1.5
    laser_msg.angle_increment = 0.1
    laser_msg.time_increment = 0.1
    laser_msg.scan_time = 0.1
    laser_msg.range_min = 0.5
    laser_msg.range_max = 1.5    
    laser_msg.ranges = [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 9.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 0.1, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 5.0, 1.0]
    laser_msg.intensities = laser_msg.ranges 

    r = rospy.Rate(100) # 10hz
    while not rospy.is_shutdown():
        laser_msg.header.stamp = rospy.get_rostime()
        pub.publish(laser_msg)
        r.sleep()

# 需要导入模块: from sensor_msgs.msg import LaserScan [as 别名]
# 或者: from sensor_msgs.msg.LaserScan import scan_time [as 别名]
def build_constant_scan(
        range_val, intensity_val,
        angle_min, angle_max, angle_increment, scan_time):
    count = np.uint(np.ceil((angle_max - angle_min) / angle_increment))
    if count < 0:
        raise BuildScanException

    scan = LaserScan()
    scan.header.stamp = rospy.rostime.Time.from_sec(10.10)
    scan.header.frame_id = "laser_frame"
    scan.angle_min = angle_min
    scan.angle_max = angle_max
    scan.angle_increment = angle_increment
    scan.scan_time = scan_time.to_sec()
    scan.range_min = PROJECTION_TEST_RANGE_MIN
    scan.range_max = PROJECTION_TEST_RANGE_MAX
    scan.ranges = [range_val for _ in range(count)]
    scan.intensities = [intensity_val for _ in range(count)]
    scan.time_increment = scan_time.to_sec()/count

    return scan

# 需要导入模块: from sensor_msgs.msg import LaserScan [as 别名]
# 或者: from sensor_msgs.msg.LaserScan import scan_time [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)

# 需要导入模块: from sensor_msgs.msg import LaserScan [as 别名]
# 或者: from sensor_msgs.msg.LaserScan import scan_time [as 别名]
    def _msg(self, ranges, intensities, scan_time):
        new_time = Time.now()
        delta_time = new_time - self._last_time
        self._last_time = new_time

        msg = LaserScan()
        msg.header.frame_id = self._frame_id
        msg.header.stamp = Time.now()
        msg.angle_max = self.__MAX_ANGLE
        msg.angle_min = self.__MIN_ANGLE
        msg.angle_increment = self.__ANGLE_INCREMENT
        # Note: time_increment is the time between measurements, i.e. how often we read the scan and publish it (in
        # seconds)
        msg.time_increment = 0.1 #delta_time.secs
        # Note: scan_time is the time between scans of the laser, i.e., the time it takes to read 360 degrees.
        msg.scan_time = 0.1 # scan_time
        msg.range_min = float(self._min_range)
        msg.range_max = float(self._max_range)
        msg.ranges = [min(max(range, msg.range_min), msg.range_max) for range in ranges]
        msg.intensities = intensities

        return msg

# 需要导入模块: from sensor_msgs.msg import LaserScan [as 别名]
# 或者: from sensor_msgs.msg.LaserScan import scan_time [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 scan_time [as 别名]
def create_lidar_msg(L):
    
    raw_lidar = L.data
    stripped_lidar = raw_lidar.translate(None, '[]').translate(None, '"').translate(None, '\'')
    array_lidar = stripped_lidar.split(",")

    num_readings = 1440
    scan = LaserScan() 
    scan.header.stamp = rospy.Time.now()        
    scan.header.frame_id = "base_scan"
    scan.angle_min = math.radians(float(array_lidar[0]))
    scan.angle_max = math.radians(float(array_lidar[1]))
    scan.angle_increment = math.radians(.25) #get from lidar
    scan.time_increment = float(25. / num_readings) #time in ms / measurements
    scan.scan_time = float(array_lidar[2])
    scan.range_min = float(array_lidar[4]) / 1000 #sent in mm, needs m
    scan.range_max = float(array_lidar[5]) / 1000 #sent in mm, needs m
#    string_array = array_lidar[3].strip("[").strip("]").split(",")
    array_string = array_lidar[3].translate(None, '[]')
    string_array = array_string.split(",")
    scan.ranges = [float(r) / 1000 for r in string_array] #better way?

    scanPub.publish(scan)

# 需要导入模块: from sensor_msgs.msg import LaserScan [as 别名]
# 或者: from sensor_msgs.msg.LaserScan import scan_time [as 别名]
def sonar():
    global port
    rospy.init_node('Sonar_Array')
    rospy.loginfo( "Sonar Array serial connection is running on " + port)
    pub = rospy.Publisher('sonar_data',LaserScan)
    sequence = 0
    while not rospy.is_shutdown():
        try:
            with serial.Serial(port = port, baudrate = 115200) as ser:
                while not rospy.is_shutdown():
                    if VERBOSE:
                        print "Starting..."
                    ser.flushInput()
                    line = ser.readline()
                    if VERBOSE:
                        print "Got line! ", line
                    tokens = line.split()
                    if VERBOSE:
                        print "Number of tokens: ", len(tokens)
                    if len(tokens) is 13:
                        if VERBOSE:
                            print '-'+tokens[0]+'-'
                        if(tokens[0] == 'Sonars:'):
                            p = LaserScan()
                            p.header.stamp = rospy.get_rostime()
                            p.header.frame_id = 'sonar_array_frame'
                            p.header.seq = sequence
                            p.angle_min = -pi/2
                            p.angle_max = pi/2
                            p.angle_increment = pi/11
                            p.scan_time = .1
                            p.ranges = [Process(i) for i in tokens[1:13]]
                            p.ranges.reverse()
                            pub.publish(p)
        except IOError:
            print "Disconnected? (Error)"
        time.sleep(1)