169 lines
4.8 KiB
Matlab
169 lines
4.8 KiB
Matlab
%
|
|
% logdemo.m : BreezySLAM Matlab demo. Reads logfile with odometry and scan
|
|
% data from Paris Mines Tech and displays the map and robot
|
|
% position in real time.
|
|
%
|
|
% For details see
|
|
%
|
|
% @inproceedings{coreslam-2010,
|
|
% author = {Bruno Steux and Oussama El Hamzaoui},
|
|
% title = {CoreSLAM: a SLAM Algorithm in less than 200 lines of C code},
|
|
% booktitle = {11th International Conference on Control, Automation,
|
|
% Robotics and Vision, ICARCV 2010, Singapore, 7-10
|
|
% December 2010, Proceedings},
|
|
% pages = {1975-1979},
|
|
% publisher = {IEEE},
|
|
% year = {2010}
|
|
% }
|
|
%
|
|
% Copyright (C) 2014 Simon D. Levy
|
|
%
|
|
% This code is free software: you can redistribute it and/or modify
|
|
% it under the terms of the GNU Lesser General Public License as
|
|
% published by the Free Software Foundation, either version 3 of the
|
|
% License, or (at your option) any later version.
|
|
%
|
|
% This code is distributed in the hope that it will be useful,
|
|
% but WITHOUT ANY WARRANTY without even the implied warranty of
|
|
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
% GNU General Public License for more details.
|
|
%
|
|
% You should have received a copy of the GNU Lesser General Public License
|
|
% along with this code. If not, see <http://www.gnu.org/licenses/>.
|
|
|
|
function logdemo(dataset, use_odometry, seed)
|
|
|
|
% Params
|
|
|
|
MAP_SIZE_PIXELS = 800;
|
|
MAP_SIZE_METERS = 32;
|
|
ROBOT_SIZE_PIXELS = 10;
|
|
|
|
% Grab input args
|
|
|
|
if nargin < 2
|
|
use_odometry = 0;
|
|
end
|
|
|
|
if nargin < 3
|
|
seed = 0;
|
|
end
|
|
|
|
% Load data from file
|
|
[times, scans, odometries] = load_data(dataset);
|
|
|
|
% Build a robot model if we want odometry
|
|
robot = [];
|
|
if use_odometry
|
|
robot = MinesRover();
|
|
end
|
|
|
|
% Create a CoreSLAM object with laser params and optional robot object
|
|
if seed
|
|
slam = RMHC_SLAM(MinesLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS, seed);
|
|
else
|
|
slam = Deterministic_SLAM(MinesLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS);
|
|
end
|
|
|
|
% Initialize previous time for delay
|
|
prevTime = 0;
|
|
|
|
% Loop over scans
|
|
for scanno = 1:size(scans, 1)
|
|
|
|
if use_odometry
|
|
|
|
% Convert odometry to velocities
|
|
[velocities,robot] = robot.computeVelocities(odometries(scanno, :));
|
|
|
|
% Update SLAM with lidar and velocities
|
|
slam = slam.update(scans(scanno,:), velocities);
|
|
|
|
else
|
|
|
|
% Update SLAM with lidar alone
|
|
slam = slam.update(scans(scanno,:));
|
|
end
|
|
|
|
% Get new position
|
|
[x_mm, y_mm, theta_degrees] = slam.getpos();
|
|
|
|
% Get current map
|
|
map = slam.getmap();
|
|
|
|
% Display map
|
|
hold off
|
|
image(map/4) % Keep bytes in [0,64] for colormap
|
|
axis('square')
|
|
colormap('gray')
|
|
hold on
|
|
|
|
% Generate a polyline to represent the robot
|
|
[x_pix, y_pix] = robot_polyline(ROBOT_SIZE_PIXELS);
|
|
|
|
% Rotate the polyline based on the robot's angular rotation
|
|
theta_radians = pi * theta_degrees / 180;
|
|
x_pix_r = x_pix * cos(theta_radians) - y_pix * sin(theta_radians);
|
|
y_pix_r = x_pix * sin(theta_radians) + y_pix * cos(theta_radians);
|
|
|
|
% Add the robot's position as offset to the polyline
|
|
x_pix = x_pix_r + mm2pix(x_mm, MAP_SIZE_METERS, MAP_SIZE_PIXELS);
|
|
y_pix = y_pix_r + mm2pix(y_mm, MAP_SIZE_METERS, MAP_SIZE_PIXELS);
|
|
|
|
|
|
% Add robot image to map
|
|
fill(x_pix, y_pix, 'r')
|
|
drawnow
|
|
|
|
% Delay based on current time in microseconds
|
|
currTime = times(scanno);
|
|
if scanno > 1
|
|
pause((currTime-prevTime)/1e6)
|
|
end
|
|
prevTime = times(scanno);
|
|
|
|
|
|
end
|
|
|
|
% Function to generate a x, y points for a polyline to display the robot
|
|
% Currently we just generate a triangle.
|
|
function [x_pix, y_pix] = robot_polyline(robot_size)
|
|
HEIGHT_RATIO = 1.25;
|
|
x_pix = [-robot_size, robot_size, -robot_size];
|
|
y_pix = [-robot_size/HEIGHT_RATIO, 0 , robot_size/HEIGHT_RATIO];
|
|
|
|
% Function to convert millimeters to pixels -------------------------------
|
|
|
|
function pix = mm2pix(mm, MAP_SIZE_METERS, MAP_SIZE_PIXELS)
|
|
pix = floor(mm / (MAP_SIZE_METERS * 1000. / MAP_SIZE_PIXELS));
|
|
|
|
|
|
% Function to load all from file ------------------------------------------
|
|
% Each line in the file has the format:
|
|
%
|
|
% TIMESTAMP ... Q1 Q1 ... Distances
|
|
% (usec) (mm)
|
|
% 0 ... 2 3 ... 24 ...
|
|
%
|
|
%where Q1, Q2 are odometry values
|
|
|
|
function [times, scans,odometries] = load_data(dataset)
|
|
|
|
data = load([dataset '.dat']);
|
|
|
|
times = data(:,1);
|
|
scans = data(:,25:end-1); % avoid final ' '
|
|
odometries = data(:,[1,3,4]);
|
|
|
|
% Function to build a Laser data structure for the Hokuyo URG04LX used for
|
|
% collecting the logfile data.
|
|
|
|
function laser = MinesLaser()
|
|
|
|
laser.scan_size = 682;
|
|
laser.scan_rate_hz = 10;
|
|
laser.detection_angle_degrees = 240;
|
|
laser.distance_no_detection_mm = 4000;
|
|
laser.detection_margin = 70;
|
|
laser.offset_mm = 145;
|