def laser_callback(self, msg):
'''
Function called each time a LaserScan message with topic "scan" arrives.
'''
# Save time
self.time = msg.header.stamp
# Project LaserScan to points in space
rng = np.array(msg.ranges)
ang = np.linspace(msg.angle_min, msg.angle_max, len(msg.ranges))
points = np.vstack((rng * np.cos(ang),
rng * np.sin(ang)))
# Filter long ranges
cond = rng < msg.range_max
points = points[:, rng < msg.range_max]
# Use split and merge to obtain lines and publish
self.lines = splitandmerge(points, split_thres=0.1,
inter_thres=0.3,
min_points=6,
dist_thres=0.12,
ang_thres=np.deg2rad(10))
# Have valid points
if self.lines is not None:
# Publish results
publish_lines(self.lines, self.pub_lines, frame='/robot',
time=msg.header.stamp, ns='scan_lines_robot', color=(0,0,1))
# Flag
self.new_laser = True
def laser_callback(self, msg):
'''
Function called each time a LaserScan message with topic "scan" arrives.
'''
# Project LaserScan to points in space
rng = np.array(msg.ranges)
ang = np.linspace(msg.angle_min, msg.angle_max, len(msg.ranges))
points = np.vstack((rng * np.cos(ang),
rng * np.sin(ang)))
msg.range_max = 3
self.pub_laser.publish(msg)
# Filter long ranges
points = points[:, rng < msg.range_max]
# Use split and merge to obtain lines and publish
lines = splitandmerge(points)
# # Publish results
# publish_lines(lines, self.pub_line, frame=msg.header.frame_id,
# time=msg.header.stamp, ns='scan_line', color=(1,0,0))
self.i = self.i + 1
nss = 'scan_line' + str(self.i)
#msg.header.frame_id = "d" + str(self.i)
#msg.header.frame_id = 'world'
publish_lines(lines, self.pub_line, frame=msg.header.frame_id,
time=msg.header.stamp, ns=nss, color=(1,0,0))
def laser_callback(self, msg):
'''
Function called each time a LaserScan message with topic "scan" arrives.
'''
# Project LaserScan to points in space
rng = np.array(msg.ranges)
ang = np.linspace(msg.angle_min, msg.angle_max, len(msg.ranges))
points = np.vstack((rng * np.cos(ang),
rng * np.sin(ang)))
msg.range_max = 3
self.pub_laser.publish(msg)
# Filter long ranges
points = points[:, rng < msg.range_max]
# Use split and merge to obtain lines and publish
lines = splitandmerge(points)
self.counter += 1
new_scan = 'scan_line' + str(self.counter)
#publish_lines(lines, self.pub_line, frame=msg.header.frame_id,
#time=msg.header.stamp, ns=new_scan, color=(1,0,0))
# Publish results
publish_lines(lines, self.pub_line, frame=msg.header.frame_id,
time=msg.header.stamp, ns='scan_line', color=(1,0,0))
def true_positif(image,mask,threshold,threshold_merge = 0.172,method = 'crop',slices = 100):
'''Input: an image, the corresponding mask, a method, a threshold and the number of slices.
Ouput : the number of voxels that are in our bounding box and in the perfect box.'''
if(method == 'crop'):
[left,right,top,bottom] = get_coordinates(image,threshold,slices)
elif(method == 'merge'):
image = splitandmerge(image,threshold_merge,method='Exponent')
[left,right,top,bottom] = get_coordinates(image,threshold,slices)
else:
raise Exception('Unknown method')
[p_left,p_right,p_top,p_bottom] = perfect_box_coord(mask)
tp_top = 0
tp_bottom = 0
tp_left = 0
tp_right = 0
total_area = (bottom-top)*(left-right)
if(p_left>left and p_left<right): tp_left = p_left
elif(p_left<left and left<p_right): tp_left = left
if(p_right<right and p_right>left): tp_right = p_right
elif(p_right>right and right>p_left): tp_right = right
if(p_top>top and p_top<bottom): tp_top = p_top
elif(p_top<top and top<p_bottom): tp_top = top
if(p_bottom<bottom and p_bottom>top): tp_bottom = p_bottom
elif(p_bottom>bottom and bottom>p_top): tp_bottom = bottom
return (tp_right-tp_left)*(tp_bottom-tp_top)
def percentage_cropped(image,threshold,threshold_merge = 0.172,method = 'crop',slices = 100):
'''Input: an image, a threshold, a method and a number of slices.
Ouput: the percentage of the image that has been cropped.'''
if(method == 'crop'):
[left,right,top,bottom] = get_coordinates(image,threshold,slices)
elif(method == 'merge'):
image = splitandmerge(image,threshold_merge,method='Exponent')
[left,right,top,bottom] = get_coordinates(image,threshold,slices)
else:
raise Exception('Unknown method')
(rows,cols) = (image.shape[0],image.shape[1])
(box_rows,box_cols) = (bottom-top,right-left)
return (1-((box_rows*box_cols)/(rows*cols)))*100
def laser_callback(self, msg):
'''
Function called each time a LaserScan message with topic "scan" arrives.
'''
# Save time
self.time = msg.header.stamp
# Project LaserScan to points in space
rng = np.array(msg.ranges)
ang = np.linspace(msg.angle_min, msg.angle_max, len(msg.ranges))
points = np.vstack((rng * np.cos(ang), rng * np.sin(ang)))
# Filter long ranges
cond = rng < msg.range_max
points = points[:, rng < msg.range_max]
# Use split and merge to obtain lines and publish
self.lines = splitandmerge(points,
split_thres=0.1,
inter_thres=0.3,
min_points=6,
dist_thres=0.12,
ang_thres=np.deg2rad(10))
# Have valid points
if self.lines is not None:
# Publish results
publish_lines(self.lines,
self.pub_lines,
frame=msg.header.frame_id,
time=msg.header.stamp,
ns='scan_lines',
color=(1, 0, 0))
publish_lines(self.lines,
self.pub_lines,
frame='/mean_particle',
time=msg.header.stamp,
ns='scan_lines_mean',
color=(0, 0, 1))
# Flag
self.new_laser = True
def laser_callback(self, msg):
'''
Function called each time a LaserScan message with topic "scan" arrives.
'''
# Save time
self.time = msg.header.stamp
# Project LaserScan to points in space
rng = np.array(msg.ranges)
ang = np.linspace(msg.angle_min, msg.angle_max, len(msg.ranges))
points = np.vstack((rng * np.cos(ang),
rng * np.sin(ang)))
# Filter long ranges
points = points[:, rng < msg.range_max]
# Use split and merge to obtain lines and publish
self.lines = splitandmerge(points, split_thres=0.1,
inter_thres=0.3,
min_points=6,
dist_thres=0.12,
ang_thres=np.deg2rad(10))
# Have valid points
if self.lines is not None:
# Transform line to robot frame
for i in range(0,self.lines.shape[0]):
point1S = np.append(self.lines[i,0:2], 0)
point2S = np.append(self.lines[i,2:4], 0)
point1R = comp(self.robotToSensor, point1S)
point2R = comp(self.robotToSensor, point2S)
self.lines[i,:] = np.append(point1R[0:2],point2R[0:2])
# Publish results
publish_lines(self.lines, self.pub_lines, frame='/robot',
time=msg.header.stamp, ns='scan_lines_robot', color=(0,0,1))
# Flag
self.new_laser = True
def percentage_outside_box_crop(image,mask,threshold,threshold_merge = 0.172,method = 'crop',slices = 100):
'''Input: an image, the corresponding mask, a method, a threshold and the number of slices.
Ouput: percentage of the organs that our outside the box.'''
if(method == 'crop'):
[left,right,top,bottom] = get_coordinates(image,threshold,slices)
elif(method == 'merge'):
image = splitandmerge(image,threshold_merge,method='Exponent')
[left,right,top,bottom] = get_coordinates(image,threshold,slices)
else:
raise Exception('Unknown method')
[p_left,p_right,p_top,p_bottom] = perfect_box_coord(mask)
h_top = 0
h_bottom = 0
w_left = 0
w_right = 0
total_area = (bottom-top)*(left-right)
if(p_left<left): w_left = left-p_left
if(p_right>right): w_right = p_right-right
if(p_top<top): h_top = top-p_top
if(p_bottom>bottom): h_bottom = p_bottom-bottom
h = h_top+h_bottom
w = w_left+w_right
if(h == 0 and w == 0):
return 0
elif(h == 0):
h = bottom-top
elif(w == 0):
w = left-right
return ((h*w)/total_area)*100
def laser_callback(self, msg):
'''
Function called each time a LaserScan message with topic "scan" arrives.
'''
# Project LaserScan to points in space
rng = np.array(msg.ranges)
ang = np.linspace(msg.angle_min, msg.angle_max, len(msg.ranges))
points = np.vstack((rng * np.cos(ang),
rng * np.sin(ang)))
# Filter long ranges
cond = rng < msg.range_max
points = points[:, rng < msg.range_max]
# Use split and merge to obtain lines and publish
lines = splitandmerge(points, split_thres=0.1,
inter_thres=0.3,
min_points=6,
dist_thres=0.12,
ang_thres=np.deg2rad(10))
# Publish results
publish_lines(lines, self.pub_line, frame=msg.header.frame_id,
time=msg.header.stamp, ns='scan_line', color=(1,0,0))
def scan_callback(self, msg):
s = self.sensor_spacing
# Range
rng = np.array(msg.ranges)
rng = rng[0:len(msg.ranges):s]
# Angle
ang = np.linspace(msg.angle_min, msg.angle_max, len(msg.ranges))
ang = ang[0:len(msg.ranges):s]
# Point features
points = np.vstack((rng * np.cos(ang),
rng * np.sin(ang)))
# Set Maximum sensing range
msg.range_max = self.sensor_max_range
# Acquire points within the maximum range
points = points[:, rng < msg.range_max]
# Split & Merge aLgorithm to get the line features
lines = splitandmerge(points)
#print lines
self.i = self.i + 1
nss = 'scan_line' + str(self.i)
utils.publish_lines(lines, self.pub_line, frame=msg.header.frame_id,
time=msg.header.stamp, ns=nss, color=(1,0,0), marker_id=1, thickness=0.05)
###################################################################
##### Compute stuff for obstacle avoidance - Daudt's method 2 #####
###################################################################
th_up = 1.5
# prune points
ang = ang[rng < msg.range_max]
rng = rng[rng < msg.range_max]
if rng.size < 1:
self.oa_v = 1
self.oa_w *= 0.8
return
# Linear velocity factor
min_dist = np.min(rng)
# Angular velocity factor
w = np.where(rng == min_dist)
angle = np.mean(ang[w])
if min_dist < th_up:
if angle <= 0:
self.oa_w = 1.0
else:
self.oa_w = -1.0
else:
self.oa_w *= 0.8
