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
