class Slam(Node):
"""
SLAM NODE
This node is in charge of receiving data from the LIDAR and creating a map of the environment
while locating the robot in the map.
INPUT
- Bottle detected by the Neuron (to be removed soon)
- Lidar detections
OUTPUT
- Robot position: /robot_pos
- Map as a byte array: /world_map
(this map can be interpred using the map_utils.py package !)
"""
def __init__(self):
super().__init__('slam_node')
# Create second subscription (SLAM)
self.subscription2 = self.create_subscription(
LidarData, 'lidar_data', self.listener_callback_lidar, 100)
self.subscription2 # prevent unused variable warning
# Create third subscription (Motor speeds to compute robot pose change)
self.subscription3 = self.create_subscription(
MotorsSpeed, 'motors_speed', self.listener_callback_motorsspeed,
1000)
self.subscription3 # prevent unused variable warning
# create subscription to control map_quality
self.slam_control_sub = self.create_subscription(
String, 'slam_control', self.slam_control_callback, 1000)
# Create publisher for the position of the robot, and for the map
self.publisher_position = self.create_publisher(
Position, 'robot_pos', 10)
self.publisher_map = self.create_publisher(Map, 'world_map', 10)
# Initialize parameters for slam
laser = LaserModel(detection_margin=DETECTION_MARGIN,
offset_mm=OFFSET_MM)
self.slam = RMHC_SLAM(laser,
MAP_SIZE_PIXELS,
MAP_SIZE_METERS,
map_quality=MAP_QUALITY,
hole_width_mm=OBSTACLE_WIDTH_MM,
x0_mm=X0,
y0_mm=Y0,
theta0=0,
sigma_xy_mm=DEFAULT_SIGMA_XY_MM)
self.trajectory = []
self.mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
self.previous_distances = None
self.previous_angles = None
self.robot_pose_change = np.array([0.0, 0.0, 0.0])
self.is_map_quality_high = True
# DEBUG parameters
self.map_index = 0
self.previous_pos = (0, 0, 0)
self.s = time.time()
print("SLAM is starting")
def slam_control_callback(self, msg):
"""Called to changed the map quality
"""
if msg.data == "save_state":
print("Slam is saving current state")
self.last_valid_map = self.mapbytes.copy()
self.last_valid_pos = self.slam.getpos()
elif msg.data == "recover_state":
# METHOD 1
self.slam.setmap(self.last_valid_map)
## self.slam.setpos(self.last_valid_pos)
elif msg.data == "freeze":
self.last_valid_map = self.mapbytes.copy()
self.last_valid_pos = self.slam.getpos()
elif msg.data == "unfreeze":
self.slam.setmap(self.last_valid_map)
# self.slam.setpos(self.last_valid_pos)
return
##########################
print("changing quality")
if self.is_map_quality_high:
#self.previous_map = self.mapbytes.copy()
self.slam.map_quality = 0
self.slam.sigma_xy_mm = 5 * DEFAULT_SIGMA_XY_MM
else:
#self.slam.setmap(self.previous_map)
self.slam.map_quality = MAP_QUALITY
self.slam.sigma_xy_mm = DEFAULT_SIGMA_XY_MM
self.is_map_quality_high = not self.is_map_quality_high
def listener_callback_motorsspeed(self, msg):
"""
Must compute the robot pose change since the last time the data was collected from the motor
The input are the motors speed and the time change.
Documentation of the SLAM library says :
"pose_change is a tuple (dxy_mm, dtheta_degrees, dt_seconds) computed from odometry"
"""
dx_left = msg.RADIUS * msg.left * (0.1047) * msg.time_delta
dx_right = msg.RADIUS * msg.right * (0.1047) * msg.time_delta
delta_r = 1 / 2 * (dx_left + dx_right) # [mm]
delta_d = -(dx_right - dx_left)
delta_theta = np.arctan(delta_d / msg.LENGTH) * 57.2958 # [deg]
self.robot_pose_change += [delta_r, delta_theta, msg.time_delta]
def listener_callback_lidar(self, msg):
# transform angles to $FORMAT1
angles = np.array(msg.angles)
angles = list(
(angles + 180) % 360) # because LIDAR is facing the robot
distances = list(msg.distances)
# Update SLAM with current Lidar scan and scan angles if adequate
if len(distances) > MIN_SAMPLES:
e = time.time()
self.s = e
self.slam.update(distances,
scan_angles_degrees=angles,
pose_change=tuple(self.robot_pose_change))
# self.analyse_odometry()
self.robot_pose_change = np.array([0.0, 0.0, 0.0])
self.previous_distances = distances.copy()
self.previous_angles = angles.copy()
elif self.previous_distances is not None:
# If not adequate, use previous
print("Slam NOT updating")
self.slam.update(self.previous_distances,
scan_angles_degrees=self.previous_angles)
# Get current robot position and current map bytes as grayscale
x, y, theta = self.slam.getpos()
self.slam.getmap(self.mapbytes)
# Send topics
pos = Position()
pos.x = float(x)
pos.y = float(y)
pos.theta = float(theta)
self.publisher_position.publish(pos)
map_message = Map()
map_message.map_data = self.mapbytes
map_message.index = self.map_index
self.publisher_map.publish(map_message)
print("Map sent: ", self.map_index)
self.map_index += 1
# debug functions here (do NOT call them on real runs)
def analyse_odometry(self):
"""Compare the odometry resuls with the change in position according to SLAM.
For this function to work, self.robot_pose_change must be set to something different than zero.
And the variabe self.previous_pos must exist
"""
x, y, theta = self.slam.getpos()
x_old, y_old, theta_old = self.previous_pos
dr = np.sqrt((x - x_old)**2 + (y - y_old)**2)
dtheta = theta - theta_old
print("-------")
print("SLAM change of pos: {:.3f} {:.3f}".format(dr, dtheta))
print("Odometry change of pos: {:.3f} {:.3f} ".format(
self.robot_pose_change[0], self.robot_pose_change[1]))
self.previous_pos = (x, y, theta)
def find_lidar_range(self, distances, angles):
"""Finds the critical angles of the LIDAR and prints them.
Careful, this function works (currently) only with the previous angles format.
"""
(i1, i2) = lidar_utils.get_valid_lidar_range(distances=distances,
angles=angles,
n_points=10)
print('indexes : {} : {} with angles {} - {}'.format(
i1, i2, angles[i1], angles[i2]))
class narlam:
def __init__(self):
self.flag = 0
self.pause = 0
self.lidar = Lidar(LIDAR_DEVICE)
self.slam = RMHC_SLAM(LaserModel(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
#self.viz = MapVisualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS, 'SLAM')
self.trajectory = []
self.mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
self.iterator = self.lidar.iter_scans()
self.previous_distances = None
self.x = 0.0
self.y = 0.0
self.theta = 0.0
def slam_no_map(self, path_map, map_name_pgm, map_name_png):
self.flag = 0
self.pause = 0
path_map_name = path_map + '/' + map_name_pgm # map for reusing
next(self.iterator)
while True:
if self.flag == 1:
break
if self.pause == 1:
continue
items = [item for item in next(self.iterator)]
distances = [item[2] for item in items]
angles = [item[1] for item in items]
if len(distances) > MIN_SAMPLES:
f = interp1d(angles, distances, fill_value='extrapolate')
distances = list(f(np.arange(360)))
self.slam.update(distances)
self.previous_distances = distances.copy()
elif self.previous_distances is not None:
self.slam.update(self.previous_distances)
self.x, self.y, local_theta = self.slam.getpos()
local_theta = local_theta % 360
if local_theta < 0:
self.theta = 360 + local.theta
else:
self.theta = local_theta
self.slam.getmap(self.mapbytes)
# On SLAM thread termination, save map image in the map directory
# PNG format(To see/pathplannig)
image = Image.frombuffer('L', (MAP_SIZE_PIXELS, MAP_SIZE_PIXELS),
self.mapbytes, 'raw', 'L', 0, 1)
image.save(path_map + '/' + map_name_png)
# PGM format(To reuse map)
pgm_save(path_map_name, self.mapbytes,
(MAP_SIZE_PIXELS, MAP_SIZE_PIXELS))
self.lidar.stop()
self.lidar.disconnect()
def slam_yes_map(self, path_map, map_name):
self.flag = 0
self.pause = 0
path_map_name = path_map + '/' + map_name
map_bytearray, map_size = pgm_load(path_map_name)
self.slam.setmap(map_bytearray)
next(self.iterator)
while True:
if self.flag == 1:
break
if self.pause == 1:
pass
items = [item for item in next(self.iterator)]
distances = [item[2] for item in items]
angles = [item[1] for item in items]
if len(distances) > MIN_SAMPLES:
f = interp1d(angles, distances, fill_value='extrapolate')
distances = list(f(np.arange(360)))
self.slam.update(distances, should_update_map=False)
self.previous_distances = distances.copy()
elif self.previous_distances is not None:
self.slam.update(self.previous_distances,
should_update_map=False)
self.x, local_y, local_theta = self.slam.getpos()
self.y = MAP_SIZE_METERS * 1000 - local_y
local_theta = local_theta % 360
if local_theta < 0:
local_theta = 360 + local.theta
else:
local_theta = local_theta
#6/11 -> we found that the vehicle's angle was reversed on the map
self.theta = (local_theta + 180) % 360
self.slam.getmap(self.mapbytes)
self.lidar.stop()
self.lidar.disconnect()
def run_slam(LIDAR_DEVICE, MAP_SIZE_PIXELS, MAP_SIZE_METERS, mapbytes,
posbytes, odometry, command, state, STATES):
MIN_SAMPLES = 200
# Connect to Lidar unit
lidar = start_lidar(LIDAR_DEVICE)
# Create an RMHC SLAM object with a laser model and optional robot model
slam = RMHC_SLAM(LaserModel(),
MAP_SIZE_PIXELS,
MAP_SIZE_METERS,
map_quality=_DEFAULT_MAP_QUALITY,
hole_width_mm=_DEFAULT_HOLE_WIDTH_MM,
random_seed=None,
sigma_xy_mm=_DEFAULT_SIGMA_XY_MM,
sigma_theta_degrees=_DEFAULT_SIGMA_THETA_DEGREES,
max_search_iter=_DEFAULT_MAX_SEARCH_ITER)
# Initialize empty map
mapbytes_ = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
# Send command to start scan
lidar.start_scan()
# We will use these to store previous scan in case current scan is inadequate
previous_distances = None
previous_angles = None
pose_change = (0, 0, 0)
pose_change_find = (0, 0, 0)
iter_times = 1
while command[0] != b'qa':
# read a scan
items = lidar.scan()
# Extract distances and angles from triples
distances = [item[2] for item in items]
angles = [item[1] for item in items]
if state[0] == STATES['stop']:
if command[0] == b'lm':
mapbytes_find = mapbytes
pose_change_find = find_slam_pos(lidar, MAP_SIZE_PIXELS,
MAP_SIZE_METERS,
mapbytes_find,
pose_change_find,
iter_times / 2.0, slam)
iter_times = iter_times + 1.0
print(pose_change_find)
pose_change = pose_change_find
pose_change_find = (0, 0, 0)
'''
items = lidar.scan()
# Extract distances and angles from triples
distances = [item[2] for item in items]
angles = [item[1] for item in items]
slam.update(distances, pose_change, scan_angles_degrees=angles)
'''
if (iter_times > 10):
command[0] = b'w'
print(slam)
slam.setmap(mapbytes_find)
pose_change = pose_change_find
pose_change_find = (0, 0, 0)
print('ennnnnnnnnnnnnnnnnnd')
print(pose_change)
iter_times = 1
else:
# Update SLAM with current Lidar scan and scan angles if adequate
if len(distances) > MIN_SAMPLES:
slam.update(distances, pose_change, scan_angles_degrees=angles)
previous_distances = distances.copy()
previous_angles = angles.copy()
# If not adequate, use previous
elif previous_distances is not None:
slam.update(previous_distances,
pose_change,
scan_angles_degrees=previous_angles)
#pass the value into point_cal function
global point_a, point_b
global current_theta_o
# Get current robot position
x, y, theta = slam.getpos()
current_theta_o = theta
point_a = x - 5000
point_b = y - 5000
# Calculate robot position in the map
xbytes = int(x / 1000. / MAP_SIZE_METERS * MAP_SIZE_PIXELS)
ybytes = int(y / 1000. / MAP_SIZE_METERS * MAP_SIZE_PIXELS)
# Update robot position
posbytes[xbytes + ybytes * MAP_SIZE_PIXELS] = 255
#print('LIDAR::', command[0], x,y,theta)
pose_change = (odometry[0], odometry[1] / np.pi * 180, odometry[2])
# Get current map bytes as grayscale
slam.getmap(mapbytes_)
# Update map
mapbytes[:] = mapbytes_
# Shut down the lidar connection
lidar.stop()
lidar.disconnect()
print('LIDAR::thread ends')
class narlam:
def __init__(self):
self.flag = 0
self.lidar = Lidar(LIDAR_DEVICE)
self.slam = RMHC_SLAM(LaserModel(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
self.viz = MapVisualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS,
'SLAM MAP') # no visualizer needed
self.trajectory = []
self.mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
self.iterator = self.lidar.iter_scans()
self.previous_distances = None
self.previous_angles = None
self.x = 0.0
self.y = 0.0
self.theta = 0.0
def slam_no_map(self, path_map_name):
# doing slam with building maps from zero simultaneously
next(self.iterator)
while True:
if self.flag == 1:
break
items = [item for item in next(self.iterator)]
distances = [item[2] for item in items]
angles = [item[1] for item in items]
if len(distances) > MIN_SAMPLES:
self.slam.update(distances, scan_angles_degrees=angles)
self.previous_distances = distances.copy()
self.previous_angles = angles.copy()
elif self.previous_distances is not None:
self.slam.update(self.previous_distances,
scan_angles_degrees=self.previous_angles)
self.x, local_y, local_theta = self.slam.getpos()
local_theta = local_theta % 360
if local_theta < 0:
self.theta = 360 + local.theta
else:
self.theta = local_theta
self.slam.getmap(self.mapbytes)
# save map generated by slam
image = Image.frombuffer('L', (MAP_SIZE_PIXELS, MAP_SIZE_PIXELS),
self.mapbytes, 'raw', 'L', 0, 1)
image.save(path_map_name)
self.lidar.stop()
self.lidar.disconnect()
def slam_yes_map(self, path_map_name):
# doing localization only, with pre-built map image file.
with open(path_map_name, "rb") as map_img:
f = map_img.read()
b = bytearray(f)
self.slam.setmap(b)
next(self.iterator)
while True:
if self.flag == 1:
break
items = [item for item in next(self.iterator)]
distances = [item[2] for item in items]
angles = [item[1] for item in items]
if len(distances) > MIN_SAMPLES:
self.slam.update(distances,
scan_angles_degrees=angles,
should_update_map=False)
self.previous_distances = distances.copy()
self.previous_angles = angles.copy()
elif self.previous_distances is not None:
self.slam.update(self.previous_distances,
scan_angles_degrees=self.previous_angles,
should_update_map=False)
self.x, self.y, self.theta = self.slam.getpos()
self.lidar.stop()
self.lidar.disconnect()
