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 lines_callback(self, msg):
"""Callback to a LaserScan message with topic "scan"."""
# Save time
self.linestime = msg.header.stamp
# Get the lines
if len(msg.points) > 0:
# Structure for the lines
self.lines = np.zeros((len(msg.points) / 2, 4))
for i in range(0, len(msg.points) / 2):
# Get start and end points
pt1 = msg.points[2 * i]
pt2 = msg.points[2 * i + 1]
# Transform to robot frame
pt1R = comp(self.robot2sensor, [pt1.x, pt1.y, 0.0])
pt2R = comp(self.robot2sensor, [pt2.x, pt2.y, 0.0])
# Append to line list
self.lines[i, :2] = pt1R[:2]
self.lines[i, 2:] = pt2R[:2]
# Flag
self.new_laser = True
# Publish
publish_lines(self.lines, self.pub_lines,
frame='/robot',
time=msg.header.stamp,
ns='lines_robot', color=(0, 0, 1))
def publish_results(self):
'''
Publishes all results from the filter.
'''
# Ground turth of the map of the room
publish_lines(self.map,
self.pub_map_gt,
frame='/world',
ns='gt_map',
color=(0.3, 0.3, 0.3))
odom, ellipse, trans, rot, room_map = get_ekf_msgs(self.ekf)
publish_lines(room_map,
self.pub_map,
frame='/world',
ns='map',
color=(0, 1, 0))
self.pub_odom.publish(odom)
self.pub_uncertainity.publish(ellipse)
self.tfBroad.sendTransform(translation=trans,
rotation=rot,
time=self.time,
child='/robot',
parent='/world')
def cbk_lines(self, msg):
"""Republish the laser scam in the /robot frame."""
# Save time
self.linestime = msg.header.stamp
# Get the lines
if len(msg.points) > 0:
# Structure for the lines
self.lines = np.zeros((len(msg.points) / 2, 4))
for i in range(0, len(msg.points)/2):
# Get start and end points
pt1 = msg.points[2*i]
pt2 = msg.points[2*i+1]
# Transform to robot frame
pt1R = funcs.comp(self.robot2sensor, [pt1.x, pt1.y, 0.0])
pt2R = funcs.comp(self.robot2sensor, [pt2.x, pt2.y, 0.0])
# Append to line list
self.lines[i, :2] = pt1R[:2]
self.lines[i, 2:] = pt2R[:2]
# Flag
self.new_laser = True
# Publish
funcs.publish_lines(self.lines, self.pub_lines, frame='robot',
time=msg.header.stamp, ns='lines_robot',
color=(0, 0, 1))
def publish_results(self, lines):
'''
Publishes all results from the filter.
'''
if self.time is not None:
time = rospy.Time.now()
# Map of the room
map_lines = get_dataset3_map()
publish_lines(map_lines, self.pub_map, frame='/world', ns='map',
color=(0,1,0))
# Particles and biggest weighted particle
msg, msg_mean, msg_odom, trans, rot, msg_wei = get_particle_msgs(self.part_filter,
time)
self.pub_particles.publish(msg)
self.pub_big_particle.publish(msg_mean)
self.pub_odom.publish(msg_odom)
self.pub_wei.publish(msg_wei)
self.tfBroad.sendTransform(translation = trans,
rotation = rot,
time = time,
parent = 'world',
child = 'mean_particle')
# Publish scanned lines
if lines is not None:
publish_lines(lines, self.pub_lines_mean, frame='mean_particle',
time=time, ns='scan_lines_mean', color=(0,0,1))
def lines_callback(self, msg):
'''
Function called each time a LaserScan message with topic "scan" arrives.
'''
# Save time
self.linestime = msg.header.stamp
# Get the lines
if len(msg.points) > 0:
# Structure for the lines
self.lines = np.zeros((len(msg.points) / 2, 4))
for i in range(0, len(msg.points)/2):
# Get start and end points
pt1 = msg.points[2*i]
pt2 = msg.points[2*i+1]
# Transform to robot frame
pt1R = comp(self.robot2sensor, [pt1.x, pt1.y, 0.0])
pt2R = comp(self.robot2sensor, [pt2.x, pt2.y, 0.0])
# Append to line list
self.lines[i, :2] = pt1R[:2]
self.lines[i, 2:] = pt2R[:2]
# Flag
self.new_laser = True
# Publish
publish_lines(self.lines, self.pub_lines,
frame = '/robot',
time = msg.header.stamp,
ns = 'lines_robot', color = (0,0,1))
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 publish_results(self):
"""Publishe results from the filter."""
# Map of the room (ground truth)
funcs.publish_lines(self.ekf.map, self.pub_lines, frame='world',
ns='map', color=(0, 1, 0))
# Get filter data
odom, ellipse, trans, rot, dummy = funcs.get_ekf_msgs(self.ekf)
# Publish results
self.pub_odom.publish(odom)
self.pub_uncertainity.publish(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_map_gt, frame='/world', ns='gt_map', color=(0.3,0.3,0.3))
odom, ellipse, trans, rot, room_map = get_ekf_msgs(self.ekf)
publish_lines(room_map, self.pub_map, frame='/world', ns='map', color=(0,1,0))
self.pub_odom.publish(odom)
self.pub_uncertainity.publish(ellipse)
self.tfBroad.sendTransform(translation = trans,
rotation = rot,
time = self.time,
child = '/robot',
parent = '/world')
def publish_results(self, lines):
'''
Publishes all results from the filter.
'''
if self.time is not None:
time = rospy.Time.now()
# Map of the room
map_lines = get_dataset3_map()
publish_lines(map_lines,
self.pub_map,
frame='/world',
ns='map',
color=(0, 1, 0))
# Particles and biggest weighted particle
msg, msg_mean, msg_odom, trans, rot, msg_wei = get_particle_msgs(
self.part_filter, time)
self.pub_particles.publish(msg)
self.pub_big_particle.publish(msg_mean)
self.pub_odom.publish(msg_odom)
self.pub_wei.publish(msg_wei)
self.tfBroad.sendTransform(translation=trans,
rotation=rot,
time=time,
parent='world',
child='mean_particle')
# Publish scanned lines
if lines is not None:
publish_lines(lines,
self.pub_lines_mean,
frame='mean_particle',
time=time,
ns='scan_lines_mean',
color=(0, 0, 1))
