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 publish_results(self):
'''
Publishes all results from the filter.
'''
# Map of the room
map_lines = get_map()
publish_lines(map_lines, self.pub_lines, frame='/world', ns='map', color=(0,1,0))
msg_odom, msg_ellipse, trans, rot = get_ekf_msgs(self.ekf)
self.pub_odom.publish(msg_odom)
self.pub_uncertainity.publish(msg_ellipse)
self.tfBroad.sendTransform(translation = trans,
rotation = rot,
time = self.time,
child = '/robot',
parent = '/world')
def publish_results(self):
'''
Publishes all results from the filter.
'''
# Ground turth of the map of the room
publish_lines(self.map, self.pub_lines, frame='/world', ns='gt_map', color=(0.3,0.3,0.3))
msg_odom, msg_ellipse, trans, rot, room_map = get_ekf_msgs(self.ekf)
publish_lines(room_map, self.pub_lines, frame='/world', ns='map', color=(0,1,0))
self.pub_odom.publish(msg_odom)
self.pub_uncertainity.publish(msg_ellipse)
self.tfBroad.sendTransform(translation = trans,
rotation = rot,
time = self.time,
child = '/robot',
parent = '/world')
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 publish_results(self):
'''
Publishes all results from the filter.
'''
# Map of the room
map_lines = get_map()
publish_lines(map_lines,
self.pub_lines,
frame='/world',
ns='map',
color=(0, 1, 0))
# Particles and biggest weighted particle
msg, msg_mean, msg_odom, trans, rot = get_particle_msgs(
self.part_filter)
self.pub_particles.publish(msg)
self.pub_big_particle.publish(msg_mean)
self.pub_odom.publish(msg_odom)
self.tfBroad.sendTransform(translation=trans,
rotation=rot,
time=self.time,
child='/mean_particle',
parent='/world')
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))
