def main():
# Connect to Lidar unit
lidar = 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)
# Set up a SLAM display
viz = MapVisualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS, 'SLAM')
# Initialize an empty trajectory
trajectory = []
# Initialize empty map
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
# Create an iterator to collect scan data from the RPLidar
iterator = lidar.iter_scans()
# We will use these to store previous scan in case current scan is inadequate
previous_distances = None
previous_angles = None
# First scan is crap, so ignore it
next(iterator)
while True:
# Extract (quality, angle, distance) triples from current scan
items = [item for item in next(iterator)]
# Extract distances and angles from triples
distances = [item[2] for item in items]
print(distances)
angles = [item[1] for item in items]
# Update SLAM with current Lidar scan and scan angles if adequate
if len(distances) > MIN_SAMPLES:
slam.update(distances, 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,
scan_angles_degrees=previous_angles)
# Get current robot position
x, y, theta = slam.getpos()
# Get current map bytes as grayscale
slam.getmap(mapbytes)
# Display map and robot pose, exiting gracefully if user closes it
if not viz.display(x / 1000., y / 1000., theta, mapbytes):
exit(0)
# Shut down the lidar connection
lidar.stop()
lidar.disconnect()
def init_map():
if DEFAULT_SLAM_MAP:
return MapVisualizer(MAP_SIZE_PIXELS, MAP_SIZE_PIXELS, MAP_SIZE_METERS,
'SLAM')
else:
return MapVisualizer(MAP_SIZE_PIXELS, MAP_SIZE_PIXELS_1,
MAP_SIZE_METERS, 'SLAM')
def __init__(self):
# Connect to Lidar unit
self.lidar = lidarReader('/dev/mylidar', 115200)
self.mover = MotorController()
# Create an RMHC SLAM object with a laser model and optional robot model
self.slam = RMHC_SLAM(LaserModel(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
# Set up a SLAM display
self.viz = MapVisualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS, 'SLAM')
# Initialize empty map
self.mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
self.left_distance_table = np.zeros(80)
self.right_distance_table = np.zeros(80)
self.thread = threading.Thread(target=self.navigate, args=())
self.thread.start()
def run(self):
# Create an RMHC SLAM object with a laser model and optional robot model
self.slam = RMHC_SLAM(LaserModel(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
# Set up a SLAM display
self.viz = MapVisualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS, 'SLAM')
command_handlers = {
Message.lidar_data: self.update_map,
Message.get_map: self.process_get_map_command,
Message.reset_map: self.process_reset_map_command,
}
self.message_bus.subscribe(topic = self.lidar_updates_topic_name, subscriber=self.name)
self.config.log.info("slam subscribed to %s" % self.lidar_updates_topic_name)
while True:
msg = self.message_bus.receive(self.name)
handler = command_handlers.get(msg.cmd, self.handle_invalid_command_id)
handler(msg)
return
def __init__(self, visualize):
super().__init__('cont')
self.visualize = visualize
self.slam = RMHC_SLAM(LaserModel(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
if self.visualize:
self.viz = MapVisualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS, 'SLAM')
self.subscriber = self.create_subscription(LaserScan, '/scan',
self.lidarCallback, 10)
self.mapPublisher = self.create_publisher(OccupancyGrid, '/map', 1)
self.locPublisher = self.create_publisher(Pose, '/pose', 1)
print("Setup Finished")
self.previous_distances = None
self.previous_angles = None
self.mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
self.i = True
def main():
# Bozo filter for input args
if len(argv) < 4:
print('Usage: %s <dataset> <use_odometry> [random_seed]' % argv[0])
print('Example: %s exp2 1 9999' % argv[0])
exit(1)
# Grab input args
dataset = argv[1]
use_odometry = True if int(argv[2]) else False
seed = int(argv[3]) if len(argv) > 3 else 0
# Allocate byte array to receive map updates
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
# Load the data from the file
timestamps, lidars, odometries = load_data('.', dataset)
# Build a robot model if we want odometry
robot = Rover() if use_odometry else None
# Create a CoreSLAM object with laser params and optional robot object
slam = RMHC_SLAM(MinesLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS, random_seed=seed) \
if seed \
else Deterministic_SLAM(MinesLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
# Set up a SLAM display, named by dataset
viz = MapVisualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS, dataset)
# Pose will be modified in our threaded code
pose = [0,0,0]
# Launch the data-collection / update thread
thread = Thread(target=threadfunc, args=(robot, slam, timestamps, lidars, odometries if use_odometry else None, mapbytes, pose))
thread.daemon = True
thread.start()
# Loop forever,displaying current map and pose
while True:
# Display map and robot pose, exiting gracefully if user closes it
if not viz.display(pose[0]/1000., pose[1]/1000., pose[2], mapbytes):
exit(0)
def __init__(self):
super().__init__("slam_viz")
# Create subscription for the map
self.subscription1 = self.create_subscription(
Position, 'robot_pos', self.listener_callback_position, 1000)
self.subscription1 # prevent unused variable warning
# Create subscription for the robot position
self.subscription2 = self.create_subscription(
Map, 'world_map', self.listener_callback_map, 1000)
self.subscription2 # prevent unused variable warning
# setup the vizualistion tool (it will open a window showing the map and the robot within the map)
self.viz = MapVisualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS, "SLAM")
# keep track of where is the robot within the class
self.x = 0
self.y = 0
self.theta = 0
def __init__(self, messagequeue):
# Connect to Lidar unit
# when initializing the robot object, we first open lidar and start
# reading data from the lidar
self.lidar = lidarReader('/dev/mylidar', 115200)
self.mover = MotorController()
# Create an RMHC SLAM object with a laser model and optional robot model
self.slam = RMHC_SLAM(LaserModel(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
# Set up a SLAM display
self.viz = MapVisualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS, 'SLAM')
# Initialize empty map
self.mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
self.messages = messagequeue
# All the functions should run in the same process so that the variable can be shared
self.thread = threading.Thread(target=self.navigate, args=())
# the main-thread will exit without checking the sub-thread at the end of the main thread.
# At the same time, all sub-threads with the daemon value of True will end with the main thread, regardless of whether the operation is completed.
self.thread.daemon = True
self.navigating = True
self.thread.start()
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
class SlamVizualizer(Node):
"""
This node visualize the map received from the SLAM node, using the PyRoboViz librairie.
This node is quite hard to use, because it slows down a lot the process. It should not be used in live with the Jetson
This node also takes a parameter when launched, so that it can:
- first argument: name of the images to save
"""
def __init__(self):
super().__init__("slam_viz")
# Create subscription for the map
self.subscription1 = self.create_subscription(
Position, 'robot_pos', self.listener_callback_position, 1000)
self.subscription1 # prevent unused variable warning
# Create subscription for the robot position
self.subscription2 = self.create_subscription(
Map, 'world_map', self.listener_callback_map, 1000)
self.subscription2 # prevent unused variable warning
# setup the vizualistion tool (it will open a window showing the map and the robot within the map)
self.viz = MapVisualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS, "SLAM")
# keep track of where is the robot within the class
self.x = 0
self.y = 0
self.theta = 0
def listener_callback_map(self, map_message):
# get the map data from the message
map_data = bytearray(map_message.map_data)
# plot data using roboviz
if not self.viz.display(self.x, self.y, self.theta, map_data): exit(0)
def listener_callback_position(self, pos):
self.x = pos.x / 1000
self.y = pos.y / 1000
self.theta = pos.theta
from roboviz import MapVisualizer
from scipy.interpolate import interp1d
import numpy as np
if __name__ == '__main__':
# Connect to Lidar unit
lidar = 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)
# Set up a SLAM display
viz = MapVisualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS, 'SLAM')
# Initialize an empty trajectory
trajectory = []
# Initialize empty map
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
# Create an iterator to collect scan data from the RPLidar
iterator = lidar.iter_scans()
# We will use this to store previous scan in case current scan is inadequate
previous_distances = None
# First scan is crap, so ignore it
next(iterator)
from breezyslam.algorithms import RMHC_SLAM
from breezyslam.sensors import RPLidarA1 as LaserModel
from rplidar import RPLidar as Lidar
from roboviz import MapVisualizer
if __name__ == '__main__':
# Connect to Lidar unit
lidar = 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)
# Set up a SLAM display
viz = MapVisualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS, 'SLAM')
# Initialize an empty trajectory
trajectory = []
# Initialize empty map
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
# Create an iterator to collect scan data from the RPLidar
iterator = lidar.iter_scans()
# We will use these to store previous scan in case current scan is inadequate
previous_distances = None
previous_angles = None
# First scan is crap, so ignore it
def init_viz(self, width, height):
return MapVisualizer(int(width), int(height))
def start_simulation(self):
self.set_motor_speed(0.8, 0)
(errorCode, detectionState, detectedPoint, detectedObjectHandle,
detectedSurfaceNormalVector) = vrep.simxReadProximitySensor(
self.client_id, self.proximity_handle, vrep.simx_opmode_streaming)
e, data = vrep.simxGetStringSignal(self.client_id, "lidarMeasuredData",
vrep.simx_opmode_streaming)
self.check_error_code(e, "simxGetStringSignal lidar error")
# Create an RMHC SLAM object with a laser model and optional robot model
slam = RMHC_SLAM(LaserModel(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
# Set up a SLAM display
viz = MapVisualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS, 'SLAM')
# Initialize empty map
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
res, v0 = vrep.simxGetObjectHandle(self.client_id, 'Vision_sensor',
vrep.simx_opmode_oneshot_wait)
res, resolution, image = vrep.simxGetVisionSensorImage(
self.client_id, v0, 0, vrep.simx_opmode_streaming)
while vrep.simxGetConnectionId(self.client_id) != -1:
is_detected, distance, vector = self.get_proximity_data()
if is_detected:
print("distance: {} {}".format(distance, vector))
e, lidar_data, dist_data = self.get_lidar_data()
point_data_len = len(lidar_data)
dist_data_len = len(dist_data)
dist_data = dist_data[::-1]
print("points len: {}; dist len: {}".format(
point_data_len, dist_data_len))
self.draw_lidar_data(dist_data[0:-2])
# Update SLAM , with current Lidar scan
slam.update(dist_data[0:-2])
# Get current robot position
x, y, theta = slam.getpos()
print("x: {}; y: {}; theta: {}".format(x, y, theta))
# Get current map bytes as grayscale
slam.getmap(mapbytes)
# Display map and robot pose, exiting gracefully if user closes it
if not viz.display(x / 1000., y / 1000., theta, mapbytes):
exit(0)
err, resolution, image = vrep.simxGetVisionSensorImage(
self.client_id, v0, 0, vrep.simx_opmode_buffer)
if err == vrep.simx_return_ok:
img = np.array(image, dtype=np.uint8)
img.resize([resolution[1], resolution[0], 3])
fimg = cv2.flip(img, 0)
cv2.imshow('vision sensor', fimg)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
elif err == vrep.simx_return_novalue_flag:
print("no image yet")
pass
else:
print(err)
simulationTime = vrep.simxGetLastCmdTime(self.client_id)
time.sleep(0.1)
vrep.simxFinish(self.client_id)
class LidarProcessingNode(Node):
def __init__(self, visualize):
super().__init__('cont')
self.visualize = visualize
self.slam = RMHC_SLAM(LaserModel(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
if self.visualize:
self.viz = MapVisualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS, 'SLAM')
self.subscriber = self.create_subscription(LaserScan, '/scan',
self.lidarCallback, 10)
self.mapPublisher = self.create_publisher(OccupancyGrid, '/map', 1)
self.locPublisher = self.create_publisher(Pose, '/pose', 1)
print("Setup Finished")
self.previous_distances = None
self.previous_angles = None
self.mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
self.i = True
def lidarCallback(self, data):
# print("Received Lidar Message")
self.angle_min = data.angle_min
self.angle_max = data.angle_max
self.angle_increment = data.angle_increment
self.time_increment = data.time_increment
self.scan_time = data.scan_time
self.range_max = data.range_max
self.range_min = data.range_min
self.intensities = data.intensities
self.distances = list(data.ranges)
self.angle_max = self.angle_max - self.angle_min
self.angle_min = 0.0
#Filtering out points that are outside the max range or Nan.
self.angles = [
i * 180.0 / np.pi for i in np.arange(
self.angle_min, self.angle_max, self.angle_increment)
] + [self.angle_max * 180.0 / np.pi]
self._distances = [i * 1000.0 for i in self.distances]
self._distances, self.angles = (list(t) for t in zip(
*[x for x in zip(self._distances, self.angles) if x[0] <= 5000]))
# print(max(filteredDistances))
# print(len(filteredDistances))
# print(len(filteredAngles))
# if len(self.angles) != len(self.distances):
# self.angles = self.angles[:len(self.distances)]
# print(len(self.distances)
# print(self.angles)
if len(self._distances) > MIN_SAMPLES:
self.slam.update(self._distances, scan_angles_degrees=self.angles)
self.previous_distances = self._distances.copy()
self.previous_angles = self.angles.copy()
# If not adequate, use previous
elif previous_distances is not None:
self.slam.update(self.previous_distances,
scan_angles_degrees=self.previous_angles)
# self.slam.update(laserScanData)
x, y, theta = self.slam.getpos()
self.slam.getmap(self.mapbytes)
if self.visualize:
if not self.viz.display(x / 1000., y / 1000., theta,
self.mapbytes):
exit(0)
#Publishing Position data
pose = Pose()
pose.position.x = x
pose.position.y = y
pose.orientation.z = theta
self.locPublisher.publish(pose)
#Publishing Map data
map = OccupancyGrid()
map.header.stamp.sec = 0
map.header.stamp.nanosec = 0
map.header.frame_id = "1"
map.info.map_load_time.sec = 0
map.info.map_load_time.nanosec = 0
map.info.resolution = MAP_SIZE_PIXELS / MAP_SIZE_METERS
map.info.width = MAP_SIZE_PIXELS
map.info.height = MAP_SIZE_PIXELS
map.info.origin.position.x = 5000.0
map.info.origin.position.y = 5000.0
mapimg = np.reshape(np.frombuffer(self.mapbytes, dtype=np.uint8),
(MAP_SIZE_PIXELS, MAP_SIZE_PIXELS))
norm_const = 100 / mapimg.max()
norm_map = (np.absolute(mapimg.astype(float) * norm_const -
100.0)).astype(int)
# plt.matshow(norm_map)
# plt.show()
map.data = norm_map.flatten().tolist()
self.mapPublisher.publish(map)
from breezyslam.sensors import XVLidar as LaserModel
from xvlidar import XVLidar as Lidar
from roboviz import MapVisualizer
if __name__ == '__main__':
# Connect to Lidar unit
lidar = 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)
# Set up a SLAM display
display = MapVisualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS, 'SLAM')
# Initialize an empty trajectory
trajectory = []
# Initialize empty map
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
while True:
# Update SLAM with current Lidar scan, using first element of (scan, quality) pairs
slam.update([pair[0] for pair in lidar.getScan()])
# Get current robot position
x, y, theta = slam.getpos()
class Robot:
def __init__(self, messagequeue):
# Connect to Lidar unit
# when initializing the robot object, we first open lidar and start
# reading data from the lidar
self.lidar = lidarReader('/dev/mylidar', 115200)
self.mover = MotorController()
# Create an RMHC SLAM object with a laser model and optional robot model
self.slam = RMHC_SLAM(LaserModel(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
# Set up a SLAM display
self.viz = MapVisualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS, 'SLAM')
# Initialize empty map
self.mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
self.messages = messagequeue
# All the functions should run in the same process so that the variable can be shared
self.thread = threading.Thread(target=self.navigate, args=())
# the main-thread will exit without checking the sub-thread at the end of the main thread.
# At the same time, all sub-threads with the daemon value of True will end with the main thread, regardless of whether the operation is completed.
self.thread.daemon = True
self.navigating = True
self.thread.start()
# Construct Map should always run in the main process
def constructmap(self):
while True:
time.sleep(0.0001)
# Update SLAM with current Lidar scan, using first element of (scan, quality) pairs
self.slam.update(self.lidar.getdata())
#print(self.lidar.getdata())
# Get current robot position
x, y, theta = self.slam.getpos()
# Get current map bytes as grayscale
self.slam.getmap(self.mapbytes)
# Display map and robot pose, exiting gracefully if user closes it
if not self.viz.display(x / 1000., y / 1000., theta,
self.mapbytes):
break
def navigate(self):
while self.navigating:
time.sleep(
0.01) # (yield) allowing reading data from the serailport
if (
self.messages.empty()
): # The camera doesn't detect one traffic sign message.empty()
#if(True): # The camera doesn't detect one traffic sign message.empty()
front_too_close, left_too_close, right_too_close = False, False, False
if (self.lidar.angle_180 < 400):
front_too_close = True
left_angle = np.argmin(self.lidar.left_container)
right_angle = np.argmin(self.lidar.right_container)
if (self.lidar.left_container[left_angle] < 250):
left_too_close = True
if (self.lidar.right_container[right_angle] < 250):
right_too_close = True
if (front_too_close and left_too_close and right_too_close):
self.mover.backward()
elif (front_too_close):
if (self.lidar.angle_270 > self.lidar.angle_90):
self.mover.turn_left()
else:
self.mover.turn_right()
elif (left_too_close or right_too_close):
if (left_too_close):
self.mover.turn_right(left_angle)
else:
self.mover.turn_left(right_angle)
else:
self.mover.forward()
#print(self.lidar.left_container)
#print(front_too_close, left_too_close, right_too_close, self.lidar.angle_180)
else:
sign = self.messages.get() # get the detection id
if (sign == 1):
self.mover.stop()
elif (sign == 2):
self.mover.turn_right()
else:
self.mover.turn_left()
clientID, proximity_sensor2, vrep.simx_opmode_streaming)
#хэндлы сенсоров лидара
errorCode, vision_sensor1 = vrep.simxGetObjectHandle(
clientID, 'SICK_TiM310_sensor1', vrep.simx_opmode_oneshot_wait)
errorCode, vision_sensor2 = vrep.simxGetObjectHandle(
clientID, 'SICK_TiM310_sensor2', vrep.simx_opmode_oneshot_wait)
#объект алгоритма слама,
#параметыр лазера: 134 - количество считываемых точек, 5-частота, 270. - угол между сенсорами, 10 - максимальное расстояние обнаружения точек
#важно поставить map_quality = 1 (качество карты), иначе будет кровь из глаз
slam = RMHC_SLAM(Laser(134, 5, 270., 10),
MAP_SIZE_PIXELS,
MAP_SIZE_METERS,
map_quality=1)
viz = MapVisualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS,
'SLAM') #объект отображения построенной карты
mapbytes = bytearray(
MAP_SIZE_PIXELS *
MAP_SIZE_PIXELS) #массив байтов, который будет отображать карту
purpose = 0.3
v = 0.7
robot = pioner()
prev_pos_left = prev_pos_right = 0 #позиции левого и правого колеса
velocities = (
) #кортеж из изменения координат, изменения угла и изменения времени
vrep.simxStartSimulation(clientID,
vrep.simx_opmode_streaming) #начало симуляции
time.sleep(3) #немного поспим, чтобы v-rep успел загрузиться
print('Simulation starts')
def slamThread():
global SLAMrot
global SLAMvel
# Connect to Lidar unit
lidar = 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)
# Set up a SLAM display
viz = MapVisualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS, 'SLAM')
# Initialize an empty trajectory
trajectory = []
# Initialize empty map
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
# Create an iterator to collect scan data from the RPLidar
iterator = lidar.iter_scans()
# We will use these to store previous scan in case current scan is inadequate
previous_distances = None
previous_angles = None
# First scan is crap, so ignore it
next(iterator)
# start time
start_time = time.time()
prevTime = start_time
print("start")
while True:
# Extract (quality, angle, distance) triples from current scan
items = [item for item in next(iterator)]
# Extract distances and angles from triples
distances = [item[2] for item in items]
angles = [item[1] for item in items]
# Update SLAM with current Lidar scan and scan angles if adequate
if len(distances) > MIN_SAMPLES:
slam.update(distances, pose_change = ( (SLAMvel, SLAMrot, time.time() - prevTime)),scan_angles_degrees=angles)
prevTime = time.time()
previous_distances = copy.copy(distances)
previous_angles = copy.copy(angles)
print("updated - if")
print((SLAMvel, SLAMrot, time.time() - prevTime))
# If not adequate, use previous
elif previous_distances is not None:
slam.update(previous_distances, pose_change = ( (SLAMvel, SLAMrot, time.time() - prevTime)),scan_angles_degrees=previous_angles)
prevTime = time.time()
print("updated - else")
print((SLAMvel, SLAMrot, time.time() - prevTime))
# Get current robot position
x, y, theta = slam.getpos()
# Add new position to trajectory
trajectory.append((x, y))
# Get current map bytes as grayscale
slam.getmap(mapbytes)
if(time.time() - start_time > 30):
# Put trajectory into map as black pixels
for coords in trajectory:
x_mm, y_mm = coords
x_pix = mm2pix(x_mm)
y_pix = mm2pix(y_mm)
mapbytes[y_pix * MAP_SIZE_PIXELS + x_pix] = 0;
pgm_save('ok.pgm', mapbytes, (MAP_SIZE_PIXELS, MAP_SIZE_PIXELS))
exit(0)
return self._refresh() # image file image_filename = '/home/ubuntu/RCWeb/dash' HOST, PORT = "192.168.0.18", 50007 s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) s.connect((HOST, PORT)) # Create an RMHC SLAM object with a laser model and optional robot model slam = RMHC_SLAM(LaserModel(), MAP_SIZE_PIXELS, MAP_SIZE_METERS) # Set up a SLAM display viz = MapVisualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS, 'SLAM') # Initialize an empty trajectory trajectory = [] # Initialize empty map mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS) # We will use these to store previous scan in case current scan is inadequate previous_distances = None previous_angles = None # Pin Definitons: pwmPin = 18 # Broadcom pin 18 (P1 pin 12) backPin = 2 # Broadcom pin 23 (P1 pin 16) frontPin = 3
class Robot:
def __init__(self):
# Connect to Lidar unit
self.lidar = lidarReader('/dev/mylidar', 115200)
self.mover = MotorController()
# Create an RMHC SLAM object with a laser model and optional robot model
self.slam = RMHC_SLAM(LaserModel(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
# Set up a SLAM display
self.viz = MapVisualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS, 'SLAM')
# Initialize empty map
self.mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
self.left_distance_table = np.zeros(80)
self.right_distance_table = np.zeros(80)
self.thread = threading.Thread(target=self.navigate, args=())
self.thread.start()
def constructmap(self):
while True:
time.sleep(0.0001)
# Update SLAM with current Lidar scan, using first element of (scan, quality) pairs
self.slam.update(self.lidar.getdata())
# Get current robot position
x, y, theta = self.slam.getpos()
# Get current map bytes as grayscale
self.slam.getmap(self.mapbytes)
# Display map and robot pose, exiting gracefully if user closes it
if not self.viz.display(x / 1000., y / 1000., theta,
self.mapbytes):
break
exit(0)
def navigate(self):
while True:
time.sleep(
0.01) # (yield) allowing reading data from the serailport
front_too_close, left_too_close, right_too_close = False, False, False
if (self.lidar.angle_180 < 400):
front_too_close = True
left_angle = np.argmin(self.lidar.left_container)
right_angle = np.argmin(self.lidar.right_container)
if (self.lidar.left_container[left_angle] < 250):
left_too_close = True
if (self.lidar.right_container[right_angle] < 250):
right_too_close = True
if (front_too_close and left_too_close and right_too_close):
self.mover.backward()
elif (front_too_close):
if (self.lidar.angle_270 > self.lidar.angle_90):
self.mover.turn_left()
else:
self.mover.turn_right()
elif (left_too_close or right_too_close):
if (left_too_close):
self.mover.turn_right(left_angle)
else:
self.mover.turn_left(right_angle)
else:
self.mover.forward()
print(front_too_close, left_too_close, right_too_close,
self.lidar.angle_180)
