コード例 #1
0
ファイル: diffdrive.py プロジェクト: dmiklic/usarros
    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)
コード例 #2
0
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()
コード例 #3
0
    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
コード例 #4
0
ファイル: laser_scan_merger.py プロジェクト: clubcapra/Ibex
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
コード例 #5
0
ファイル: lidar_pub_udp.py プロジェクト: mattjrush/tankbot
    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
コード例 #6
0
    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
コード例 #7
0
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
コード例 #8
0
ファイル: nao_interface.py プロジェクト: aspa1/thesis
	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)
コード例 #9
0
ファイル: laser.py プロジェクト: kochigami/pepper_robot
 def createLaserMessage(self, frameID, keyPrefix, scanNum):
     laserScanMsg = LaserScan()
     laserScanMsg.header.frame_id = frameID
     laserScanMsg.angle_min = self.PEPPER_LASER_MIN_ANGLE
     laserScanMsg.angle_max = self.PEPPER_LASER_MAX_ANGLE
     laserScanMsg.angle_increment = self.PEPPER_LASER_FOV/scanNum
     laserScanMsg.range_min = self.PEPPER_LASER_MIN_RANGE
     laserScanMsg.range_max = self.PEPPER_LASER_MAX_RANGE
     return laserScanMsg
コード例 #10
0
ファイル: dummy.py プロジェクト: rqou/prlite-pc
    def spin(self):        
        encoders = [0,0]

        self.x = 0                  # position in xy plane
        self.y = 0
        self.th = 0
        then = rospy.Time.now()

        # things that don't ever change
        scan_link = rospy.get_param('~frame_id','neato_link')
        scan = LaserScan(header=rospy.Header(frame_id=scan_link)) 
        scan.angle_min = 0
        scan.angle_max = 6.26
        scan.angle_increment = 0.017437326
        scan.range_min = 0.020
        scan.range_max = 5.0

        # The LiDAR spins counterclockwise at 10 revolutions per second.
        # Each revolution yields 90 packets.
        # Each packet contains 22 bytes.
        # Within these 22 bytes are 4 distance measurements and more
        # (4 data points/packet * 90 packets = 360 data points). 
        # So there is one data measurement per degree turn.
        # Byte 01, "FA" is a starting byte which appears between the ending 
        # and beginning of two packets.
        # Byte 02 is a hex value from A0-F9, representing the 90 packets 
        # outputted per revolution.
        # Byte 03 and 04 are a 16bit (combined) value representing the speed 
        # at which the LiDAR is rotating.
        # Bytes 06 and 05 are 1st  distance measurement in this packet.
        # Bytes 10 and 09 are 2nd  distance measurement in this packet.
        # Bytes 14 and 13 are 3rd  distance measurement in this packet.
        # Bytes 18 and 17 are the 4th distance measurement in this packet. 
        # Bytes 08:07, 12:11, 16:15, and 20:19 represent information about 
        # the surface off of which the laser has bounced to be detected by 
        # the LiDAR.
        # Bytes 22 and 21 are checksum and are used for determining the 
        # validity of the received packet.
    
        # main loop of driver
        # r = rospy.Rate(10)
        rospy.loginfo("0")
        # requestScan()
        data = []
        i = 0
        while not rospy.is_shutdown():
            # string = self.port.readline()
            byte = self.port.read()
            b = ord(byte)
            data.append(b)
            i = i +1
            if i > 1000:
              for j in range(0,999):
                rospy.loginfo("%d", j);
                rospy.loginfo(": {:02X}".format(data[j]));
                i = 0
                data = []
コード例 #11
0
ファイル: process.py プロジェクト: gaojun4ever/mobile_robot
    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)
コード例 #12
0
ファイル: controller.py プロジェクト: kam3k/MSL-Simulator
 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)
コード例 #13
0
ファイル: particle_filter.py プロジェクト: mikemwang/obstacle
 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)
コード例 #14
0
	def make_wallscan(self, data):
		num_readings = len(data)
		wall_scan = LaserScan()
		wall_scan.header.frame_id = "base_laser_link"
		wall_scan.ranges = data
		wall_scan.angle_min = -3.14;
		wall_scan.angle_max = 3.14;
		wall_scan.angle_increment = (3.14*2) / num_readings;
		wall_scan.range_min = 0.0;
		wall_scan.range_max = 5;

		return wall_scan
コード例 #15
0
ファイル: localization.py プロジェクト: lsouchet/naoqi_bridge
 def build_laser_scan(self, ranges):
     result = LaserScan()
     result.header.stamp = rospy.Time.now()
     result.angle_min = -almath.PI
     result.angle_max = almath.PI
     if len(ranges[1]) > 0:
         result.angle_increment = (result.angle_max - result.angle_min) / len(ranges[1])
     result.range_min = 0.0
     result.range_max = ranges[0]
     for range_it in ranges[1]:
         result.ranges.append(range_it[1])
     return result
コード例 #16
0
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
コード例 #17
0
ファイル: int2range.py プロジェクト: jfstepha/yertle-bot
 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 )
コード例 #18
0
ファイル: akai_setup.py プロジェクト: k29645806/robot
 def publishLaserScan(self,data=None):
     scan = LaserScan()
     scan.header.seq = 1
     scan.header.stamp = rospy.Time.now()
     num_readings = 100
     laser_frequency = 40
     scan.header.frame_id = "base_laser"
     scan.angle_min = radians(-30)
     scan.angle_max = radians(30)
     scan.angle_increment = radians(60) / num_readings
     scan.time_increment = (1 / laser_frequency) / (num_readings)
     scan.range_min = 0.5
     scan.range_max = 6
     scan.ranges = [5]*num_readings
     self.laserScanPublisher.publish(scan)
コード例 #19
0
ファイル: tf_broadcster.py プロジェクト: ResByte/graph_slam
def handle_laser_scan(laserMsg,currTime):
	scan = LaserScan()
	scan.header.stamp = currTime
	scan.header.frame_id = "base_link"
	scan.angle_min = -1.57
	scan.angle_max = 1.57
	scan.angle_increment = 3.14/181
	scan.time_increment = (1/0.5)/181
	scan.range_min = 0.0
	scan.range_max = 10.0
	scan.ranges = [0.0 for i in xrange(len(laserMsg)-1)]
	scan.intensities = [0.0 for i in xrange(len(laserMsg)-1)]
	for i in xrange(len(laserMsg)-1):
		scan.ranges[i] =laserMsg[i]
		scan.intensities[i]= 100
	laserPub =  rospy.Publisher('scan',LaserScan,queue_size = 1)
	laserPub.publish(scan)
	rospy.sleep(4.0)
コード例 #20
0
ファイル: co_to_scan.py プロジェクト: BOTSlab/bupimo_src
def co_callback(coa): # coa = CastObstacleArray

    scan = LaserScan()
    scan.header.frame_id = '/base_link'
    scan.angle_min = 0

    n = len(co_msg.obstacles)
    scan.angle_min = coa.obstacles[0].theta
    scan.angle_max = coa.obstacles[n-1].theta
    scan.angle_increment = coa.obstacles[1].theta - coa.obstacles[0].theta

    for obs in coa.obstacles:
        distance = rho2range(obs.rho)
        scan.ranges.append(distance)
        scan.range_min = min(scan.range_min, distance)
        scan.range_max = max(scan.range_min, distance)

    scan_publisher.publish(scan)
コード例 #21
0
ファイル: shared_control.py プロジェクト: aladds/recycle-bot
    def publishZoneScores(self,data, prh_resolution):
        """
        Publishes the zone scores
        """
        pc = LaserScan()
        pc.header.frame_id = "/base_link"
        pc.header.stamp = rospy.get_rostime()

        pc.angle_min = -np.pi
        pc.angle_max = np.pi
        pc.angle_increment = prh_resolution
        pc.range_min = 0.00
        pc.range_max = 5.0

        for r in data:
            pc.ranges.append(r)

        self.zone_score_pub.publish(pc)
コード例 #22
0
ファイル: shared_control.py プロジェクト: aladds/recycle-bot
    def publishPolarHistogram(self):
        """
        Publishes the polar histogram
        """
        pc = LaserScan()
        pc.header.frame_id = "/base_link"
        pc.header.stamp = rospy.get_rostime()

        pc.angle_min = -np.pi
        pc.angle_max = np.pi
        pc.angle_increment = self.prh_resolution
        pc.range_min = 0.00
        pc.range_max = 5.0

        self.polar_range_hist_lock.acquire()
        for r in self.polar_range_hist:
            pc.ranges.append(r)

        self.polar_range_hist_lock.release()
        self.polar_hist_pub.publish(pc)
コード例 #23
0
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()		
コード例 #24
0
ファイル: fake_laser.py プロジェクト: rqou/prlite-pc
    def __init__(self):
        rospy.init_node('fake_laser')

        # parameters
        self.rate = rospy.get_param("~rate", 1.0)
        self.frame_id = rospy.get_param("~frame", "base_laser")
        self.range_min = rospy.get_param("~range_min", 0.2)
        self.range_max = rospy.get_param("~range_max", 5.5)
        self.angle_min = rospy.get_param("~angle_min", -1.57)
        self.angle_max = rospy.get_param("~angle_max", 1.57)
        self.num_readings = rospy.get_param("~num_readings", 100)
        self.fixed_reading = rospy.get_param("~fixed_reading", 2.0)
        self.scan_time = rospy.get_param("~scan_time", 0.1)
        
        self.angle_increment = (self.angle_max - self.angle_min) / self.num_readings
        self.time_increment = self.scan_time / (self.num_readings)

        self.scanPub = rospy.Publisher('scan', LaserScan)

        rospy.loginfo("Started fake laser node publishing to /scan topic.")

        r = rospy.Rate(self.rate)
        
        while not rospy.is_shutdown():
            ranges = list()
            # Generate the fake data.
            for i in range(self.num_readings):
                ranges.append(self.fixed_reading * sin(i) * random.uniform(0, 1))
                
            scan = LaserScan()
            scan.header.stamp = rospy.Time.now()     
            scan.header.frame_id = self.frame_id
            scan.angle_min = self.angle_min
            scan.angle_max = self.angle_max
            scan.angle_increment = self.angle_increment
            scan.time_increment = self.time_increment
            scan.range_min = self.range_min
            scan.range_max = self.range_max
            scan.ranges = ranges    
            self.scanPub.publish(scan)
            r.sleep()
コード例 #25
0
ファイル: imu_serial.py プロジェクト: Jae-hyun/imu_serial
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()
コード例 #26
0
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
コード例 #27
0
ファイル: fake_urg_node.py プロジェクト: prl-mushr/mushr_sim
    def timer_cb(self, event):

        now = rospy.Time.now()
        ls = LaserScan()
        ls.header.frame_id = self.TF_PREFIX + "laser_link"
        ls.header.stamp = now
        ls.angle_increment = self.ANGLE_STEP
        ls.angle_min = self.ANGLE_MIN
        ls.angle_max = self.ANGLE_MAX
        ls.range_min = self.MIN_RANGE_METERS
        ls.range_max = self.MAX_RANGE_METERS
        ls.intensities = []

        ranges = np.zeros(len(self.ANGLES) * 1, dtype=np.float32)

        try:
            base_to_map_trans, base_to_map_rot = self.tl.lookupTransform(
                "/map", self.TF_PREFIX + "base_link", rospy.Time(0))
        except Exception:
            return

        laser_quat = Quaternion()
        laser_quat.x = base_to_map_rot[0]
        laser_quat.y = base_to_map_rot[1]
        laser_quat.z = base_to_map_rot[2]
        laser_quat.w = base_to_map_rot[3]

        laser_angle = utils.quaternion_to_angle(laser_quat)
        laser_pose_x = base_to_map_trans[0] + self.x_offset * np.cos(
            laser_angle)
        laser_pose_y = base_to_map_trans[1] + self.x_offset * np.sin(
            laser_angle)

        range_pose = np.array((laser_pose_x, laser_pose_y, laser_angle),
                              dtype=np.float32).reshape(1, 3)
        self.range_method.calc_range_repeat_angles(range_pose, self.ANGLES,
                                                   ranges)
        self.noise_laser_scan(ranges)
        ls.ranges = ranges.tolist()
        self.laser_pub.publish(ls)
コード例 #28
0
def callback(sub, sub5):
    l = [sub.ranges, sub5.range]
    ir = sub5.range  # sub5.range is the point values of the IR sensor

    num_readings = 720
    laser_frequency = 20

    count = 0
    r = rospy.Rate(1.0)
    while not rospy.is_shutdown():
        current_time = rospy.Time.now()
        scan_pub = rospy.Publisher('uvbot_laser_scan', LaserScan,
                                   queue_size=0)  # my_laser_scan is my topic

        scan = LaserScan()
        scan.header.stamp = current_time
        scan.header.frame_id = 'hokuyo'
        scan.angle_min = -1.57
        scan.angle_max = 1.57
        scan.angle_increment = 3.14 / num_readings
        scan.time_increment = (1.0 / laser_frequency) / (num_readings)
        scan.range_min = 0.1
        scan.range_max = 10.0
        #l = []
        #for i in range(720):
        #l.append(sub.ranges[i])

        #scan.ranges = l

        scan.ranges = sub.ranges
        if sub5.range < 0.1:  # if obstacle from IR is less than 10 cm
            for i in range(320, 340):
                a = sub5.range  # set a variable, to the value of reading
                scan.ranges[
                    i] = a  # add the values to the scan.ranges, of my Laserscan topic
                scan.intensities = []

        if sub5.range > 0.1:
            scan.ranges = sub.ranges  # if IR data is more than 10 cm, let the scan.ranges(my laser scan topic) be equal to the actual Lidar scan
            scan.intensities = []
コード例 #29
0
    def _publish_laser_scan(self, pointcloud, startPoint):
        scan = LaserScan()
        scan.header.stamp = rospy.Time.now()
        scan.header.frame_id = 'laser_frame'
        scan.range_min = 0.0
        scan.range_max = self.range * self.scaling
        scan.angle_min = self.angle_min
        scan.angle_max = self.angle_max
        scan.angle_increment = self.angle_increment

        scan.ranges = []
        scan.intensities = []
        for i in range(0, len(pointcloud)):
            if pointcloud[i][2] == 1:
                dist = math.hypot(pointcloud[i][0] - startPoint[0],
                                  pointcloud[i][1] - startPoint[1])
                scan.ranges.append(self.scaling * dist)
                scan.intensities.append(pointcloud[i][2])
            else:
                scan.ranges.append(scan.range_max)
                scan.intensities.append(pointcloud[i][2])
        self.laser_scan_publisher.publish(scan)
コード例 #30
0
    def publish_filtered_laser_scan(self, laser_original_data,
                                    new_filtered_laser_range):

        rospy.logdebug("new_filtered_laser_range==>" +
                       str(new_filtered_laser_range))

        laser_filtered_object = LaserScan()

        h = Header()
        # Note you need to call rospy.init_node() before this will work
        h.stamp = rospy.Time.now()
        h.frame_id = laser_original_data.header.frame_id

        laser_filtered_object.header = h
        laser_filtered_object.angle_min = laser_original_data.angle_min
        laser_filtered_object.angle_max = laser_original_data.angle_max

        new_angle_incr = abs(laser_original_data.angle_max -
                             laser_original_data.angle_min) / len(
                                 new_filtered_laser_range)

        #laser_filtered_object.angle_increment = laser_original_data.angle_increment
        laser_filtered_object.angle_increment = new_angle_incr
        laser_filtered_object.time_increment = laser_original_data.time_increment
        laser_filtered_object.scan_time = laser_original_data.scan_time
        laser_filtered_object.range_min = laser_original_data.range_min
        laser_filtered_object.range_max = laser_original_data.range_max

        laser_filtered_object.ranges = []
        laser_filtered_object.intensities = []
        for item in new_filtered_laser_range:
            if item == 0.0:
                laser_distance = 0.1
            else:
                laser_distance = item
            laser_filtered_object.ranges.append(laser_distance)
            laser_filtered_object.intensities.append(item)

        self.laser_filtered_pub.publish(laser_filtered_object)
コード例 #31
0
 def cb():
     global last_time
     current_time = rospy.Time.now()
     ranges = []
     msg = LaserScan()  #Initialize blank message
     msg.header.stamp = rospy.Time.now()
     msg.header.frame_id = 'base_link'
     msg.angle_min = pi / 6.  #rough estimate for position of first sensor
     msg.angle_max = 13. * pi / 6.  #2pi+ where we started
     msg.angle_increment = pi / 3.  #6 sensors around robot. Change if configuration changes
     msg.time_increment = 0  #used for rotating scanners. this one is simultaneous
     msg.scan_time = (current_time - last_time).to_sec()  #current time
     msg.range_min = 0.004  #rough estimate on how far to the side sensors can detect
     msg.range_max = 0.05
     for s in sensors:  #read each sensor and add to result
         try:
             ranges.append(float(s.value) * 0.03)
         except:
             pass
     msg.ranges = ranges
     publisher.publish(msg)
     last_time = current_time
コード例 #32
0
ファイル: scan_sensor.py プロジェクト: tslator/arlobot_rpi
    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
コード例 #33
0
def generateScanMsg(ranges, intensities, sonarRange, step, maxAngle, minAngle):
    """
    Generates the laserScan message for the scan topic
    Args:
        angle (int): Gradian Angle
        data (list): List of intensities
        sonarRange (int)
        step (int)
     """
    msg = LaserScan()
    msg.header.stamp = rospy.Time.now()
    msg.header.frame_id = 'sonar_frame'
    msg.angle_min = 2 * pi * minAngle / 400
    msg.angle_max = 2 * pi * maxAngle / 400
    msg.angle_increment = 2 * pi * step / 400
    msg.time_increment = 0
    msg.range_min = .75
    msg.range_max = sonarRange
    msg.ranges = ranges
    msg.intensities = intensities

    return msg
コード例 #34
0
 def publish_laser_data(self, publisher):
     scan = LaserScan()
     scan.header.stamp = rospy.Time.now()
     scan.header.frame_id = "base_laser_link"
     scan.angle_min = -30 * math.pi / 180
     scan.angle_max = 30 * math.pi / 180
     scan.angle_increment = 1 * math.pi / 180 
     scan.range_min = 0.03 
     scan.range_max = 3.00
     scan.ranges = []
     # read the ranges from left to right
     for i in range(0,15):
         scan.ranges.append(self.ranges["ultrasonic_3"])
     for i in range(15,30):
         scan.ranges.append(self.ranges["ultrasonic_1"])
     for i in range(30,45):
         scan.ranges.append(self.ranges["ultrasonic_2"])
     for i in range(45,60):
         scan.ranges.append(self.ranges["ultrasonic_4"])
     publisher.publish(scan)
     # clear out the scans
     self.resetRanges()
コード例 #35
0
    def generate_laser_msg(self, laser_data):
        '''Generates a 'LaserScan' message from the input data.

        Keyword arguments:
        laser_data -- A 'LaserData' object.

        Returns:
        msg -- A 'LaserScan' message.

        '''
        laser_scan_msg = LaserScan()
        laser_scan_msg.header.stamp = rospy.Time.now()
        laser_scan_msg.header.frame_id = laser_data.frame_id
        laser_scan_msg.angle_min = self.scanner_min_angle
        laser_scan_msg.angle_max = self.scanner_max_angle
        laser_scan_msg.angle_increment = self.scanner_angle_increment
        laser_scan_msg.time_increment = self.scanner_time_increment
        laser_scan_msg.range_min = self.scanner_min_range
        laser_scan_msg.range_max = self.scanner_max_range
        laser_scan_msg.ranges = self.calculate_ranges(laser_data)

        return laser_scan_msg
コード例 #36
0
ファイル: lidarnode.py プロジェクト: bchampp/scylla
    def process_lidar_data(self, pub_lidar, distances, num_measurement,
                           scan_times):

        scan = LaserScan()
        scan.header.stamp = rospy.Time.now()
        scan.header.frame_id = 'lidar'
        scan.angle_min = 0.0
        scan.angle_max = 6.2744
        # scan.angle_increment = 6.2744 / num_measurement
        # scan.time_increment = scan_times / num_measurement
        scan.angle_increment = 0.0174532923847
        scan.time_increment = 0.000132342218421
        scan.scan_time = scan_times
        scan.range_min = 0.15
        scan.range_max = 12.0
        # print('before')
        scan.ranges = distances.tolist()
        # scan.ranges = []
        # print('after')
        # for i in range(num_measurement):
        # 	scan.ranges.append(distances[i])
        pub_lidar.publish(scan)
コード例 #37
0
ファイル: seagoat_ros_client.py プロジェクト: clubcapra/Ibex
    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
コード例 #38
0
ファイル: trhub_py.py プロジェクト: JakeGoh/air
    def timer_callback(self):         # create the message containing the moving average

        for i in range(len(self.sensor)):
            #print "publishing"
            distance = self.sensor[i].readRangeData()
            #print distance
            #if (distance < 14000 and distance > 200):
            terarangers_msg = LaserScan()
            terarangers_msg.header.frame_id = "base_range"
            terarangers_msg.header.stamp = rospy.Time.now()
            terarangers_msg.angle_min = 0
            terarangers_msg.angle_max = 0
            terarangers_msg.angle_increment = 0
            terarangers_msg.time_increment = 0 # 14 metres
            terarangers_msg.scan_time = 0
            terarangers_msg.range_min = 200
            terarangers_msg.range_max = 14000
            terarangers_msg.ranges = [distance/1000.0]
            terarangers_msg.intensities = [0]
                # publish the moving average
            self.range_pub[i].publish(terarangers_msg)
            rospy.sleep(1.)
コード例 #39
0
    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
コード例 #40
0
def scan():
    global stepSweep, sweepDir
    # Maximum count to the left
    countMax = 2420
    # Minimum count to the right
    countMin = 620
    # Populate the LaserScan message
    numReadings = (countMax - countMin) / abs(stepSweep)
    now = rospy.get_rostime()
    msgScan = LaserScan()
    msgScan.header.stamp = now
    msgScan.header.frame_id = 'base_laser'
    msgScan.range_min = 0.010
    msgScan.range_max = 2
    msgScan.time_increment = 0.04
    msgScan.angle_increment = (3.14 / numReadings) * sweepDir
    msgScan.angle_min = 1.57 * sweepDir * -1
    msgScan.angle_max = 1.57 * sweepDir
    msgScan.ranges = []
    msgScan.intensities = []
    msgScan.ranges = []
    # Start and end index based on direction of sweep
    if sweepDir > 0:
        rangeStart = countMin
        rangeEnd = countMax
    elif sweepDir < 0:
        rangeStart = countMax
        rangeEnd = countMin

    # Carry out the sweep
    for i in range(rangeStart, rangeEnd, stepSweep * sweepDir):
        objGpg.set_servo(objGpg.SERVO_1, i)
        msgScan.ranges.append(objDstSns.read_range_single() / 1000.0)

    # Invert sign of sweepDir to signal sweep direction change
    sweepDir = sweepDir * -1

    return msgScan
コード例 #41
0
 def _receive_message(self,message):
        global my
        rospy.loginfo(rospy.get_caller_id() + " Message type %s ",self._message_type)
        rospy.loginfo(rospy.get_caller_id() + " Time from previous message %s ",(rospy.get_time()-my))
        my=rospy.get_time()
        try:
         msg=LaserScan()
         msg.header.seq=message['header']['seq']
         msg.header.stamp.secs=message['header']['stamp']['secs']
         msg.header.stamp.nsecs=message['header']['stamp']['nsecs']
         msg.header.frame_id=message['header']['frame_id']
         msg.angle_min=message['angle_min'] 
         msg.angle_max=message['angle_max'] 
         msg.angle_increment=message['angle_increment']
         msg.time_increment=message['time_increment'] 
         msg.scan_time=message['scan_time']
         msg.range_min=message['range_min']
         msg.range_max=message['range_max']
         ranges_list=[]
         intensities_list=[] 
         for i in range(len(message['ranges'])):
          ranges_list.append(float)
          if message['ranges'][i]==None:
           ranges_list[i]=float('NaN')
          else:
           ranges_list[i]=message['ranges'][i]
         for i in range(len(message['intensities'])):
          intensities_list.append(float)
          if message['intensities'][i]==None:
           intensities_list[i]=float('NaN')
          else:
           intensities_list[i]=message['intensities'][i]
         msg.ranges=ranges_list
         msg.intensities=intensities_list
         self._rosPub=rospy.Publisher(self._local_topic_name, LaserScan, queue_size=10)
         self._rosPub.publish(msg)
        except:
         print('Error')        
コード例 #42
0
    def publish_laser_scan(self):
        """Publish range as laser scan."""
        if not self._running:
            return

        laser_scan_message = LaserScan()
        laser_scan_message.range_max = self._scan_max_range + self._scan_robot_radius
        laser_scan_message.range_min = self._scan_min_range + self._scan_robot_radius

        laser_scan_message.angle_min = self._scan_start
        laser_scan_message.angle_max = self._scan_end
        laser_scan_message.angle_increment = self._scan_step

        # Laser scans are counter clockwise
        laser_scan_message.ranges = [
            self.calculate_reading(step * self._scan_step + self._scan_start)
            for step in reversed(range(self._scan_steps + 1))
        ]

        laser_scan_message.header.frame_id = self._tf_reference_frame
        laser_scan_message.header.stamp = self._latest_message

        self._merged_topic.publish(laser_scan_message)
コード例 #43
0
    def lidarPub(self, timestamp, player):
        """
            Publish lidar
        """

        scan = self.rcs.runScan(player)

        scan_msg = LaserScan()
        scan_msg.header.stamp = timestamp
        scan_msg.header.frame_id = self.scan_frame
        scan_msg.angle_min = -self.car_config["scan_fov"] / 2.0
        scan_msg.angle_max = self.car_config["scan_fov"] / 2.0
        scan_msg.angle_increment = self.car_config[
            "scan_fov"] / self.car_config["scan_beams"]
        scan_msg.range_max = self.car_config["scan_max_range"]
        scan_msg.ranges = scan
        scan_msg.intensities = scan

        if player == "one":
            self.scan_pub_one.publish(scan_msg)

        else:
            self.scan_pub_two.publish(scan_msg)
コード例 #44
0
def ls_publisher(line_scan, width):
    # Calculate the minimum and maximum angle in degrees
    min_max_angle_deg = DEPTH_FOV / 2.

    # Convert min and max angles to radians
    min_max_angle_rad = np.deg2rad(min_max_angle_deg)

    # Calculate the angular resolution
    angle_increment = np.deg2rad(DEPTH_FOV) / width

    # Create laser scan message object
    msg = LaserScan()
    msg.angle_min = -min_max_angle_rad
    msg.angle_max = min_max_angle_rad
    msg.angle_increment = angle_increment
    msg.time_increment = 0
    msg.scan_time = 1. / RS_FPS
    msg.range_min = 0.0
    msg.range_max = MAX_RANGE
    msg.ranges = line_scan
    msg.intensities = np.asanyarray([])
    pub = rospy.Publisher('/scan', LaserScan, queue_size=1)
    pub.publish(msg)
def _toy_scan_initializer():
    """
    Initialize toy LIDAR scan, leave ranges empty and fill in other values
    :returns: LaserScan msg, LIDAR scan data; int, number of scan readings
    """
    # generate toy laser scan
    num_readings = 90
    laser_frequency = 40

    scan = LaserScan()

    scan.header.stamp = 0
    scan.header.frame_id = 'laser_frame'
    scan.angle_min = - np.pi / 4
    scan.angle_max = np.pi / 4
    scan.angle_increment = 1.0
    scan.time_increment = (1.0 / laser_frequency) / num_readings
    scan.range_min = 0.0
    scan.range_max = 10.0

    scan.ranges = []
    scan.intensities = [1] * num_readings
    return scan, num_readings
コード例 #46
0
    def timer_callback(self, timer):
        ts = rospy.Time.now()

        # pub scan
        scan = LaserScan()
        scan.header.stamp = ts
        scan.header.frame_id = 'ego_racecar/laser'
        scan.angle_min = self.angle_min
        scan.angle_max = self.angle_max
        scan.angle_increment = self.angle_inc
        scan.range_min = 0.
        scan.range_max = 30.
        scan.ranges = self.ego_scan
        self.ego_scan_pub.publish(scan)

        # pub tf
        self.publish_odom(ts)
        self.publish_transforms(ts)
        self.publish_laser_transforms(ts)
        self.publish_wheel_transforms(ts)

        # pub race info
        self.publish_race_info(ts)
コード例 #47
0
    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)
コード例 #48
0
    def callback_get_sonar_scan(self, data):
        self.scanFiltered.sonar_range = data.sonar

        if (self.scanFiltered.is_water == True):
            # Construct LaserScan message
            sonarScanMsg = LaserScan()
            sonarScanMsg.header = data.header
            sonarScanMsg.header.frame_id = "base_scan"
            sonarScanMsg.angle_min = -0.1
            sonarScanMsg.angle_max = 0.1
            sonarScanMsg.angle_increment = 0.1
            sonarScanMsg.range_min = 0.02  # unit in meters
            sonarScanMsg.range_max = 2.0  # unit in meters
            sonarDist = data.sonar / 100  # Convert from cm to meters
            sonarScanMsg.ranges = [
                sonarDist, sonarDist, sonarDist
            ]  # Easy fix because not able to make pointcloud with 1 point.

            # Convert to PointCloud2
            cloud_out = self.laserProj.projectLaser(sonarScanMsg)

            # Publish PointCloud2
            self.pcWaterPub.publish(cloud_out)
コード例 #49
0
ファイル: task2.py プロジェクト: padhupradheep/hackathon
def sonar_callback():
    
	
	scan = LaserScan()
	rospy.init_node('task2')  
	scan_pub = rospy.Publisher('scan', LaserScan, queue_size=50)
	num_readings = 100
	laser_frequency = 40
	ranges[num_readings]=0
	intensities[num_readings]=0

	count = 0
	r = rospy.Rate(1.0)
	for i in range (0,num_readings):
		ranges[i]=count;
		intensities[i]=100+count;

	current_time = rospy.Time.now()
	

	scan.header.stamp = current_time
	scan.header.frame_id = 'laser_frame'
	scan.angle_min = -1.57
	scan.angle_max = 1.57
	scan.angle_increment = 3.14 / num_readings
	scan.time_increment = (1.0 / laser_frequency) / (num_readings)
	scan.range_min = 0.0
	scan.range_max = 100.0
	scan.ranges = []
	scan.intensities = []
	for i in range(0, num_readings):
		scan.ranges.append(1.0 * count)  # fake data
		scan.intensities.append(1)  # fake data

	scan_pub.publish(scan)
	count += 1
	r.sleep()
コード例 #50
0
    def laser_callback(self, msg):
        filtered_values = LaserScan()
        distance = np.array(msg.ranges)
        filtered_values.header = msg.header
        filtered_values.angle_increment = msg.angle_increment
        filtered_values.time_increment = msg.time_increment
        filtered_values.scan_time = msg.scan_time
        filtered_values.range_min = msg.range_min
        filtered_values.range_max = msg.range_max
        filtered_values.intensities = msg.intensities
        angle = filtered_values.angle_increment
        min_angle = msg.angle_min
        max_angle = msg.angle_max

        median_filter_size = rospy.get_param('median_filter_size')
        if median_filter_size < 1:
            median_filter_size = 1
        elif median_filter_size > len(distance) / 2 - 1:
            median_filter_size = int(len(distance) / 2 - 1)

        filtered_values_ranges = np.zeros(len(distance))

        for i in range(len(distance) - median_filter_size - 1):
            if i < median_filter_size:
                filtered_values_ranges[i] = 0
            else:
                filtered_values_ranges[i] = np.median(
                    distance[(i - median_filter_size):(i + median_filter_size +
                                                       1)])
                if filtered_values_ranges[
                        i] > msg.range_max or filtered_values_ranges[i] < 0:
                    filtered_values_ranges[i] = 0

        filtered_values.ranges = filtered_values_ranges
        filtered_values.angle_min = min_angle
        filtered_values.angle_max = max_angle
        self.scan_pub.publish(filtered_values)
コード例 #51
0
def update_lidar():
    global x, theta, lidar_seq, pub_lidar
    # print("IN lidar_seq")
    lidar_len = 400
    scan = LaserScan()
    inc = 2 * math.pi / lidar_len
    ranges = []
    for i in range(lidar_len):
        phi_lidar = -math.pi + i * inc
        phi = theta + phi_lidar
        if math.pi / 2 - 0.1 <= abs(phi) <= math.pi / 2 + 0.1:
            ranges.append(100)
        else:
            if abs(phi) < math.pi / 2:
                dx = 10 - x
            else:
                dx = -10 - x
            r = dx / math.cos(phi)
            if 0 <= r < 40:
                ranges.append(r)
            else:
                ranges.append(100)

    scan.header.stamp = rospy.get_rostime()
    scan.header.frame_id = "os1_lidar"
    scan.header.seq = lidar_seq
    lidar_seq += 1
    scan.angle_min = -math.pi
    scan.angle_max = math.pi
    scan.angle_increment = inc
    scan.ranges = ranges

    scan.range_min = 0.0
    scan.range_max = 100.0
    scan.scan_time = 0.01

    pub_lidar.publish(scan)
コード例 #52
0
def test_range_filter():
    """
    Test range filter
    """
    print('\n===== Testing range filter =====\n')
    # generate toy laser scan
    num_readings = 20
    laser_frequency = 40

    scan = LaserScan()

    scan.header.stamp = 0
    scan.header.frame_id = 'laser_frame'
    scan.angle_min = -1.57
    scan.angle_max = 1.57
    scan.angle_increment = 3.14 / num_readings
    scan.time_increment = (1.0 / laser_frequency) / (num_readings)
    scan.range_min = 0.0
    scan.range_max = 100.0

    scan.ranges = []
    scan.intensities = []
    for i in range(0, num_readings):
        scan.ranges.append(np.random.choice(a=[i, np.inf], p=[0.8, 0.2]))  # fake data
        scan.intensities.append(1)  # fake data
    print('\nBefore filter:\n', scan)

    # test filter
    lower_range = 0.5
    upper_range = 10
    print('*** Test: lower range: {}, upper range: {} ***'.format(lower_range, upper_range))
    range_filter(scan, lower_range, upper_range)
    print('\nAfter range filter:\n', scan)

    assert len(scan.ranges) == len(scan.intensities) == num_readings
    assert np.min(scan.ranges) >= lower_range
    assert np.max(scan.ranges) <= upper_range
コード例 #53
0
    def update(self, msg):
        """里程计更新

        Args:
            odom_msg:   ros里程计输入

        See Also:
            _pos:   位置矩阵
            _pose_base: 位置基准矩阵
            _pose_update:   坐标系变换
            _quat:  四元数

        """

        scan_filter = LaserScan()
        scan_filter.header = msg.header
        scan_filter.angle_increment = msg.angle_increment
        scan_filter.angle_max = msg.angle_max
        scan_filter.angle_min = msg.angle_min
        scan_filter.intensities = msg.intensities
        scan_filter.scan_time = msg.scan_time
        scan_filter.range_max = msg.range_max
        scan_filter.range_min = msg.range_min
        scan_filter.time_increment = msg.time_increment
        scan_range = []
        for i in msg.ranges:
            if i >= 64:
                scan_range.append(float("inf"))
            else:
                i_fix = i - 0.1
                if i_fix < scan_filter.range_min:
                    i_fix = scan_filter.range_min
                scan_range.append(i_fix)
        scan_filter.ranges = scan_range
        self.scan_msg = scan_filter
        self.publish()
コード例 #54
0
    def scan_1_callback(self, scan_msg):
        scan_1 = scan_msg

        scan_filtered_msg = LaserScan()
        scan_filtered_msg.header = scan_1.header
        scan_filtered_msg.angle_min = scan_1.angle_min
        scan_filtered_msg.angle_max = scan_1.angle_max
        scan_filtered_msg.angle_increment = scan_1.angle_increment
        scan_filtered_msg.time_increment = scan_1.time_increment
        scan_filtered_msg.scan_time = scan_1.scan_time
        scan_filtered_msg.range_min = scan_1.range_min
        scan_filtered_msg.range_max = scan_1.range_max

        for i in range(0, len(scan_1.ranges)):
            if np.isinf(scan_1.ranges[i]):
                if np.isinf(self.scan_2.ranges[i]):
                    scan_filtered_msg.ranges.append(0)
                else:
                    scan_filtered_msg.ranges.append(scan_1.range_max)

            else:
                scan_filtered_msg.ranges.append(scan_1.ranges[i])

        self.scan_filtered_pub.publish(scan_filtered_msg)
コード例 #55
0
    def scan_cb(self, msg):
        condensed_scan = LaserScan()
        condensed_scan.header = msg.header
        condensed_scan.angle_min = msg.angle_min  # must be -72 or -fov/2
        condensed_scan.angle_max = msg.angle_max  # must be  72 or  fov/2
        condensed_scan.angle_increment = (self.fov /
                                          self.scan_output_size) * self.DEG2RAD
        condensed_scan.time_increment = msg.time_increment
        condensed_scan.scan_time = msg.scan_time
        condensed_scan.range_min = msg.range_min
        condensed_scan.range_max = msg.range_max
        condensed_scan.intensities = [0.0] * self.scan_output_size

        patial_scan = []
        for i in range(len(msg.ranges)):
            patial_scan.append(msg.ranges[i])
            if len(patial_scan) == int(
                    len(msg.ranges) / self.scan_output_size):
                condensed_scan.ranges.append(patial_scan[len(patial_scan) //
                                                         2])
                patial_scan = []
            if len(condensed_scan.ranges) == self.scan_output_size: break

        self.scan_pub.publish(condensed_scan)
コード例 #56
0
def lidarToROS(data, avgSpeed=200):
    global T
    """Converts data from LiDAR into a LaserScan ROS message """
    # Calculate measurement increment time
    Tmeasure = T / 360  # A single revolution is divided into 360 measurements

    # Create a LaserScan message with the correct values
    msg = LaserScan()
    msg.header.stamp = rospy.get_rostime()
    #msg.header.frame_id = "laser_frame"
    msg.angle_min = 0.0
    msg.angle_max = 359.0
    msg.time_increment = Tmeasure
    msg.angle_increment = 1.0
    msg.scan_time = 1.0
    msg.range_min = 0.15
    msg.range_max = 6.0

    # Store the data into LaserScan message
    for i in range(data.shape[0]):
        msg.ranges.append(data[i, 0])
        msg.intensities.append(data[i, 1])

    return msg
コード例 #57
0
    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
        if (self.publishTracks):
            self._currentLaserMsgs[self._getLaserID(readingType,
                                                    content)] = laserscan
コード例 #58
0
    def publish_picked_heading_scan(self, avg_angle, num_pts, laser_scan, pub):
        scan = LaserScan()

        scan.ranges = [0.0] * num_pts
        scan.intensities = [0.0] * num_pts
        current_angle = 0
        avg_angle = (3.142 - avg_angle) % (2 * 3.142)
        for i in range(len(scan.ranges)):
            if current_angle >= (avg_angle -
                                 0.1) and current_angle <= (avg_angle + 0.1):
                scan.ranges[i] = 1.0 + (i * 0.01)
                scan.intensities[i] = 10.0

            current_angle += laser_scan.angle_increment

        scan.header.stamp = rospy.Time.now()
        scan.header.frame_id = laser_scan.header.frame_id
        scan.range_min = laser_scan.range_min
        scan.range_max = laser_scan.range_max
        scan.angle_min = laser_scan.angle_min
        scan.angle_max = laser_scan.angle_max
        scan.angle_increment = laser_scan.angle_increment

        pub.publish(scan)
コード例 #59
0
ファイル: subscriber.py プロジェクト: anoop-ra/fyp
def ros_pub():

    current_time = rospy.Time.now()
    global arr
    scan = LaserScan()
    print "ROS OK!"
    scan.header.stamp = current_time
    scan.header.frame_id = 'laser'
    scan.angle_min = -3.14
    scan.angle_max = 3.14
    scan.angle_increment = 3.14 * 2 / num_readings
    scan.time_increment = (1.0 / laser_frequency) / (num_readings)
    scan.range_min = 0.0
    scan.range_max = 100.0
    scan.ranges = []
    scan.intensities = []
    scan.ranges = arr
    for i in range(0, num_readings):
        #scan.ranges.append(10*i)  # fake data
        scan.intensities.append(50)  # fake data

    scan_pub.publish(scan)

    r.sleep()
コード例 #60
0
ファイル: ros_wrapper.py プロジェクト: gitter-badger/qibullet
    def _updateLasers(self):
        """
        INTERNAL METHOD, updates the laser values in the ROS framework
        """
        if not self.virtual_pepper.laser_manager.isActive():
            return

        scan = LaserScan()
        scan.header.stamp = rospy.get_rostime()
        scan.header.frame_id = "base_footprint"
        # -120 degres, 120 degres
        scan.angle_min = -2.0944
        scan.angle_max = 2.0944

        # 240 degres FoV, 61 points (blind zones inc)
        scan.angle_increment = (2 * 2.0944) / (15.0 + 15.0 + 15.0 + 8.0 + 8.0)

        # Detection ranges for the lasers in meters
        scan.range_min = 0.1
        scan.range_max = 1.5

        # Fill the lasers information
        right_scan = self.virtual_pepper.getRightLaserValue()
        front_scan = self.virtual_pepper.getFrontLaserValue()
        left_scan = self.virtual_pepper.getLeftLaserValue()

        if isinstance(right_scan, list):
            scan.ranges.extend(list(reversed(right_scan)))
            scan.ranges.extend([-1]*8)
        if isinstance(front_scan, list):
            scan.ranges.extend(list(reversed(front_scan)))
            scan.ranges.extend([-1]*8)
        if isinstance(left_scan, list):
            scan.ranges.extend(list(reversed(left_scan)))

        self.laser_pub.publish(scan)