Add arbitrary number of obstacles

.gitattributes

@@ -0,0 +1,29 @@
+*.fig binary
+*.mat binary
+*.mdl binary
+*.mdlp binary
+*.mexa64 binary
+*.mexw64 binary
+*.mexmaci64 binary
+*.mlapp binary
+*.mldatx binary
+*.mlproj binary
+*.mlx binary
+*.p binary
+*.sfx binary
+*.sldd binary
+*.slreqx binary
+*.slmx binary
+*.sltx binary
+*.slxc binary
+*.slx binary merge=mlAutoMerge
+*.slxp binary
+
+## Other common binary file types
+*.docx binary
+*.exe binary
+*.jpg binary
+*.pdf binary
+*.png binary
+*.xlsx binary
+*.gif binary

.gitignore

@@ -0,0 +1,6 @@
+*.asv
+*.slxc
+slprj
+*.autosave
+## temporal office files
+~$*     

lib/Resources/Utils/PlaygroundCreateSimMap.m

@@ -3,15 +3,15 @@
 % mask and creates an occupancy grid in order to implement sensors
 %  Copyright 2018 The MathWorks, Inc.
 
-arenaDimensions=[Arena.width Arena.length];
+arenaDimensions=[Arena.length Arena.width];
 % Normalize the arena dimensions 
 normDims=arenaDimensions/max(arenaDimensions);
 mapScale=200/max(arenaDimensions);
 mapGridSize=round(normDims*200);
 
 
 % Initialize obstacle map
-locObsMap = ones(mapGridSize(1),mapGridSize(2),'single');
+locObsMap = ones(mapGridSize(2),mapGridSize(1),'single');
 for idx = 1:size(Arena.obj,2)
     % add walls to occupancy map
     locObsMap(1:5,:)=0;
@@ -22,12 +22,13 @@
     % if obstacle is enabled in mask then add to occupancy map
     if strcmp(Arena.obj(idx).status,'on')
 
-
-        obsLoc(1)=((Arena.obj(idx).position(1)+Arena.length/2)*mapScale)-(Arena.obj(idx).width*mapScale/2);
-        obsLoc(2)=mapGridSize(1)-((Arena.obj(idx).position(2)+Arena.width/2)*mapScale)-(Arena.obj(idx).length*mapScale/2);
-        obsLoc(3)= Arena.obj(idx).width*mapScale;
-        obsLoc(4)= Arena.obj(idx).length*mapScale;
-
+        recPose=Arena.obj(idx).recPose;
+
+        % change coordinates frames to image
+        x=  recPose(1);      
+        y= Arena.width -recPose(2)-recPose(4);
+        obsLoc= [x y recPose(3:4)]*mapScale;
+
         % Get obstacle indices
         currObsLoc = ceil(obsLoc);
         colIdx =  currObsLoc(1):(currObsLoc(1)+currObsLoc(3));
@@ -49,10 +50,10 @@
 
 mapForSim.simMap = robotics.BinaryOccupancyGrid(compMap,mapScale);
 % image(mapForSim.obsMap*240)
-% show(mapForSim.simMap)
+ %show(mapForSim.simMap)
 
 % Only for testing
 % assignin('base','occGrid',mapForSim.simMap)
 
 
-end
+end

lib/Resources/Utils/RPInit.m

@@ -37,17 +37,28 @@ function RPInit()
     % Disable link so that the changes can be saved with the model
     set_param(block,'LinkStatus','breakWithoutHierarchy');
 
-    Arena.width=eval(get_param(block,'arenaWidth'));
     Arena.length=eval(get_param(block,'arenaLength'));
+    Arena.width=eval(get_param(block,'arenaWidth'));
+
+    obstacles=eval(get_param(block,'obstacles'));
+
+    create_and_delete_blocks(obstacles,block) % update obstacles 
 
-    for idx=1:10
-        Arena.obj(idx).status=get_param(block,strcat('obs',num2str(idx)));
-        Arena.obj(idx).position=eval(get_param(block,strcat('obsPose',num2str(idx))));
-        Arena.obj(idx).width=eval(get_param(block,strcat('obsWidth',num2str(idx))));
-        Arena.obj(idx).length=eval(get_param(block,strcat('obsLength',num2str(idx))));
+    for idx=1:size(obstacles,1)
+        Arena.obj(idx).recPose=obstacles(idx,:);
+
+        pos=obstacles(idx,[1,2]);
+        length=obstacles(idx,3);
+        width=obstacles(idx,4);
+
+        Arena.obj(idx).status='on';
+        Arena.obj(idx).position=pos + [length,width]/2;
+        Arena.obj(idx).length=length;
+        Arena.obj(idx).width=width;
+
     end
-    mapForSim=PlaygroundCreateSimMap(Arena,envBlk{1});
-    ws=get_param(gcs,'modelworkspace');
+    mapForSim=PlaygroundCreateSimMap(Arena,block);
+    ws=get_param(model,'modelworkspace');
 
     try
         ws.assignin('mapForSim',mapForSim);
@@ -121,4 +132,4 @@ function RPInit()
         error('Please ensure there is only one environment block in your model')
     end
 end
-end
+end

lib/Resources/Utils/create_and_delete_blocks.m

@@ -0,0 +1,61 @@
+function create_and_delete_blocks(obstacles,block)
+% obstacles matrix[nx4] with rectangles with the form [x,y,length,width] 
+Arena.length=eval(get_param(block,'arenaLength'));  
+Arena.width=eval(get_param(block,'arenaWidth'));
+
+% Delete
+  enviroment=[block '/Obstacles'];
+
+  blk_list=find_system(enviroment,'Type','Block');
+
+  ref_obj=[enviroment, '/Obstacle1'];     % reference block to be copied
+  bus_blk=[enviroment '/RP-Bus'];      % bus block where get new blocks get connected
+
+  blk_safe={enviroment,ref_obj,bus_blk};  % list of blocks that are not deleted
+  is_safe=ismember(blk_list,blk_safe);
+  blk_remove=blk_list(~is_safe); 
+
+
+   for idx=2:size(blk_remove,1)+1
+        deleted_obj=[enviroment, '/Obstacle' num2str( idx)];
+        LH_deleted_obj=get_param(deleted_obj,'LineHandles'); % Line handle 
+        delete_line(LH_deleted_obj.LConn);
+        delete_block(deleted_obj);
+
+   end
+
+  % create
+  n=size(obstacles,1);
+
+  PH_bus=get_param(bus_blk,'PortHandles'); %Port Handle bus
+
+  for idx=2:n
+    % positionate the blocks in a  grid path 
+    block_pos=[ 415+100*fix(idx/10), 180+90*mod(idx,10),  480+100*fix(idx/10),  240+90*mod(idx,10)];
+
+
+    new_obj=[enviroment, '/Obstacle' num2str( idx)];
+    add_block(ref_obj,ref_obj,'MakeNameUnique','on', 'Position',block_pos); 
+    PH_new=get_param(new_obj,'PortHandles');
+
+    % connect each new block to the common bus
+    add_line(enviroment,PH_bus.RConn, PH_new.LConn,  'autorouting', 'on');
+
+  end
+
+% set obstacle properties
+  for idx=1:n
+
+    pos=obstacles(idx,[1,2]);
+    length=obstacles(idx,3);
+    width=obstacles(idx,4);
+
+    % reference frame transformation
+    pos=pos + [length-Arena.length,width-Arena.width]/2;
+    obj=[enviroment, '/Obstacle' num2str( idx)];
+    set_param(obj,'Pose',mat2str( pos ))
+    set_param(obj,'length',num2str(length),'width',num2str(width)) 
+  end
+
+end
+

lib/Resources/Utils/setObstaclesUI.m

@@ -1,66 +1,92 @@
 function setObstaclesUI(blk)
-% This function is an interactive obstacle placement tool
-% It is implemented within the obstacle environment mask and provides the
-% user the ability to draw the desired obstacles within the map.
-%  Copyright 2018 The MathWorks, Inc.
-
-numObs=inputdlg('Number of obstacles to place (Max 10): ','Obstacle sekection');
+  % This function is an interactive obstacle placement tool
+  % It is implemented within the obstacle environment mask and provides the
+  % user the ability to draw the desired obstacles within the map.
+  %  Copyright 2018 The MathWorks, Inc.
+
+  numObs=inputdlg('Number of obstacles to place (Max 10): ','Obstacle sekection');
+  % 
+  % for idx=1:10
+  %     set_param(blk,['obs' num2str(idx)],'off')
+  %     set_param([blk '/Obstacles/Obstacle' num2str(idx)],'Commented','on');
+  % end
+
+  if ~isempty(numObs)
+
+      n=eval(numObs{1})       % number of obstacles
+
+      S.f=figure('Name','Set Arena Obstacles','NumberTitle','off');
+      length=eval(get_param(blk,'arenaLength'));
+      width=eval(get_param(blk,'arenaWidth'));
+      arenaDimensions=[length width];
+
+
+
+
+      plot(length/2,width/2,'+')
+      arena=[0 0 length width];    
+      rectangle('Position',arena ,'FaceColor',[0.8 .8 .8 0.1])
+      ax = gca;
+      axis equal 
+      set(ax,'XTick', [0:length],'XTickLabel', [])
+      set(ax,'YTick', [0:width],'YTickLabel', [])
+      set(ax,'xlim',[0 length],'ylim',[0 width]);
+      grid on
+
+      % Walls 
+      xline(0,'k','LineWidth',3)
+      yline(0,'g','LineWidth',3)
+      xline(length,'b','LineWidth',3)
+      yline(width,'r','LineWidth',3)
+
+
+      S.obstacles=[0 0 1 1];  % default obstacle   
+      S.blk=blk;              % add block reference to structure  
+
+
+      for idx= 1:n
+
+          inputRect{n}=imrect;
+          recPose=getPosition(inputRect{n}); 
+          rectangle('Position',recPose,'FaceColor',[1 0.4 0.2])
+          S.obstacles(idx,:)=recPose;
+      end
+      min=0.1;    % minimum block thickenss 
+      is_slim=logical([zeros(n,2),S.obstacles(:,3:4)<min]);
+      S.obstacles(is_slim)=min;
+      S.obstacles=round(S.obstacles,2)
+     %callback function for the pushbutton  
+      S.pb = uicontrol('style','push',...
+                   'units','pix',...
+                   'position',[10 5 180 35],...
+                   'fontsize',12,...
+                   'string','Export obstacle position',...
+                   'callback',{@pb_export,S});
+
+      S.pb2 = uicontrol('style','push',...
+                   'units','pix',...
+                   'position',[300 5 180 35],...
+                   'fontsize',12,...
+                   'string','Done',...
+                   'callback',{@pb_done,S});
+
+  end
+
 
-for idx=1:10
-    set_param(blk,['obs' num2str(idx)],'off')
-    set_param([blk '/Obstacles/Obstacle' num2str(idx)],'Commented','on');
 end
-
 
-if ~isempty(numObs)
+ function pb_export(varargin)
+    [file,path] = uiputfile('maze.csv')  
+    S = varargin{3};  % Get the structure.
+    csvwrite([path file],S.obstacles)
+
+end
 
-    % disable all obstacle sin mask
-
-
-    hg=figure('Name','Set Arena Obstacles','NumberTitle','off');
-    length=eval(get_param(blk,'arenaLength'));
-    width=eval(get_param(blk,'arenaWidth'));
-    arenaDimensions=[width length];
-
-    % Normalize the arena dimensions 
-
-    normDims=arenaDimensions/max(arenaDimensions);
-    mapScale=2048/max(arenaDimensions);
-    mapGridSize=round(normDims*2048);
-    xScale=mapGridSize(2)/length;
-    yScale=mapGridSize(1)/width;
-
-    image=imread('PlaygroundImage.png');
-    scaledImg=imresize(image,mapGridSize);
+ function pb_done(varargin)
+    S = varargin{3};  % Get the structure.
+    blk=S.blk;
 
-    imshow(scaledImg)
-
-    for idx=1:eval(numObs{1})
-        title(['Draw obstacle ' num2str(idx) ' as a rectangle by clicking and dragging on map'])
-        h=imrect;
-        %     wait(h(idx));
-        recPose(:)=getPosition(h);        
-
-        % Fill-in rectangle with obstacle color for user feedback
-        rectangle('Position',recPose(:),'FaceColor',[1 0.4 0.2]);
-
-        % Correct y axis inversion from image axis and scale to world
-        % coordinates
-
-        imgPose(:)=[-mapGridSize(2)/2+recPose(1)+recPose(3)/2 mapGridSize(1)/2-recPose(2)-recPose(4)/2 recPose(3:4)];
+    set_param(blk,'obstacles',mat2str(S.obstacles));
 
-        % shift objects to correct reference frame
-        obsPose(:)=[imgPose(1)/xScale imgPose(2)/yScale imgPose(3)/xScale imgPose(4)/yScale];
-
-        % set parameters on block mask
-        set_param(blk,['obs' num2str(idx)],'on')
-        set_param(blk,['obsPose' num2str(idx)],['[' num2str(obsPose(1:2)) ']']);
-        set_param(blk,['obsWidth' num2str(idx)],num2str(obsPose(3)));
-        set_param(blk,['obsLength' num2str(idx)],num2str(obsPose(4)));
-        set_param([blk '/Obstacles/Obstacle' num2str(idx)],'Commented','off');
-
-    end
-    pause(1)
-    close(hg)
-end
+    closereq(); 
 end