Example #1
0
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
Example #2
0
    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
Example #3
0
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)
Example #4
0
	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)
Example #5
0
    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
Example #8
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()
    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
Example #11
0
 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 )
Example #12
0
 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)
Example #13
0
    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)
Example #15
0
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()
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
Example #17
0
 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)
Example #18
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)
Example #19
0
    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
Example #20
0
    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)
Example #21
0
 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)
	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
Example #23
0
 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
Example #24
0
    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 = []
 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
Example #26
0
  def __init__(self):
    self.bridge = cv_bridge.CvBridge()
    cv2.namedWindow("window", 1)
    
    # Initialise found variable flags
    self.greenFound = False
    self.blueFound = False
    self.yellowFound = False
    self.redFound = False
    
    # intial global methods and variables
    self.laser = LaserScan()
    self.distance = [0]    
    self.objectFound = False
    self.goalSeeking = True
    self.inSearchMode = False
    self.elapsedScanTime = time.time()
    
    # Initialise global array which contains co-ordinates for all way-points
    self.atPoint = False
    self.goalPositions = [[2.01, -4.04],
                          [0.996, -1.05], 
                          [-2.24, -1.57], 
                          [-3.95, -1.04], 
                          [-3.56, 2.93], 
                          [-3.97, -1.57], 
                          [-0.476, 0.804],
                          [-0.298, 3.93]]
                    
    ## Arrays to hold values 
    self.lowerBoundColours = [[40, 200, 100], [100, 200, 100], [25, 200, 100], [0, 210, 100]]
    self.upperBoundColours = [[60, 255, 255], [120, 255, 255], [30, 255, 255], [4, 255, 255]]
    
    
 ## For SIMULATION  
    self.infrared_camera = rospy.Subscriber('/turtlebot/scan', LaserScan, self.laserRange)    
    rospy.sleep(2)
    self.image_sub = rospy.Subscriber('/turtlebot/camera/rgb/image_raw', Image, self.image_callback)
    self.cmd_vel_pub = rospy.Publisher('/turtlebot/cmd_vel', Twist, queue_size=1)
    
    # For Move_Base to set goals/waypoints throughout map
    self.setGoal = rospy.Publisher('/turtlebot/move_base_simple/goal', PoseStamped, queue_size=1)
    self.confirmGoal = rospy.Subscriber('/turtlebot/move_base/result', MoveBaseActionResult, self.goalCallback)
    
    # Initialise Twist() method
    self.twist = Twist()
    
    # Sleep for 3 seconds 
    rospy.sleep(3)
Example #27
0
def scanner():
    rospy.init_node('fake_scan_publisher')
    scan = LaserScan()
    fp = rospy.get_param('/fpubt', 'fake_scan')
    pub = rospy.Publisher(fp, LaserScan, queue_size=10)
    scan.header.frame_id = "base_link"
    scan.header.stamp = rospy.Time.now()
    ra = rospy.get_param('/r')
    rate = rospy.Rate(ra)
    while not rospy.is_shutdown():
        scan.angle_min = rospy.get_param('angle_min')
        scan.angle_max = rospy.get_param('angle_max')
        scan.angle_increment = rospy.get_param('angle_increment')
        scan.scan_time = 0.05
        scan.range_min = rospy.get_param('range_min')
        scan.range_max = rospy.get_param('range_max')
        scan.ranges = numpy.random.uniform(scan.range_min, scan.range_max, 400)
        pub.publish(scan)
        rospy.loginfo(scan)
        rospy.sleep
Example #28
0
    def callback_get_sonar_scan(self, data):
        #self.scanFiltered.sonar_range = data.range * 100
        if (self.isWater == True):
            # Construct LaserScan message
            sonarScanMsg = LaserScan()
            sonarScanMsg.header = data.header
            sonarScanMsg.angle_min = -0.1
            sonarScanMsg.angle_max = 0.1
            sonarScanMsg.angle_increment = 0.1
            sonarScanMsg.range_min = data.min_range
            sonarScanMsg.range_max = data.max_range
            sonarScanMsg.ranges = [
                data.range, data.range, data.range
            ]  # 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)
Example #29
0
    def __init__(self,
                 cmd_vel_topic="cmd_vel",
                 odom_topic="odom",
                 scan_topic="base_scan"):
        self._pose_2d = Pose2D()
        self._pose_stamped = PoseStamped()
        self._laser_scan = LaserScan()

        self._publishers = {}
        self._publishers["cmd_vel"] = rospy.Publisher(cmd_vel_topic,
                                                      Twist,
                                                      queue_size=10)

        rospy.Subscriber(scan_topic, LaserScan, self.__cb_laser_scan)
        rospy.Subscriber(odom_topic, Odometry, self.__cb_odom)
Example #30
0
    def __init__(self):
        self.mover = move_publisher()

        # load map
        path = __file__
        pkg_path = path.split("/src/main.py")[0]
        map_path = pkg_path + "/maps/map.npy"
        self.map = np.load(map_path)

        # get escape-route from A*-algorithm
        a_star_obj = A_star(self.map)
        a_star_obj.start_algorithm()
        self.directions = a_star_obj.return_direction_list()

        self.laser_data = LaserScan()
Example #31
0
def publish_laser_scan(index):
    laser_scan = LaserScan()
    laser_scan.header.frame_id = 'world'
    laser_scan.header.stamp = rospy.get_rostime()
    laser_scan.angle_min = math.pi / 6
    laser_scan.angle_max = (math.pi / 6) * 11
    laser_scan.angle_increment = math.pi / 180
    laser_scan.scan_time = 0.1
    laser_scan.range_min = 0.1
    laser_scan.range_max = 2
    for i in range(
            int((laser_scan.angle_max - laser_scan.angle_min) /
                laser_scan.angle_increment)):
        laser_scan.ranges.append(1.5 + abs(math.cos(i)))
        laser_scan.intensities.append(5)

    sensor_publisher[index].publish(laser_scan)
    def __init__(self):

        self.ns = rospy.get_param("~namespace", "/catvehicle")
        rospy.init_node('Qlearner', anonymous=True)

        rospy.Subscriber('{0}/front_laser_points'.format(self.ns), LaserScan,
                         self.callback)
        self.pub_cmdvel = rospy.Publisher('{0}/cmd_vel'.format(self.ns),
                                          Twist,
                                          queue_size=50)

        self.vel = 0
        self.ang = 0
        self.laserMsg = LaserScan()
        self.xyz = [[] for i in range(3)]
Example #33
0
    def __init__(self):

        rospy.init_node('No_TransRotation', anonymous=True)

        rospy.Subscriber('/tb3_0/cmd_vel', Twist, self.update_vel)
        rospy.Subscriber('/tb3_0/scan', LaserScan, self.update)
        rospy.Subscriber('/tb3_0/odom', Odometry, self.update_pose)

        self.vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
        self.pose = Pose()
        self.scan = LaserScan()
        self.vel = Twist()
        self.rate = rospy.Rate(10)
        self.max_vel = 0.22
        self.max_ang = 2.84
 def __init__(self):
     self.velocity_publisher = rospy.Publisher(
         'turtle1/cmd_vel', Twist, queue_size=5)
     self.odom_subscriber = rospy.Subscriber(
         "/odom", Odometry, self.odeometry_callback, queue_size=5)
     self.laser_subscriber = rospy.Subscriber(
         "/scan", LaserScan, self.laser_callback, queue_size=5)
     self.min_distance = 10
     self.Flag_duck = False
     self.Flag_turn_left = True
     self.Flag_arrived_pos = False
     self.alarm_level = "Green"
     self.obs_place = "N/A"
     self.turtlebot_odom = Odometry()
     self.turtlebot_laser = LaserScan()
    def __init__(self, map_frame, robot_frame):
        self.map_frame = map_frame
        self.robot_frame = robot_frame

        self.pose_msg = PoseStamped()
        self.mapinfo = OccupancyGrid()
        self.laser_data = LaserScan()

        self.tf_listener = TransformListener()
        rospy.Subscriber('map', OccupancyGrid, self.map_callback)
        rospy.Subscriber('pose', PoseStamped, self.pose_callback)
        rospy.Subscriber('scan', LaserScan, self.laser_callback)
        rospy.Service('api/rt_grid_laser', GridLaser,
                      self.handle_realtimelaser)
        rospy.Service('api/grid_laser', GridLaser, self.handle_gridlaser)
Example #36
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)
Example #37
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
 def publish(self, lidar, transforms):
     """Publish the laser scan topics with up to date value."""
     nextTime = self.robot.getTime() + 0.001 * self.timestep
     nextSec = int(nextTime)
     # rounding prevents precision issues that can cause problems with ROS timers
     nextNanosec = int(round(1000 * (nextTime - nextSec)) * 1.0e+6)
     for i in range(lidar.getNumberOfLayers()):
         name = self.prefix + lidar.getName() + '_scan'
         if lidar.getNumberOfLayers() > 1:
             name += '_' + str(i)
         # publish the lidar to scan transform
         transformStamped = TransformStamped()
         transformStamped.header.stamp = Time(sec=nextSec, nanosec=nextNanosec)
         transformStamped.header.frame_id = self.prefix + lidar.getName()
         transformStamped.child_frame_id = name
         q1 = transforms3d.quaternions.axangle2quat([0, 1, 0], -1.5708)
         q2 = transforms3d.quaternions.axangle2quat([1, 0, 0], 1.5708)
         result = transforms3d.quaternions.qmult(q1, q2)
         if lidar.getNumberOfLayers() > 1:
             angleStep = lidar.getVerticalFov() / (lidar.getNumberOfLayers() - 1)
             angle = -0.5 * lidar.getVerticalFov() + i * angleStep
             q3 = transforms3d.quaternions.axangle2quat([0, 0, 1], angle)
             result = transforms3d.quaternions.qmult(result, q3)
         transformStamped.transform.rotation.x = result[0]
         transformStamped.transform.rotation.y = result[1]
         transformStamped.transform.rotation.z = result[2]
         transformStamped.transform.rotation.w = result[3]
         transforms.append(transformStamped)
         # publish the actual laser scan
         msg = LaserScan()
         msg.header.stamp = Time(sec=self.node.sec, nanosec=self.node.nanosec)
         msg.header.frame_id = name
         msg.angle_min = -0.5 * lidar.getFov()
         msg.angle_max = 0.5 * lidar.getFov()
         msg.angle_increment = lidar.getFov() / (lidar.getHorizontalResolution() - 1)
         msg.scan_time = lidar.getSamplingPeriod() / 1000.0
         msg.range_min = lidar.getMinRange()
         msg.range_max = lidar.getMaxRange()
         lidarValues = lidar.getLayerRangeImage(i)
         for j in range(lidar.getHorizontalResolution()):
             msg.ranges.append(lidarValues[j])
         if lidar.getNumberOfLayers() > 1:
             key = self.prefix + lidar.getName() + '_' + str(i)
             self.publishers[lidar][key].publish(msg)
         else:
             self.publishers[lidar].publish(msg)
Example #39
0
 def scan_cb(self, msg):
     self.scan_msg = LaserScan()
     self.scan_msg.header = msg.header
     self.scan_msg.angle_min = msg.angle_min
     self.scan_msg.angle_max = msg.angle_max
     self.scan_msg.angle_increment = msg.angle_increment
     self.scan_msg.time_increment = msg.time_increment
     self.scan_msg.scan_time = msg.scan_time
     self.scan_msg.range_min = msg.range_min
     self.scan_msg.range_max = msg.range_max
     for element in msg.ranges:
         if element == float('Inf'):
             self.scan_msg.ranges.append(self.max_limit)
         else:
             self.scan_msg.ranges.append(element)
Example #40
0
	def callback(self,_data):

		_ls=LaserScan()
		_ls.header.frame_id="laser"
		_ls.angle_min=-3.14
		_ls.angle_max=+3.14
		_ls.angle_increment=6.28/360.0
		_ls.time_increment=0.1
		_ls.range_min=_data.range_min
		_ls.range_max=_data.range_max

		for i in range(0,len(_data.ranges)):
			_ls.ranges.append(2)
		for i in range(0, len(_data.intensities)):
			_ls.intensities.append(3)
		
		self.pub_laser.publish(_ls)
Example #41
0
    def __init__(self, bot_name):
        # bot name 
        self.name = bot_name
        # velocity publisher
        self.vel_pub = rospy.Publisher('cmd_vel', Twist,queue_size=1)

        # Lidar
        self.scan = LaserScan()
        self.lidar_sub = rospy.Subscriber('/blue_bot/scan', LaserScan, self.lidarCallback)
        
        # usb camera
        self.img = None
        self.camera_preview = True
        self.bridge = CvBridge()
        self.image_sub = rospy.Subscriber('/blue_bot/image_raw', Image, self.imageCallback)
Example #42
0
def lidar_cb(data):
    msg = LaserScan()
    msg.header = data.header
    msg.angle_min = data.angle_min
    msg.angle_max = data.angle_max
    msg.angle_increment = data.angle_increment
    msg.scan_time = data.scan_time
    msg.range_min = data.range_min
    msg.range_max = data.range_max
    current_angle = data.angle_min
    i = 0
    for range in data.ranges:
        if ((i >= 30 and i <= 45) or (i >= 75 and i <= 90)
                or (i >= 265 and i <= 280) or (i >= 305 and i <= 325)):
            msg.ranges.append(0)
            msg.intensities.append(0)
        else:
            msg.ranges.append(data.ranges[i])
            msg.intensities.append(data.intensities[i])
        i += 1
    publisher.publish(msg)
Example #43
0
def PrepareScan(sequence, frame_id):
    scan = LaserScan()
    scan.header.seq = sequence
    scan.header.stamp = rospy.get_rostime()
    scan.header.frame_id = frame_id
    scan.angle_min = -kAngleMax
    scan.angle_max = kAngleMax
    scan.angle_increment = (2 * kAngleMax) / kNumberOfScans
    scan.range_max = 2.54
    return scan
Example #44
0
    def __init__(self, pins=["P9_27", "P8_15", "P8_11", "P8_12"]):

        #sets lidar lite addresses
        self.address = 0x62
        self.distWriteReg = 0x00
        self.distWriteVal = 0x04
        self.distReadReg1 = 0x8f
        self.distReadReg2 = 0x10

        #initilizes the i2c bus on address lidar lite address on bus 1
        self.i2c = Adafruit_I2C(self.address, 1)

        #creates pins for the stepper motors
        self.pins = pins

        #initlizes the pins for stepper
        initialize_pins(self.pins)
        set_all_pins_low(self.pins)

        #sets angle to 0
        self.angle = 0

        # Initialize stepping mode
        self.drivemode = fullstep

        #sets up ADC
        ADC.setup()
        #cretes a value for the threshold
        self.threshold = .2
        #creates a variable for the distance measurment
        self.distance = 0
        #creates a variable for the last ir sensor read
        self.lastval = 1
        #creates variables for time
        self.startTime = 0.0
        self.finishTime = 0.0
        self.datatime1 = 0.0
        self.datatime2 = 0.0
        #creates a list of all angles throughout the scan (saves angle in radians)
        self.angleR = []

        #creates a variable for the ROS message and initilizes constant parameters
        self.msg = LaserScan()
        self.header = Header()

        self.msg.angle_increment = 0.015708
        self.msg.range_min = 0
        self.msg.range_max = 40
    def __init__(self):
        self.position_share_pub = rospy.Publisher('/position_share',
                                                  PoseTwistWithCovariance,
                                                  queue_size=1)
        # Set the name
        self.name = rospy.get_namespace()
        if self.name == "/":
            self.name = gethostname()
        else:
            self.name = self.name.replace('/', '')

        self.name = rospy.get_param('~name', self.name)
        rospy.loginfo("Position Share started with name: %s", self.name)

        self.neighbors = {}
        self.neighbors_lock = Lock()
        self.me = None

        self.sensor_link = rospy.get_param(
            "~base_frame_id",
            rospy.get_namespace()[1:] + "base_link")

        self.cloud_header = Header()
        self.cloud_header.frame_id = self.sensor_link
        self.point_cloud_pub = rospy.Publisher('stationary_robots',
                                               PointCloud2,
                                               queue_size=2)

        self.clearing_laser_pub = rospy.Publisher('clearing_scan',
                                                  LaserScan,
                                                  queue_size=1)
        self.clearing_laser_scan = LaserScan()
        self.clearing_laser_scan.header.frame_id = self.sensor_link
        self.clearing_laser_scan.angle_min = -np.math.pi
        self.clearing_laser_scan.angle_max = np.math.pi
        num_scans = 600
        self.clearing_laser_scan.angle_increment = np.math.pi * 2 / num_scans
        self.clearing_laser_scan.range_min = 0
        self.clearing_laser_scan.range_max = 5
        self.clearing_laser_scan.ranges = [3.0] * num_scans

        # self.reset_srv = rospy.ServiceProxy('move_base/DWAPlannerROS/clear_local_costmap', Empty, persistent=True)
        # rospy.wait_for_service('move_base/DWAPlannerROS/clear_local_costmap', timeout=10.)
        rospy.Subscriber('/position_share',
                         PoseTwistWithCovariance,
                         self.position_share_cb,
                         queue_size=1)
        rospy.Service('get_neighbors', GetNeighbors, self.get_neighbors_cb)
Example #46
0
def move():
    rospy.init_node('robot_24', anonymous=True)
    velocity_publisher = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
    vel_msg = Twist()
    vel_msg.linear.x = 0.0
    vel_msg.linear.y = 0
    vel_msg.linear.z = 0
    vel_msg.angular.x = 0
    vel_msg.angular.y = 0
    vel_msg.angular.z = 0

    scan_publisher = rospy.Subscriber('/scan', LaserScan, callback)
    lidar_msg = LaserScan()

    filname = 'n'
    with open(filname) as f_obj:
        line = f_obj.readlines()
    n = int(line[0])
    m = 0
    new_dist = float(line[1]) - 0.04
    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        m += 1
        vel_msg.angular.z = -1
        velocity_publisher.publish(vel_msg)
        rate.sleep()
        if k0 <= new_dist:
            vel_msg.angular.z = 0
            velocity_publisher.publish(vel_msg)
            break
    while not rospy.is_shutdown():
        n -= 1
        vel_msg.linear.x = 100
        velocity_publisher.publish(vel_msg)
        rate.sleep()
        if n == 0:
            break
    vel_msg.linear.x = 0
    velocity_publisher.publish(vel_msg)
    while not rospy.is_shutdown():
        m -= 1
        vel_msg.angular.z = -1
        velocity_publisher.publish(vel_msg)
        rate.sleep()
        if m == 0:
            break
    vel_msg.angular.z = 0
    velocity_publisher.publish(vel_msg)
    def on_scan(self, msg):
        self.scan_count += 1

        cur_reading = None
        if self.mode == MODE.POSE_SCAN or self.mode == MODE.CMD_VEL_SCAN:
            # 0th range measurment is forward using the builtin lidar
            cur_reading = msg.ranges[0]
        elif self.mode == MODE.POSE_RGBD or self.mode == MODE.CMD_VEL_RGBD:
            # for RGBD conversion, the middle range measurement is forward
            #    > for increased robustness, look at the middle few messages
            tries = 0
            midI = int(len(msg.ranges) / 2)
            loffset = 0
            roffset = 0
            while cur_reading is None or math.isnan(cur_reading):
                if tries % 2 == 0:
                    cur_reading = msg.ranges[midI + loffset]
                    loffset -= 1
                if tries % 2 == 1:
                    cur_reading = msg.ranges[midI + roffset]
                    roffset += 1
                tries += 1
                if tries > 10:
                    break

        rospy.loginfo("scan #{0}     dist:{1}".format(self.scan_count,
                                                      cur_reading))
        msg = LaserScan()

        if math.isnan(cur_reading):
            rospy.loginfo("\tUnusable scan reading. Ignoring scan msg")
            return

        if self.prev_wall_loc is not None:

            # Need to jump through some hoops to get sensor model,
            # because we choose to make 0 be the starting point of the robot
            robot_x = self.kfilter.belief.item(0)
            expected_wall_distance = self.prev_wall_loc - robot_x

            self.kfilter.update(expected_reading=expected_wall_distance,
                                reading=cur_reading)
            self.publish_state_estimation(msg.header.stamp)

        # Setting odom origin as 0, the wall is located at (scan_reading), offset by
        # our current location (stored in self.kfilter.belief)
        self.prev_wall_loc = self.kfilter.belief.item(0) + cur_reading
        rospy.loginfo("\twall location: {0}".format(self.prev_wall_loc))
Example #48
0
    def __init__(self,
                 bot_name="NoName",
                 use_lidar=False,
                 use_camera=False,
                 use_imu=False,
                 use_odom=False,
                 use_joint_states=False,
                 use_rviz=False):
        self.name = bot_name

        # velocity publisher
        self.vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)

        # lidar scan subscriber
        if use_lidar:
            self.scan = LaserScan()
            self.lidar_sub = rospy.Subscriber('scan', LaserScan,
                                              self.lidarCallback)

        # camera subscribver
        # please uncoment out if you use camera
        if use_camera:
            # for convert image topic to opencv obj
            self.img = None
            self.bridge = CvBridge()
            self.image_sub = rospy.Subscriber('image_raw', Image,
                                              self.imageCallback)

        # imu subscriber
        if use_imu:
            self.imu_sub = rospy.Subscriber('imu', Imu, self.imuCallback)

        # odom subscriber
        if use_odom:
            self.odom_sub = rospy.Subscriber('odom', Odometry,
                                             self.odomCallback)

        # joint_states subscriber
        if use_joint_states:
            self.odom_sub = rospy.Subscriber('joint_states', JointState,
                                             self.jointstateCallback)

        if use_rviz:
            # for convert image topic to opencv obj
            self.rviz_img = None
            self.rviz_bridge = CvBridge()
            self.rviz_image_sub = rospy.Subscriber('image_raw', Image,
                                                   self.rvizimageCallback)
Example #49
0
    def __init__(self, dimensions, step):
        self.dimensions = dimensions
        self.step = step
        self.map = OccupancyGrid(self.dimensions, self.step)
        self.current_scan = LaserScan()
        self.current_pose = RobotPose()
        self.sensor_model = SensorModelNarrowNoIntensity()
        self.got_scan = False
        self.first_odom = True
        self.start = True

        rospy.init_node("mapper")
        rospy.Subscriber("/base_scan", LaserScan, self.laser_callback)
        rospy.Subscriber("/odom", Odometry, self.odom_callback)
        rospy.Subscriber("/joy", Joy, self.joy_callback)
        atexit.register(self.output_maps)
    def __init__(self):
        scan_front_topic = rospy.get_param("~scan_front", "scan_front")
        scan_back_topic = rospy.get_param("~scan_back", "scan_back")
        # vel_topic = rospy.get_param("~vel_topic", "cmd_vel")

        self.params_config()

        self.scan_msg = LaserScan()
        rospy.Subscriber(scan_front_topic, LaserScan, self.scan_front_callback)
        rospy.Subscriber(scan_back_topic, LaserScan, self.scan_back_callback)
        rospy.Service("nav_goal/storage_avoidance", StorageAvoidance, self.handle_avoidance)
        # self.vel_pub = rospy.Publisher(vel_topic, Twist, queue_size=1)
        # rospy.sleep(rospy.Duration(1.0))
        # rate = rospy.Rate(3)
        while not rospy.is_shutdown():
            rospy.spin()
Example #51
0
    def timer_cb(self, event):

        now = rospy.Time.now()
        ls = LaserScan()
        ls.header.frame_id = "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", "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)
Example #52
0
def slice_scan(scan, slice):
    """Slice a laser scan like a python list"""
    start, end, step = slice.indices(len(scan.ranges))
    # TODO - update the timestamp in the header
    return LaserScan(
        header=scan.header,
        angle_min=scan.angle_min + scan.angle_increment * start,
        angle_max=scan.angle_min + scan.angle_increment * end,
        angle_increment=scan.angle_increment*step,
        time_increment=scan.time_increment*step,
        scan_time=scan.scan_time,
        range_min=scan.range_min,
        range_max=scan.range_max,
        ranges=scan.ranges[start:end],
        intensities=scan.intensities[start:end]
    )
Example #53
0
class laser_feature:
    ranges = LaserScan()
    def __init__(self):
        '''Initialize ros publisher, ros subscriber'''
        # topic where we publish
        #self.image_pub = rospy.Publisher("/output/image_raw/compressed", CompressedImage)
        # self.bridge = CvBridge()
        # subscribed Topic
        self.subscriber = rospy.Subscriber("/kobuki/laser/scan",
            LaserScan, self.callback,  queue_size = 1)

    def callback(self, ros_data):
        ranges = ros_data.ranges
        arq.write(str(ranges))
        arq.write("\n")
        time.sleep(1)
    def __init__(self, num_lidar_beams: int, lidar_range: float):
        """ a class to collect and merge observations

        Args:
            num_lidar_beams (int): [description]
            lidar_range (float): [description]
        """
        # define observation_space
        self.observation_space = ObservationCollector._stack_spaces(
            (spaces.Box(low=0,
                        high=lidar_range,
                        shape=(num_lidar_beams, ),
                        dtype=np.float32),
             spaces.Box(low=0, high=10, shape=(1, ), dtype=np.float32),
             spaces.Box(low=-np.pi, high=np.pi, shape=(1, ),
                        dtype=np.float32)))

        # flag of new sensor info
        self._flag_all_received = False

        self._scan = LaserScan()
        self._robot_pose = Pose2D()
        self._robot_vel = Twist()
        self._subgoal = Pose2D()

        # message_filter subscriber: laserscan, robot_pose
        self._scan_sub = message_filters.Subscriber("scan", LaserScan)
        self._robot_state_sub = message_filters.Subscriber(
            'plan_manager/robot_state', RobotStateStamped)

        # message_filters.TimeSynchronizer: call callback only when all sensor info are ready
        self.ts = message_filters.ApproximateTimeSynchronizer(
            [self._scan_sub, self._robot_state_sub], 100,
            slop=0.05)  #,allow_headerless=True)
        self.ts.registerCallback(self.callback_observation_received)

        # topic subscriber: subgoal
        #TODO should we synchoronize it with other topics
        self._subgoal_sub = message_filters.Subscriber(
            'plan_manager/subgoal', PoseStamped
        )  #self._subgoal_sub = rospy.Subscriber("subgoal", PoseStamped, self.callback_subgoal)
        self._subgoal_sub.registerCallback(self.callback_subgoal)

        # service clients
        self._service_name_step = 'step_world'
        self._sim_step_client = rospy.ServiceProxy(self._service_name_step,
                                                   StepWorld)
Example #55
0
 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
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)
Example #57
0
    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()
Example #58
0
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)
    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
Example #60
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