def main(): rospy.init_node('fake_scan', anonymous=True) scan_subscriber = rospy.Subscriber('/remapped_scan', LaserScan, scan_callback) scan_publisher = rospy.Publisher('/scan', LaserScan, queue_size=10) rate = rospy.Rate(3) m = 720 empty_scan_msg = LaserScan() empty_scan_msg.angle_min = -2.3561899662 empty_scan_msg.angle_max = 2.3561899662 empty_scan_msg.angle_increment = 0.0065540750511 empty_scan_msg.time_increment = 0.0 empty_scan_msg.scan_time = 0.0 empty_scan_msg.range_min = 0.10000000149 empty_scan_msg.range_max = 30.0 empty_scan_msg.ranges = [0.0 for i in range(m)] empty_scan_msg.intensities = [float('inf') for i in range(m)] h = std_msgs.msg.Header() h.frame_id = "base_laser" for i in range(10): h.stamp = rospy.Time.now() h.seq += 1 empty_scan_msg.header = h scan_publisher.publish(empty_scan_msg) rate.sleep() #infinite range, zero intensity --> can't detect anyting empty_scan_msg.ranges = [float('inf') for i in range(m)] empty_scan_msg.intensities = [0.0 for i in range(m)] while not rospy.is_shutdown(): h.stamp = rospy.Time.now() h.seq += 1 empty_scan_msg.header = h scan_publisher.publish(empty_scan_msg) rate.sleep()
def scanCB(self, msg): range_list = list(msg.ranges) intensity_list = list(msg.intensities) for i in xrange(len(msg.ranges)): current_angle = msg.angle_min + msg.angle_increment * i # Delete the first element if (current_angle < np.deg2rad(self.ang_min)): del range_list[0] del intensity_list[0] # Delete the last element elif (current_angle > np.deg2rad(self.ang_max)): del range_list[-1] del intensity_list[-1] new_scan = LaserScan() new_scan.header = msg.header new_scan.angle_min = np.deg2rad(self.ang_min) new_scan.angle_max = np.deg2rad(self.ang_max) new_scan.angle_increment = msg.angle_increment new_scan.time_increment = msg.time_increment new_scan.scan_time = msg.scan_time new_scan.range_min = msg.range_min new_scan.range_max = msg.range_max new_scan.ranges = range_list if (self.intensity == False): new_scan.intensities = [] else: new_scan.intensities = intensity_list self.scan_pub.publish(new_scan)
def callback(sub,sub5): l = [sub.ranges,sub5.range] ir = sub5.range #print sub.ranges[320], "The Laser Scan Data" #print ir, "The IR sensor Data" pub = rospy.Publisher('scan2', LaserScan, queue_size=10) #rospy.init_node('laser_scan_publisher') scan_pub = rospy.Publisher('uvbot_laser_scan', LaserScan, queue_size=10) num_readings = 720 laser_frequency = 60 count = 0 r = rospy.Rate(1.0) while not rospy.is_shutdown(): current_time = rospy.Time.now() 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: for i in range(320,340): a = sub5.range scan.ranges[i] = a scan.intensities = [] else: scan.ranges = sub.ranges scan.intensities = sub.intensities #for i in range(0, 100): #if i<=50: #scan.ranges.append(100) #else: #scan.ranges.append(20) print len(scan.ranges) scan_pub.publish(scan) count += 1 r.sleep()
def laserscan_err_inj(tb3_name): #Create error-injected topic rospy.init_node('laser_err_inj') ######################################### #Create new message laserscan_msg = LaserScan() #Fill message with values laserscan_msg.header.seq = 0 laserscan_msg.header.stamp.secs = 0 laserscan_msg.header.stamp.nsecs = 0 laserscan_msg.header.frame_id = "" laserscan_msg.angle_min = 0.0 laserscan_msg.angle_max = 0.0 laserscan_msg.angle_increment = 0.0 laserscan_msg.time_increment = 0.0 laserscan_msg.scan_time = 0.0 laserscan_msg.range_min = 0.0 laserscan_msg.range_max = 0.0 laserscan_msg.ranges = [0.0] * 360 laserscan_msg.intensities = [0.0] * 360 ######################################### rate = rospy.Rate(50) #Publish message into new topic while not rospy.is_shutdown(): my_sub = rospy.Subscriber(turtlebot_dict[tb3_name] + 'scan', LaserScan, listener) my_pub = rospy.Publisher(turtlebot_dict[tb3_name] + 'laser_err_inj', LaserScan, queue_size = 10) ######################################### #INJECT ERRORS HERE laserscan_msg.header.seq = actual_seq laserscan_msg.header.stamp.secs = actual_secs laserscan_msg.header.stamp.nsecs = actual_nsecs laserscan_msg.header.frame_id = actual_frameid laserscan_msg.angle_min = actual_anglemin laserscan_msg.angle_max = actual_anglemax laserscan_msg.angle_increment = actual_angleincrement laserscan_msg.time_increment = actual_timeincrement laserscan_msg.scan_time = actual_scantime laserscan_msg.range_min = actual_rangemin laserscan_msg.range_max = actual_rangemax for i in range(len(actual_ranges)): laserscan_msg.ranges[i] = actual_ranges[i] #inject error here (i just made everything = 0 because it's easy to see when testing) for j in range(len(actual_intensities)): laserscan_msg.intensities[i] = actual_intensities[i] #inject error here (i just made everything = 0 because it's easy to see when testing) ######################################### my_pub.publish(laserscan_msg) rate.sleep() rospy.spin()
def publish_scan(self): if self.viewpoint == None: rospy.logdebug( 'Mock laser did not receive a hint to indicate the viewpoint. Not publishing mock scans!' ) return rospy.logdebug('Publishing mock laser scan viewpoint %s' % self.viewpoint) scan = LaserScan() scan.header.stamp = rospy.Time.now() scan.header.frame_id = 'hokuyo_laser_frame' # reverse scan since it's from right to left! scan.ranges = self.scans[self.viewpoint]['laser'][::-1] scan.intensities = [1] * len(scan.ranges) # Here, we are giving the full 360 scan, nothing is dropped so set the range. scan.angle_min = -math.pi scan.angle_max = math.pi scan.angle_increment = 2 * math.pi / len(scan.ranges) scan.time_increment = 0.01 / len(scan.ranges) scan.range_min = 0.0 scan.range_max = 100.0 self.pub_scan.publish(scan)
def call_back(timer): current_time = rospy.Time.now() scan = LaserScan() scan.header.stamp = current_time scan.header.frame_id = 'laser_frame' scan.angle_min = 0 scan.angle_max = 2 * math.pi scan.angle_increment = 2 * math.pi / 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 = [] if multiranger.front == None: scan.ranges.append(0) else: scan.ranges.append(multiranger.front) #scan.intensities.append(1) scan.ranges.append(multiranger.left) # #scan.intensities.append(1) scan.ranges.append(multiranger.back) # #scan.intensities.append(1) scan.ranges.append(multiranger.right) #scan.intensities.append(1) #print(x,y) b.sendTransform( (x, y, 0.0), transformations.quaternion_from_euler(0, 0, yaw[0] * (math.pi / 180)), Time.now(), '/base_link', '/odom') scan_pub.publish(scan)
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() h.stamp = rospy.Time.now() # Note you need to call rospy.init_node() before this will work 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)
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 mapping(self, ScanData): # remap data from SBG_IMU_msg to ROS_IMU_msg and publish FakeScan = LaserScan() # FakeScan = ScanData FakeScan.header.frame_id = 'laser' FakeScan.header.seq = ScanData.header.seq FakeScan.header.stamp = ScanData.header.stamp FakeScan.angle_min = ScanData.angle_min FakeScan.angle_max = ScanData.angle_max FakeScan.angle_increment = ScanData.angle_increment FakeScan.time_increment = ScanData.time_increment FakeScan.scan_time = ScanData.scan_time FakeScan.range_min = ScanData.range_min FakeScan.range_max = ScanData.range_max i = 0 FakeScan.ranges = [1] * len(ScanData.ranges) FakeScan.intensities = [1] * len(ScanData.intensities) for i in range(len(ScanData.ranges)): if ScanData.ranges[i] < ScanData.range_min: FakeScan.ranges[i] = 30.0 FakeScan.intensities[i] = 430.0 else: FakeScan.ranges[i] = ScanData.ranges[i] FakeScan.intensities[i] = ScanData.intensities[i] self.pub.publish(FakeScan)
def callback(self, msg): scan = LaserScan() scan.header.seq = msg.header.seq scan.header.stamp.secs = msg.header.stamp.secs scan.header.stamp.nsecs = msg.header.stamp.nsecs scan.header.frame_id = msg.header.frame_id scan.angle_min = msg.angle_min scan.angle_max = msg.angle_max scan.angle_increment = msg.angle_increment scan.time_increment = msg.time_increment scan.range_min = msg.range_min scan.range_max = msg.range_max scan.intensities = msg.intensities num_readings = len(msg.ranges) scan.ranges = list() for i in range(0, num_readings): if msg.ranges[i] < msg.range_min or msg.ranges[i] > msg.range_max: # print("u[pdated something") scan.ranges.append(float("inf")) else: scan.ranges.append(msg.ranges[i]) # scan.ranges[i] = float("inf") self._pub.publish(scan) print("published")
def test_distance_calculator_stress_test(): """ Stress test of distance calculator and speed check """ print('\n====== Stress test ======\n') # generate toy laser scan num_readings = 100 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(i) # fake data scan.intensities.append(1) # fake data # distance calculator filter speed test time_start = time.time() for i in range(100): calculate_closest_object_distance(scan) time_end = time.time() print('*** Distance calculator run time: {} sec over {} runs, FPS = {} ***'.format(time_end - time_start, i + 1, ((i + 1) / ( time_end - time_start))))
def convert_laser(outbag, csv_file_name, topic_name, tf_msg): data_frame = pd.read_csv(csv_file_name) num_rows = len(data_frame.index) t_range = [] for i, row in data_frame.iterrows(): update_progress(float(i + 1) / num_rows, topic_name) t = rospy.Time.from_sec(float(row[0])) if not t_range: t_range = [float(row[0]), 0.0] t_range[1] = float(row[0]) scan = LaserScan() scan.header.seq = i scan.header.stamp = t scan.header.frame_id = tf_msg.child_frame_id scan.angle_min = - math.pi / 2 scan.angle_max = math.pi / 2 scan.angle_increment = math.pi / 180 scan.time_increment = (1.0 / 75.0)/181 scan.range_min = 0.0 scan.range_max = 30.0 scan.intensities = [] scan.ranges = row[3:184] outbag.write(topic_name, scan, t) return t_range
def main(): rospy.init_node('test') # to initiate and create an unique node rospy.Subscriber( '/tim561_scan', LaserScan, callback) # subscribing to the topic from lidar_test_data.bag pub = rospy.Publisher('/tim561_scan_filtered', LaserScan, queue_size=10) # Publisher for filtered data while laserscan is None and not rospy.is_shutdown( ): # For when data is not available keeping the node on hold rospy.sleep(1) rate = rospy.Rate(10) new_data = LaserScan() # Object for filtered data while not rospy.is_shutdown(): new_data.header = laserscan.header # Header is not changed in new data new_data.header.frame_id = 'laser' # ID changed for rviz visualization new_data.angle_min = laserscan.angle_min # Doesn't change after filtering new_data.angle_max = laserscan.angle_max # Doesn't change after filtering new_data.angle_increment = laserscan.angle_increment # Doesn't change after filtering new_data.time_increment = laserscan.time_increment # Doesn't change after filtering new_data.ranges = lowpass(laserscan.ranges, laserscan.angle_increment, RC) # Filtering the data new_data.range_min = min( new_data.ranges) # Finding new minimum of filtered data new_data.range_max = max( new_data.ranges) # Finding new maximum of filtered data new_data.intensities = laserscan.intensities # Doesn't change after filtering(empty) pub.publish(new_data) # Publishing filtered data rate.sleep()
def lidar_publisher(self): global compress_data if len(compress_data) > 0: # header _Scan = LaserScan() _Scan.header.stamp = rospy.Time.now() _Scan.header.seq = self.seq self.seq += 1 _Scan.header.frame_id = self.rplidar_frame # rplidar_parameters _Scan.angle_max = numpy.pi - numpy.radians(0.0) _Scan.angle_min = numpy.pi - numpy.radians(360.0) _Scan.angle_increment = -numpy.radians(1.0) # _Scan.time_increment = self.duration / 360 # _Scan.scan_time = self.duration _Scan.range_min = self.range_min _Scan.range_max = self.range_max # rplidar_ranges [ranges, intensive] = compress_data.pop() _Scan.ranges = ranges _Scan.intensities = intensive if _Scan != LaserScan(): pub_data = rospy.Publisher(self.scan_topic, LaserScan, queue_size=1) pub_data.publish(_Scan)
def talker(): pub = rospy.Publisher('recorded_scan', LaserScan, queue_size=10) rospy.init_node('talker', anonymous=True) rate = rospy.Rate(12) lines = f.readlines() scancount = 0 i=0 while not rospy.is_shutdown(): if i >= len(lines): scancount = 0 i=0 f.seek(0) if len(lines[i]) > 100: scancount += 1 scan = LaserScan() scan.header.stamp = rospy.Time.now() scan.header.frame_id = 'laser' scan.angle_min = -2.28987193108 scan.angle_max = 2.28987193108 scan.angle_increment = 0.0069813169539 scan.time_increment = 0.083333 scan.scan_time = 0.0833*scancount scan.range_min = 0.002 scan.range_max = 50 scan.ranges = map(float, lines[i][1:-2].split(', ')) scan.intensities = map(float, lines[i][1:-2].split(', ')) pub.publish(scan) rate.sleep() i+=1
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 laser_noise_variance = rospy.get_param('laser_noise_variance') if laser_noise_variance <= 0: laser_noise_variance = 0.1 filtered_values_ranges = np.zeros(len(distance)) noise_values_ranges = np.random.normal(loc=0, scale=laser_noise_variance, size=len(distance)) for i in range(len(distance)): filtered_values_ranges[i] = noise_values_ranges[i] + distance[i] filtered_values.ranges = filtered_values_ranges filtered_values.angle_min = min_angle filtered_values.angle_max = max_angle self.scan_pub.publish(filtered_values)
def laser_test(): pub = rospy.Publisher('laser_scan', LaserScan) rospy.init_node('laser_test') 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(10) # 10hz while not rospy.is_shutdown(): laser_msg.header.stamp = rospy.get_rostime() pub.publish(laser_msg) r.sleep()
def callback(msg): #function callback which recieves a parameters named msg l = msg.ranges m = min(x,y,z) list_l = list(l) # List of the original ranges data r = rospy.Rate(10) # define lidar msg to new scan object if m > 0.4: # IR distance is more than 0.4 list_l = list(l) #print list_l # No change in the original Lidar data if m <= 0.4 and x !=0 : # If IR data is less than 0.4 for i in range(160,200): list_l[i] = x # replace the 320 to 330 index in the lidar with x #print list_l scan = LaserScan() # Laserscan object scan = msg current_time = rospy.Time.now() scan.header.stamp = current_time scan.ranges = list_l scan.intensities = [] scan_pub.publish(scan)
def ls_scan_pub(self, pub, laser_distance1, laser_angle1): # distance data is from -pi to pi laser_distance1 = np.array(laser_distance1) scan_msg = LaserScan() point_num = 360.0 scan_msg.angle_max = self.angle_max # LS-lidar rotates clockwise, which is disobey right-hand rules for coordinate system scan_msg.angle_min = self.angle_min scan_msg.header.frame_id = 'base_laser_link' scan_msg.angle_increment = 2 * math.pi / point_num scan_msg.range_min = 0.15 scan_msg.range_max = 3.0 scan_msg.header.stamp = rospy.Time.now() # print("begin to select") if (self.angle_min > 0) or (self.angle_max <= 0): range_part = laser_distance1[(self.angle_range >= self.angle_min) & (self.angle_range <= self.angle_max)] scan_msg.ranges = np.flipud(range_part) else: range_part1 = laser_distance1[(self.angle_range >= self.angle_min) & (self.angle_range <= 0)] range_part2 = laser_distance1[(self.angle_range <= self.angle_max) & (self.angle_range > 0)] p1 = np.flipud(range_part1) p2 = np.flipud(range_part2) scan_msg.ranges = np.append(p1, p2) scan_msg.intensities = [255] * len(laser_distance1) pub.publish(scan_msg)
def handleScanMsg(data): rospy.loginfo("\n/scan is now being handled...") #### Setup Corrected-LaserScan Publisher correctedScan_publisher = rospy.Publisher("/scanCorrected", LaserScan, queue_size=10) correctedScan_msg = LaserScan() #### Start copying the variables correctedScan_msg.angle_min = data.angle_min correctedScan_msg.angle_max = data.angle_max correctedScan_msg.angle_increment = data.angle_increment correctedScan_msg.time_increment = data.time_increment correctedScan_msg.scan_time = data.scan_time correctedScan_msg.range_min = data.range_min correctedScan_msg.range_max = data.range_max correctedScan_msg.intensities = data.intensities #### Start copying 'ranges' using the corrected values if necessary # if (+inf) -> change it to (-1) correctedScan_msg.ranges = [0] * len(data.ranges) for i in range(len(data.ranges)): if numpy.isinf(data.ranges[i]): correctedScan_msg.ranges[i] = -100 else: correctedScan_msg.ranges[i] = data.ranges[i] #### Publish msg rate = rospy.Rate(1800) # 1800hz correctedScan_publisher.publish(correctedScan_msg) rospy.loginfo("\n/scanCorrected is now being published...") rate.sleep()
def LaserReceiver(): pub = rospy.Publisher('scanPi', LaserScan, queue_size=10) rospy.init_node('LaserReceiver', anonymous=True) rate = rospy.Rate(10) # 10hz sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) # UDP sock.bind(("192.168.1.65", 4001)) while not rospy.is_shutdown(): data, addr = sock.recvfrom(15024) data = json.loads(data) laser = LaserScan() laser.header.stamp.secs = data["header"]["stamp"]["secs"] laser.header.stamp.nsecs = data["header"]["stamp"]["nsecs"] laser.header.frame_id = data["header"]["frame_id"] laser.header.seq = data["header"]["seq"] laser.angle_min = data["angle_min"] laser.angle_max = data["angle_max"] laser.angle_increment = data["angle_increment"] laser.time_increment = data["time_increment"] laser.range_min = data["range_min"] laser.range_max = data["range_max"] laser.scan_time = data["scan_time"] laser.ranges = data["ranges"] laser.intensities = data["intensities"] pub.publish(laser) rate.sleep()
def callback(self, msg): rate = rospy.Rate(10) scan_msg = LaserScan() scan_msg.header = msg.header scan_msg.angle_min = msg.angle_min scan_msg.angle_max = msg.angle_max scan_msg.angle_increment = msg.angle_increment scan_msg.time_increment = msg.time_increment scan_msg.scan_time = msg.scan_time scan_msg.range_min = msg.range_min scan_msg.range_max = msg.range_max #print np.shape(msg.ranges) count = int(msg.scan_time / msg.time_increment) scan_msg.ranges = np.zeros((count, ), dtype=np.float) scan_msg.intensities = np.zeros((count, ), dtype=np.float) for i in xrange(count): #if i < 359 or i > 1079: if i < 489 or i > 949: scan_msg.ranges[i] = msg.ranges[i] scan_msg.intensities[i] = msg.intensities[i] else: scan_msg.ranges[i] = float('inf') scan_msg.intensities[i] = 0.0 #print "here" #rate.sleep() self.scan_pub.publish(scan_msg)
def data(self,a): angle = [] for i in reversed(range(90)): angle.append(i) for i in range(270,360): angle.append(i) Range = [a.ranges[i] for i in angle] # rospy.loginfo_throttle(2,a.ranges[0]) pub=rospy.Publisher('/CloudPoint',LaserScan,queue_size=10) msg=LaserScan() msg.header=a.header msg.header.frame_id = 'laser' msg.angle_min=a.angle_min msg.angle_max=a.angle_max msg.angle_increment=a.angle_increment msg.time_increment=a.time_increment msg.scan_time=a.scan_time msg.range_min=a.range_min msg.range_max=a.range_max List = self.ranges New = numpy.hstack([List,Range]) self.ranges = New if len(self.ranges) >=4500: self.ranges = self.ranges[900:] msg.ranges=self.ranges msg.intensities=a.intensities pub.publish(msg) print len(msg.ranges)
def scanCallback(self, scan): rmin = [0 , 315] rmax = [45, 360] reduced_scan = LaserScan() reduced_scan.header = scan.header reduced_scan.angle_min = scan.angle_min reduced_scan.angle_max = scan.angle_max reduced_scan.angle_increment = scan.angle_increment reduced_scan.time_increment = scan.time_increment reduced_scan.scan_time = scan.scan_time reduced_scan.range_min = scan.range_min reduced_scan.range_max = scan.range_max reduced_scan.intensities = scan.intensities for i in range(len(scan.ranges)): if i >= rmin[0] and i <= rmax[0]-1: reduced_scan.ranges.append(scan.ranges[i]) elif i >= rmin[1] and i <= rmax[1]-1: reduced_scan.ranges.append(scan.ranges[i]) else: reduced_scan.ranges.append(0) rospy.loginfo("val[%d]:%f", i, reduced_scan.ranges[i]) self.pub_reduced_scan.publish(reduced_scan)
def publish_filtered_laser_scan(self, laser_original_data, new_filtered_laser_range): length_range = len(laser_original_data.ranges) length_intensities = len(laser_original_data.intensities) laser_filtered_object = LaserScan() h = Header() h.stamp = rospy.Time.now( ) # Note you need to call rospy.init_node() before this will work h.frame_id = "chassis" 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 laser_filtered_object.angle_increment = laser_original_data.angle_increment 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: laser_filtered_object.ranges.append(item) laser_filtered_object.intensities.append(item) self.laser_filtered_pub.publish(laser_filtered_object)
def read(pub): if (ser.in_waiting >= 9): a = rospy.Time.now() # print("read") Dist_Total = -1 if ((b'Y' == ser.read()) and (b'Y' == ser.read())): Dist_L = ser.read() Dist_H = ser.read() Strength_L = ser.read() Strength_H = ser.read() Strength = ord(Strength_L) + 256 * ord(Strength_H) Mode = ser.read() tmp = ser.read() CheckSum = ser.read() Dist_Total = (ord(Dist_H) * 256) + (ord(Dist_L)) scan = LaserScan() # print(rospy.Time.now(),Dist_Total) scan.header.stamp = a scan.header.frame_id = 'laser_frame' scan.angle_min = 0. scan.angle_max = 0. scan.angle_increment = 0 scan.time_increment = 0 scan.range_min = 30. scan.range_max = 1200. scan.ranges = [] scan.intensities = [] scan.ranges.append(Dist_Total) scan.intensities.append(Strength) pub.publish(scan)
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 talker(): pub = rospy.Publisher('scan', LaserScan, queue_size=10) rospy.init_node('talker', anonymous=True) rate = rospy.Rate(10) # 10hz br = tf.TransformBroadcaster() sequence = 1 while not rospy.is_shutdown(): br.sendTransform((0.1, 0.1, 0), tf.transformations.quaternion_from_euler(0, 0, 0), rospy.Time.now(), 'base_laser', 'base_link') br.sendTransform( (sequence * 0.01, sequence * 0.01, 0), tf.transformations.quaternion_from_euler(0, 0, sequence * 0.01), rospy.Time.now(), 'base_link', 'odom') scan = LaserScan() scan.header.seq = sequence scan.header.stamp = rospy.get_rostime() scan.header.frame_id = 'base_laser' scan.angle_min = -0.1 scan.angle_max = 0.1 scan.angle_increment = 0.1 scan.range_max = 5.0 scan.ranges = [4.0, 4.0, 4.0] scan.intensities = [0.1, 0.1, 0.1] rospy.logdebug(scan) pub.publish(scan) sequence += 1 rate.sleep()
def pubSpot(scan, target, targetRange, FRAME_ID): """Generate spot scan data from input. Parameters ---------- scan : LaserScan scan from LIDAR target : float or str target angle in degree targetRange : float distance to target in m FRAME_ID : str frame id """ output = LaserScan() output.header.seq = scan.header.seq output.header.stamp.secs = scan.header.stamp.secs output.header.stamp.nsecs = scan.header.stamp.nsecs output.header.frame_id = FRAME_ID output.angle_min = scan.angle_min output.angle_max = scan.angle_max output.angle_increment = scan.angle_increment output.time_increment = scan.time_increment output.scan_time = scan.scan_time output.range_min = scan.range_min output.range_max = scan.range_max output.ranges = np.zeros(360) output.intensities = np.zeros(360) if target != "NaN": output.ranges[int(target)] = targetRange output.intensities[int(target)] = 10000.0 pub = rospy.Publisher("spots_laserdata", LaserScan, queue_size=1) pub.publish(output) spot = Marker() spot.id = np.random.random_integers(2**16 - 1) spot.type = spot.CYLINDER spot.scale.x = 0.05 spot.scale.y = 0.05 spot.scale.z = 1.0 spot.pose.position.x = targetRange * np.cos(target / 360.0 * 2.0 * np.pi) spot.pose.position.y = targetRange * np.sin(target / 360.0 * 2.0 * np.pi) spot.header.frame_id = FRAME_ID spot.header.stamp = rospy.Time.now() spot.color.r = 255.0 spot.color.g = 255.0 spot.color.b = 255.0 spot.color.a = 255.0 spot.lifetime.secs = 300 pub = rospy.Publisher(PUB_TO, Marker, queue_size=1) pub.publish(spot)
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 ir_callback(data): global list_ang global count global timer # Twist is a message type in ros, here we use an Twist message to control kobuki's speed # twist. linear.x is the forward velocity, if it is zero, robot will be static, # if it is grater than 0, robot will move forward, otherwise, robot will move backward # twist.angular.axis is the rotatin velocity around the axis # # Around which axis do we have to turn? In wich direction will it turn with a positive value? # Right hand coordinate system: x forward, y left, z up #twist = Twist() #twist.linear.x = 0. #twist.angular.z = 0. # write your code here raw_data = data.data ang = raw_data >> 16 dis = raw_data & 65535 voltage = float(dis) * 5.0 / 1024.0 distance = 72.4883100736 * (1.0 / float(voltage)) - 13.641013781 count += 1 list_ang.append(distance) if (ang < 280) and (count > 20): temp = time.clock() delta_time = temp - timer if delta_time < 0.2: print("Delta time = {}".format(delta_time)) print("Time increment = {}".format(delta_time / count)) scan_pub = rospy.Publisher('scan', LaserScan, queue_size=50) r = rospy.Rate(1.0) if not rospy.is_shutdown(): current_time = rospy.Time.now() scan = LaserScan() scan.header.stamp = current_time scan.header.frame_id = 'laser_frame' scan.angle_min = -math.pi scan.angle_max = math.pi scan.angle_increment = 2 * math.pi / count scan.time_increment = delta_time / count scan.range_min = 0 scan.range_max = 100 scan.ranges = [] scan.intensities = [] for i in list_ang: scan.ranges.append(i) scan_pub.publish(scan) r.sleep() list_ang = [] count = 0 timer = temp
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 = []
def inputCallback(self, msg): ######################################################################### # rospy.loginfo("-D- range_filter inputCallback") cur_val = msg.value if cur_val <= self.max_valid and cur_val >= self.min_valid: self.prev.append(cur_val) del self.prev[0] p = array(self.prev) self.rolling_ave = p.mean() self.rolling_std = p.std() self.rolling_meters = ((self.b * self.rolling_ave) ** self.m) / 100 self.filtered_pub.publish( self.rolling_meters ) self.std_pub.publish( self.rolling_std ) rng = Range() rng.radiation_type = 1 rng.min_range = self.min_range rng.max_range = self.max_range rng.range = self.rolling_meters rng.header.frame_id = self.frame rng.field_of_view = 0.1 rng.header.stamp = rospy.Time.now() self.range_pub.publish( rng ) ranges = [] intensities = [] angle_start = 0.0 - self.field_of_view angle_stop = self.field_of_view for angle in linspace( angle_start, angle_stop, 10): ranges.append( self.rolling_meters ) intensities.append( 1.0 ) scan = LaserScan() scan.ranges = ranges scan.header.frame_id = self.frame scan.time_increment = 0; scan.range_min = self.min_range scan.range_max = self.max_range scan.angle_min = angle_start scan.angle_max = angle_stop scan.angle_increment = (angle_stop - angle_start) / 10 scan.intensities = intensities scan.scan_time = self.scan_time scan.header.stamp = rospy.Time.now() self.scan_pub.publish( scan )
def 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)
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
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
jntState = JointState() jntState.name = jntState_name jntState.effort = [0.0, 0.0, 0.1, 0.0] #/scan publisher scanPub = rospy.Publisher('scan',LaserScan) scanMsg = LaserScan() scanMsg.header.frame_id = 'laser' scanMsg.angle_min = 0.0 scanMsg.angle_max = 0.0 scanMsg.angle_increment = 0.0 scanMsg.time_increment = 0.0 scanMsg.scan_time = 0.0 scanMsg.range_min = 0.02 scanMsg.range_max = 1.5 scanMsg.intensities = [1.0] #Socket initialization MCAST_GRP = '239.133.1.206' MCAST_PORT = 2000 sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) sock.bind(('', MCAST_PORT)) mreq = struct.pack("4sl", socket.inet_aton(MCAST_GRP), socket.INADDR_ANY) sock.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, mreq) #robot joint state (joint angles in radians) left_wheel_angle = 0.0; right_wheel_angle = 0.0;
odoMsg.child_frame_id = '/odom' odoMsg.pose.covariance = [0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000.0] #/scan publisher scanPub = rospy.Publisher('scan',LaserScan) scanMsg = LaserScan() scanMsg.header.frame_id = 'ir_base' scanMsg.angle_min = 0.01 scanMsg.angle_max = 0.01 scanMsg.angle_increment = 0.02 scanMsg.time_increment = 0.0 scanMsg.scan_time = 0.0 scanMsg.range_min = 0.02 scanMsg.range_max = 1.5 scanMsg.intensities = [] #robot joint state (joint angles in radians) left_wheel_angle = 0.0; right_wheel_angle = 0.0; # Open a file logFile = open("log.txt", "r") # robot states w.r.t inertial frame robot_x_i = 0.0 robot_y_i = 0.0 robot_theta_i = 0.0