Added Java support.

matlab/CoreSLAM.m

@@ -96,15 +96,20 @@ classdef CoreSLAM
             %     Calls the the implementing class's updateMapAndPointcloud()
             %     method with the specified velocities.
             %
-            %     slam = update(slam, scans_mm, velocities)
+            %     slam = update(slam, scans_mm, [velocities])
             %
             %     scan_mm is a list of Lidar scan values, whose count is specified in the scan_size
-            %     velocities is a list of velocities [dxy_mm, dtheta_degrees, dt_seconds] for odometry
-                        
+            %     velocities is an optional list of velocities [dxy_mm, dtheta_degrees, dt_seconds] for odometry
+                                    
             % Build a scan for computing distance to map, and one for updating map
             slam.scan_update(slam.scan_for_mapbuild, scans_mm)            
             slam.scan_update(slam.scan_for_distance, scans_mm)
-                                   
+                                        
+            % Default to zero velocities
+            if nargin < 3
+                velocities = [0, 0, 0];
+            end
+            
             % Update velocities
             velocity_factor = 0;
             if velocities(3) > 0

matlab/Scan.m

@@ -19,7 +19,6 @@ classdef Scan
    properties (Access = {?Map, ?RMHC_SLAM})
        
        c_scan
-       c_laser
    end
    
    methods
@@ -34,14 +33,14 @@ classdef Scan
           %         distance_no_detection_mm 
           %         distance_no_detection_mm
           %         detection_margin
-          % offset_mm = offset_mm
+          %         offset_mm = offset_mm
           %     span (default=1) supports spanning laser scan to cover the space better
 
           if nargin < 2
               span = 1;
           end
-                    
-          [scan.c_scan, scan.c_laser] = mex_breezyslam('Scan_init', laser, span);
+                              
+          scan.c_scan = mex_breezyslam('Scan_init', laser, span);
                               
       end 
       
@@ -59,7 +58,7 @@ classdef Scan
           %    hole_width_mm is the width of holes (obstacles, walls) in millimeters.
           %    velocities is an optional list[dxy_mm, dtheta_degrees]
           %    i.e., robot's (forward, rotational velocity) for improving the quality of the scan.
-          mex_breezyslam('Scan_update', scan.c_scan, scan.c_laser, int32(scans_mm), hole_width_mm, velocities)
+          mex_breezyslam('Scan_update', scan.c_scan, int32(scans_mm), hole_width_mm, velocities)
       end
       
    end

matlab/mex_breezyslam.c

@@ -83,16 +83,6 @@ static position_t _rhs2pos(const mxArray * prhs[], int index)
     return position;
 }
 
-static void _rhs2laser(laser_t * laser, const mxArray * prhs[], int index)
-{
-    laser->scan_size =           (int)_get_field(prhs[index], "scan_size");
-    laser->scan_rate_hz =             _get_field(prhs[index], "scan_rate_hz");
-    laser->detection_angle_degrees =  _get_field(prhs[index], "detection_angle_degrees");
-    laser->distance_no_detection_mm = _get_field(prhs[index], "distance_no_detection_mm");
-    laser->detection_margin =    (int)_get_field(prhs[index], "detection_margin");
-    laser->offset_mm =                _get_field(prhs[index], "offset_mm");
-}
-
 /* Class methods ------------------------------------------------------- */
 
 static void _map_init(mxArray *plhs[], const mxArray * prhs[])
@@ -153,19 +143,29 @@ static void _map_get(mxArray *plhs[], const mxArray * prhs[])
 static void _scan_init(mxArray *plhs[], const mxArray * prhs[])
 {
     
-    laser_t * laser = (laser_t *)mxMalloc(sizeof(laser_t));
-    
     scan_t * scan = (scan_t *)mxMalloc(sizeof(scan_t));
     
     int span = (int)mxGetScalar(prhs[2]);
-    
-    _rhs2laser(laser, prhs, 1);
-    
-    scan_init(scan, laser->scan_size, span);
-            
-    
+
+    const mxArray * laser = prhs[1];
+    int scan_size =                  (int)_get_field(laser, "scan_size");
+    double scan_rate_hz =             _get_field(laser, "scan_rate_hz");
+    double detection_angle_degrees =  _get_field(laser, "detection_angle_degrees");
+    double distance_no_detection_mm = _get_field(laser, "distance_no_detection_mm");
+    int detection_margin =           (int)_get_field(laser, "detection_margin");
+    double offset_mm =                _get_field(laser, "offset_mm");
+ 
+    scan_init(
+            scan, 
+            span,
+            scan_size, 
+            scan_rate_hz,                
+            detection_angle_degrees,     
+            distance_no_detection_mm,    
+            detection_margin,               
+            offset_mm);
+     
     _insert_obj_lhs(plhs, scan, 0);
-    _insert_obj_lhs(plhs, laser, 1);
 }
 
 static void _scan_disp(const mxArray * prhs[])
@@ -183,17 +183,15 @@ static void _scan_update(const mxArray * prhs[])
 {
     scan_t * scan = _rhs2scan(prhs, 1);
     
-    laser_t * laser = (laser_t *)_rhs2ptr(prhs, 2);
+    int scansize = (int)mxGetNumberOfElements(prhs[2]);
     
-    int scansize = (int)mxGetNumberOfElements(prhs[3]);
+    int * lidar_mm = (int *)mxGetPr(prhs[2]);
     
-    int * lidar_mm = (int *)mxGetPr(prhs[3]);
+    double hole_width_mm = mxGetScalar(prhs[3]);
     
-    double hole_width_mm = mxGetScalar(prhs[4]);
+    double * velocities = mxGetPr(prhs[4]);
     
-    double * velocities = mxGetPr(prhs[5]);
-    
-    scan_update(scan, lidar_mm, *laser, hole_width_mm, velocities[0], velocities[1]);
+    scan_update(scan, lidar_mm, hole_width_mm, velocities[0], velocities[1]);
 }
 
 static void _randomizer_init(mxArray *plhs[], const mxArray * prhs[])
@@ -215,7 +213,6 @@ static void _rmhcPositionSearch(mxArray *plhs[], const mxArray * prhs[])
     
     scan_t * scan = _rhs2scan(prhs, 3);
     
-    laser_t laser;
     position_t new_pos;
     
     double sigma_xy_mm = mxGetScalar(prhs[5]);
@@ -226,13 +223,10 @@ static void _rmhcPositionSearch(mxArray *plhs[], const mxArray * prhs[])
     
     void * randomizer = (void *)(long)mxGetScalar(prhs[8]);
     
-    _rhs2laser(&laser, prhs, 4);
-    
     new_pos =  rmhc_position_search(
             start_pos,
             map,
             scan,
-            laser,
             sigma_xy_mm,
             sigma_theta_degrees,
             max_search_iter,