added support for multiple LCM input channels (via multicoordinateframe)

git-svn-id: https://svn.csail.mit.edu/locomotion/robotlib/trunk@4811 c9849af7-e679-4ec6-a44e-fc146a885bd3

systems/@DynamicalSystem/runLCM.m

@@ -5,8 +5,6 @@ function runLCM(obj,x0,options)
 %  conditions.
 %
 %  @option tspan a 1x2 vector defining the start and end time of the simulation.  default [0,inf]
-%  @option inchannel
-%  @option outchannel
 
 
 if (nargin<2) x0=[]; end
@@ -21,17 +19,17 @@ function runLCM(obj,x0,options)
 checkDependency('lcm_enabled');
 fin = obj.getInputFrame;
 fout = obj.getOutputFrame;
-if obj.getNumInputs>0
-  typecheck(fin,'LCMSubscriber');  % e.g., an LCMCoordinateFrame
-  if (~isfield(options,'inchannel')) options.inchannel = fin.defaultChannel(); end
-end
-if obj.getNumOutputs>0
-  typecheck(fout,'LCMPublisher');
+if obj.getNumOutputs>0 && typecheck(fout,'LCMPublisher');
   if (~isfield(options,'outchannel')) options.outchannel = fout.defaultChannel(); end
 end
 
-if (obj.getNumInputs>0 && getNumStates(obj)<1) % if there are no state variables, then just trigger on input
+if (obj.getNumInputs>0 && getNumStates(obj)<1 && isa(fin,'LCMSubscriber')) 
+  % if there are no state variables, and the input frame is a simple
+  % (one channel) lcm input, then just trigger on that input
+
+  if (~isfield(options,'inchannel')) options.inchannel = fin.defaultChannel(); end
   fin = subscribe(fin,options.inchannel);
+  b_lcm_output = isa(fout,'LCMPublisher');
 
   global g_scope_enable; g_scope_enable = true;
 
@@ -45,7 +43,7 @@ function runLCM(obj,x0,options)
     else
       last_t=t; tic;
       y = obj.output(t,[],u);
-      if (getNumOutputs(obj)>0)
+      if (getNumOutputs(obj)>0 && b_lcm_output)
         publish(fout,t,y,options.outchannel);
       end
     end
@@ -62,13 +60,11 @@ function runLCM(obj,x0,options)
 
   load_system('drake');
   if getNumInputs(obj)>0
-    assignin('base',[mdl,'_subscriber'],fin);
-    add_block('drake/lcmInput',[mdl,'/lcmInput'],'channel',['''',options.inchannel,''''],'dim',num2str(fin.dim),'lcm_subscriber',[mdl,'_subscriber']);
-    add_line(mdl,'lcmInput/1','system/1');
+    setupLCMInputs(fin,mdl,'system',1);
   end
   % note: if obj has inputs, but no lcminput is specified, then it will just have the default input behavior (e.g. zeros)
 
-  if getNumOutputs(obj)>0
+  if getNumOutputs(obj)>0 && typecheck(fout,'LCMPublisher')
     assignin('base',[mdl,'_publisher'],fout);
     add_block('drake/lcmOutput',[mdl,'/lcmOutput'],'channel',['''',options.outchannel,''''],'dim',num2str(fout.dim),'lcm_publisher',[mdl,'_publisher']);
     add_line(mdl,'system/1','lcmOutput/1');
@@ -94,7 +90,7 @@ function runLCM(obj,x0,options)
       warning('Your model appears to have an initial conditions block in it (e.g., from SimMechanics).  That block will overwrite any initial conditions that you pass in to simulate.');
     end
   end  
-
+    
   sim(mdl,pstruct);
 end
 

systems/frames/CoordinateFrame.m

@@ -325,6 +325,16 @@ function generateLCMType(obj,robot_name,signal_name)
       end
     end
 
+    function setupLCMInputs(obj,mdl,subsys,subsys_portnum)
+      typecheck(mdl,'char');
+      typecheck(subsys,'char');
+      uid = datestr(now,'MMSSFFF');
+      if (nargin<4) subsys_portnum=1; end
+      typecheck(subsys_portnum,'double'); 
+      add_block('simulink3/Sources/In1',[mdl,'/in',uid]);
+      add_line(mdl,['in',uid,'/1'],[subsys,'/',num2str(subsys_portnum)]);
+    end
+
   end
 
   methods (Access=private)

systems/frames/LCMCoordinateFrame.m

@@ -39,6 +39,7 @@
 %      check anything other than the classname here, since I can't return a
 %      different class type from this constructor.
       obj.lcmtype = lcmtype;
+      obj.channel = name;
 
       constructors = lcmtype.getConstructors();
       for i=1:length(constructors)
@@ -104,9 +105,26 @@ function publish(obj,t,x,channel)
       lc.publish(channel,msg);
     end
 
-    function name = defaultChannel(obj)
-      name = obj.name;
+    function setDefaultChannel(obj,channel)
+      typecheck(channel,'char');
+      obj.channel = channel;
     end
+
+    function channel = defaultChannel(obj)
+      channel = obj.channel;
+    end
+
+    function setupLCMInputs(obj,mdl,subsys,subsys_portnum)
+      typecheck(mdl,'char');
+      typecheck(subsys,'char');
+      uid = datestr(now,'MMSSFFF');
+      if (nargin<4) subsys_portnum=1; end
+      typecheck(subsys_portnum,'double'); 
+      assignin('base',[mdl,'_subscriber',uid],obj);
+      add_block('drake/lcmInput',[mdl,'/in',uid],'channel',['''',obj.channel,''''],'dim',num2str(obj.dim),'lcm_subscriber',[mdl,'_subscriber',uid]);
+      add_line(mdl,['in',uid,'/1'],[subsys,'/',num2str(subsys_portnum)]);
+    end
+
   end
 
   properties
@@ -115,6 +133,7 @@ function publish(obj,t,x,channel)
     last_x;
     last_t;
     lcmtype_constructor;
+    channel;
   end
 
 end

systems/frames/MultiCoordinateFrame.m

@@ -131,13 +131,27 @@
       end
     end
 
+    function setupLCMInputs(obj,mdl,subsys,subsys_portnum)
+      typecheck(mdl,'char');
+      typecheck(subsys,'char');
+      uid = datestr(now,'MMSSFFF');
+      if (nargin<4) subsys_portnum=1; end
+      typecheck(subsys_portnum,'double'); 
+      valuecheck(obj.frame_id,sort(obj.frame_id));  % assume that the simple ordering is ok
+      add_block('simulink3/Signals & Systems/Mux',[mdl,'/mux',uid],'Inputs',num2str(length(obj.frame)));
+      for i=1:length(obj.frame)
+        setupLCMInputs(obj.frame{i},mdl,['mux',uid],i);
+      end
+      add_line(mdl,['mux',uid,'/1'],[subsys,'/',num2str(subsys_portnum)]);
+    end
+
   end
 
   methods (Access=protected)
     function [A,fr] = extractFrameGraph(obj)
       [A,fr] = extractFrameGraph@CoordinateFrame(obj);
       for i=1:length(obj.frame)
-        [B,frb] = extractFrameGraph(obj.frame(i));
+        [B,frb] = extractFrameGraph(obj.frame{i});
         A = blkdiag(A,B);
         fr = vertcat(fr,frb);
       end

systems/frames/test/basicMultiFrameTest.m

@@ -3,7 +3,7 @@
 f1 = CoordinateFrame('test1',3,'a');
 f2 = CoordinateFrame('test2',2,'b');
 
-mf = MultiCoordinateFrame([f1,f2]);
+mf = MultiCoordinateFrame({f1,f2});
 
 p = Point(mf,(1:5)');
 valuecheck(p.a1,1);

util/LCMCoordinateFrameWCoder.m

@@ -11,6 +11,7 @@
 
       obj = obj@CoordinateFrame(name,dim,prefix);
       obj.lcmcoder = lcmcoder;
+      obj.channel = name;
 
       msg = obj.lcmcoder.encode(0,zeros(obj.dim,1));
       obj.monitor = drake.util.MessageMonitor(msg,obj.lcmcoder.timestampName());
@@ -46,15 +47,32 @@ function publish(obj,t,x,channel)
       msg = obj.lcmcoder.encode(t,x);
       lc.publish(channel,msg);
     end
-
-    function str = defaultChannel(obj)
-      str = obj.name;
+
+    function setDefaultChannel(obj,channel)
+      typecheck(channel,'char');
+      obj.channel = channel;
     end
-
+
+    function channel = defaultChannel(obj)
+      channel = obj.channel;
+    end
+
+    function setupLCMInputs(obj,mdl,subsys,subsys_portnum)
+      typecheck(mdl,'char');
+      typecheck(subsys,'char');
+      uid = datestr(now,'MMSSFFF');
+      if (nargin<4) subsys_portnum=1; end
+      typecheck(subsys_portnum,'double'); 
+      assignin('base',[mdl,'_subscriber',uid],obj);
+      add_block('drake/lcmInput',[mdl,'/in',uid],'channel',['''',obj.channel,''''],'dim',num2str(obj.dim),'lcm_subscriber',[mdl,'_subscriber',uid]);
+      add_line(mdl,['in',uid,'/1'],[subsys,'/',num2str(subsys_portnum)]);
+    end
+
   end
 
   properties
     lcmcoder;
     monitor;
+    channel;
   end
 end