robotflow-cvs

[Fernyqc <fe...@us...>]

Update of /cvsroot/robotflow/RobotFlow/MARIE/src
In directory sc8-pr-cvs9.sourceforge.net:/tmp/cvs-serv22729

Added Files:
	Data2DPoint.n newMarieData2DPoint.cpp 
	extractMarieData2DPoint.cpp 
Log Message:
Blocs to create and extract Data2DPoint;






--- NEW FILE: Data2DPoint.n ---
#!/usr/bin/env batchflow
<?xml version="1.0"?>
<Document>
  <Network type="subnet" name="MAIN">
    <Node name="node_Constant_5" type="Constant" x="532.000000" y="74.000000">
      <Parameter name="VALUE" type="string" value="failure of RANGES comparison while testing DataIR" description="The value"/>
    </Node>
    <Node name="node_IF_1" type="IF" x="719.000000" y="49.000000">
      <Parameter name="PULL_ANYWAY" type="bool" value="" description="If true, the IF statement pulls also on the branch not taken"/>
    </Node>
    <Node name="node_Throw_1" type="Throw" x="612.000000" y="73.000000"/>
    <Node name="node_NOP_2" type="NOP" x="803.000000" y="48.000000"/>
    <Node name="node_Constant_6" type="Constant" x="-445.000000" y="119.000000">
      <Parameter name="VALUE" type="int" value="2" description="The value"/>
    </Node>
    <Node name="node_newMarieData2DPoint_1" type="newMarieData2DPoint" x="-253.000000" y="96.000000"/>
    <Node name="node_extractMarieData2DPoint_1" type="extractMarieData2DPoint" x="-87.000000" y="96.000000"/>
    <Node name="node_Equal_2" type="Equal" x="185.000000" y="129.000000"/>
    <Node name="node_Constant_1" type="Constant" x="-448.000000" y="33.000000">
      <Parameter name="VALUE" type="float" value="1.0" description="The value"/>
    </Node>
    <Node name="node_Constant_2" type="Constant" x="-448.000000" y="-5.000000">
      <Parameter name="VALUE" type="float" value="2000.0" description="The value"/>
    </Node>
    <Node name="node_Constant_3" type="Constant" x="-448.000000" y="-41.000000">
      <Parameter name="VALUE" type="float" value="1000.0" description="The value"/>
    </Node>
    <Node name="node_Equal_3" type="Equal" x="184.000000" y="59.000000"/>
    <Node name="node_Equal_4" type="Equal" x="185.000000" y="4.000000"/>
    <Node name="node_Equal_5" type="Equal" x="184.000000" y="-45.000000"/>
    <Node name="node_AND_1" type="AND" x="351.000000" y="-21.000000">
      <Parameter name="PULL_ANYWAY" type="bool" value="false" description="Pull on INPUT2 even if INPUT1 is false"/>
    </Node>
    <Node name="node_AND_2" type="AND" x="349.000000" y="97.000000">
      <Parameter name="PULL_ANYWAY" type="bool" value="false" description="Pull on INPUT2 even if INPUT1 is false"/>
    </Node>
    <Node name="node_AND_3" type="AND" x="467.000000" y="33.000000">
      <Parameter name="PULL_ANYWAY" type="bool" value="false" description="Pull on INPUT2 even if INPUT1 is false"/>
    </Node>
    <Link from="node_Throw_1" output="OUTPUT" to="node_IF_1" input="ELSE"/>
    <Link from="node_Constant_5" output="VALUE" to="node_Throw_1" input="INPUT"/>
    <Link from="node_IF_1" output="OUTPUT" to="node_NOP_2" input="INPUT"/>
    <Link from="node_newMarieData2DPoint_1" output="DATA2DPOINT" to="node_extractMarieData2DPoint_1" input="DATA2DPOINT"/>
    <Link from="node_Constant_6" output="VALUE" to="node_newMarieData2DPoint_1" input="ID"/>
    <Link from="node_extractMarieData2DPoint_1" output="ID" to="node_Equal_2" input="INPUT1"/>
    <Link from="node_Constant_1" output="VALUE" to="node_newMarieData2DPoint_1" input="THETA">-420.5 33 -390 103 -351 103.5 </Link>
    <Link from="node_Constant_2" output="VALUE" to="node_newMarieData2DPoint_1" input="Y">-420.5 -5 -380 87 -351 88.5 </Link>
    <Link from="node_Constant_3" output="VALUE" to="node_newMarieData2DPoint_1" input="X">-416 -39 -370 73 -351 73.5 </Link>
    <Link from="node_extractMarieData2DPoint_1" output="THETA" to="node_Equal_3" input="INPUT2">18.5 103.5 84 103 121.5 66.5 </Link>
    <Link from="node_extractMarieData2DPoint_1" output="Y" to="node_Equal_4" input="INPUT2">18.5 88.5 76 89 122.5 11.5 </Link>
    <Link from="node_extractMarieData2DPoint_1" output="X" to="node_Equal_5" input="INPUT2">18.5 73.5 68 75 121.5 -37.5 </Link>
    <Link from="node_Constant_3" output="VALUE" to="node_Equal_5" input="INPUT1"/>
    <Link from="node_Constant_2" output="VALUE" to="node_Equal_4" input="INPUT1"/>
    <Link from="node_Constant_1" output="VALUE" to="node_Equal_3" input="INPUT1"/>
    <Link from="node_Equal_4" output="OUTPUT" to="node_AND_1" input="INPUT2"/>
    <Link from="node_Equal_5" output="OUTPUT" to="node_AND_1" input="INPUT1"/>
    <Link from="node_Equal_2" output="OUTPUT" to="node_AND_2" input="INPUT2"/>
    <Link from="node_Equal_3" output="OUTPUT" to="node_AND_2" input="INPUT1"/>
    <Link from="node_AND_2" output="OUTPUT" to="node_AND_3" input="INPUT2"/>
    <Link from="node_AND_1" output="OUTPUT" to="node_AND_3" input="INPUT1"/>
    <Link from="node_AND_3" output="OUTPUT" to="node_IF_1" input="COND"/>
    <Link from="node_AND_3" output="OUTPUT" to="node_IF_1" input="THEN"/>
    <Link from="node_Constant_6" output="VALUE" to="node_Equal_2" input="INPUT2">-415 123 -363 137 122.5 136.5 </Link>
    <NetOutput name="OUTPUT" node="node_NOP_2" terminal="OUTPUT" object_type="any" description="The output = The input"/>
    <Note x="0" y="0" visible="0" text="Created with FlowDesigner 0.9.0"/>
  </Network>
</Document>

--- NEW FILE: newMarieData2DPoint.cpp ---
/*
*    MARIE - Mobile and Autonomous Robotics Integration Environment
*    Copyright (C) 2004 Dominic Letourneau / Carle Cote
*
*    This library 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 2.1 of the License, or (at your option) any later version.
*
*    This library 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
*    Lesser General Public License for more details.
*
*    You should have received a copy of the GNU Lesser General Public
*    License along with this library; if not, write to the Free Software
*    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*    You can contact MARIE development team at http://marie.sourceforge.net
*/

#include <sstream>
#include <string>
#include "BufferedNode.h"
#include "MarieObject.h"
#include "Data2DPoint.h"

using namespace std;
using namespace FD;

namespace marie
{
   //forward declaration
   class newMarieData2DPoint;
   
   DECLARE_NODE(newMarieData2DPoint)
   /*Node
   *
   * @name newMarieData2DPoint
   * @category RobotFlow:MARIE:DATA
   * @description Create an object of type MarieObject containing a Data2DPoint data
   *
   * @input_name X
   * @input_type float
   * @input_description x coordinate
   *
   * @input_name Y
   * @input_type float
   * @input_description y coordinate
   *
   * @input_name THETA
   * @input_type float
   * @input_description angle value 
   *
   * @input_name ID
   * @input_type int
   * @input_description id value
   *
   * @output_name DATA2DPOINT
   * @output_description Output MarieObject containing a Data2Dpoint data
   * @output_type MarieObject
   *
   END*/
   
   using namespace std;
   
   class newMarieData2DPoint : public BufferedNode
   {
      
   private:
      //intputs
      int m_nXid,m_nYid,m_nTid,m_nIDid;
            
      //outputs
      int m_OutID;
      
   public:
      
      newMarieData2DPoint(string nodeName, ParameterSet params)
      : BufferedNode(nodeName,params)
      {
         //intputs
         m_nXid = addInput("X");
	 m_nYid = addInput("Y");
	 m_nTid = addInput("THETA");
	 m_nIDid = addInput("ID");
         
         
         //outputs
         m_OutID = addOutput("DATA2DPOINT");
      }
      
      void calculate(int output_id, int count, Buffer &out)
      {
         try
         {
            RCPtr<Float> X = getInput(m_nXid,count);
	    RCPtr<Float> Y = getInput(m_nYid,count);
	    RCPtr<Float> T = getInput(m_nTid,count);
	    RCPtr<Int> ID = getInput(m_nIDid,count);
            
	    Data2DPoint * point = new Data2DPoint();
                        
            point->setX(*X);
	    point->setY(*Y);
	    point->setTheta(*T);
	    point->setId(*ID);
            //output result
            out[count] = ObjectRef(new MarieObject(point));
         }
         catch (BaseException *e)
         {
            e->print(cerr);
            e->add(new GeneralException("Unable to process",__FILE__,__LINE__));
            throw e;
         }
      }
   };//class newMarieData2Dpoint
}//namespace marie

--- NEW FILE: extractMarieData2DPoint.cpp ---
/*
*    MARIE - Mobile and Autonomous Robotics Integration Environment
*    Copyright (C) 2004 Dominic Letourneau / Carle Cote
*
*    This library 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 2.1 of the License, or (at your option) any later version.
*
*    This library 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
*    Lesser General Public License for more details.
*
*    You should have received a copy of the GNU Lesser General Public
*    License along with this library; if not, write to the Free Software
*    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*    You can contact MARIE development team at http://marie.sourceforge.net
*/

#include <sstream>
#include <string>
#include "BufferedNode.h"
#include "MarieObject.h"
#include "Data2DPoint.h"

using namespace std;
using namespace FD;

namespace marie
{
   //forward declaration
   class extractMarieData2DPoint;
   
   DECLARE_NODE(extractMarieData2DPoint)
   /*Node
   *
   * @name extractMarieData2DPoint
   * @category RobotFlow:MARIE:DATA
   * @description Extract data in the FlowDesigner format from MarieObject
   *
   * @input_name DATA2DPOINT
   * @input_type MarieObject
   * @input_description The MarieObject object containing a Data2DPoint
   *
   * @output_name X
   * @output_type float
   * @output_description x value
   *
   * @output_name Y
   * @output_type float
   * @output_description y value
   *
   * @output_name THETA
   * @output_type float
   * @output_description theta value
   *
   * @output_name ID
   * @output_type int
   * @output_description ID value
   *
   END*/
   
   using namespace std;
   
   class extractMarieData2DPoint : public BufferedNode 
   {
      
   private:
      //intputs
      int m_Data2DPointID;
      
      //outputs       
      int m_XOutID;
      int m_YOutID;
      int m_TOutID;
      int m_IDOutID;
      
   public:
      
      extractMarieData2DPoint(string nodeName, ParameterSet params)
      : BufferedNode(nodeName,params) 
      {
         //inputs
         m_Data2DPointID = addInput("DATA2DPOINT");
         
         //outputs
         m_XOutID = addOutput("X");
	 m_YOutID = addOutput("Y");
	 m_TOutID = addOutput("THETA");
         m_IDOutID = addOutput("ID");      
      }
      
      void calculate(int output_id, int count, Buffer &out)
      {
         try
         {
            //get input
            RCPtr<MarieObject> dataPtr = getInput(m_Data2DPointID,count);
            Data2DPoint *point = dynamic_cast<Data2DPoint*>(dataPtr->getData());
            if (point != NULL)
            {
               //output data
               (*outputs[m_XOutID].buffer)[count] = ObjectRef(Float::alloc(point->getX()));
               (*outputs[m_YOutID].buffer)[count] = ObjectRef(Float::alloc(point->getY()));
	       (*outputs[m_TOutID].buffer)[count] = ObjectRef(Float::alloc(point->getTheta()));
	       (*outputs[m_IDOutID].buffer)[count] = ObjectRef(Int::alloc(point->getId()));
            }
            else // commandMotor == NULL
            {
               std::cerr << "FD::extractMarieCommandMotor -> MarieObject received data not of Data2DPoint type" << std::endl;
               out[count] = nilObject;
            }
         }
         catch (BaseException *e)
         {
            e->print(cerr);
            e->add(new GeneralException("Unable to process",__FILE__,__LINE__));
            throw e;
         }
      }  
   };//class extractMarieCommandMotor
}//namespace marie