[Robotflow-cvs] RobotFlow/MARIE/src Makefile.am,NONE,1.1 MarieCommandBehavior.cpp,NONE,1.1 MarieCommandCamera.cpp,NONE,1.1 MarieCommandMotor.cpp,NONE,1.1 MarieDataCamera.cpp,NONE,1.1 MarieDataLaser.cpp,NONE,1.1 MarieDataOdometry.cpp,NONE,1.1 MarieDataSonar.cpp,NONE,1.1 MarieLoad.cpp,NONE,1.1 MarieObject.cpp,NONE,1.1 MarieSave.cpp,NONE,1.1

[Dominic L?t. <ma...@us...>]

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

Added Files:
	Makefile.am MarieCommandBehavior.cpp MarieCommandCamera.cpp 
	MarieCommandMotor.cpp MarieDataCamera.cpp MarieDataLaser.cpp 
	MarieDataOdometry.cpp MarieDataSonar.cpp MarieLoad.cpp 
	MarieObject.cpp MarieSave.cpp 
Log Message:
first MARIE blocks merged, fixed configure to support MARIE

--- NEW FILE: MarieCommandCamera.cpp ---
/*
*    MARIE - Mobile and Autonomous Robotics Integration Environment
*    Copyright (C) 2004 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 "MarieCommandCamera.h"
#include <sstream>
#include <string>

#include "MarieXMLDataFactory.h"

namespace marie
{
  DECLARE_NODE(MarieCommandCamera)
  /*Node
   *
   * @name MarieCommandCamera
   * @category MARIE:COMMAND
   * @description Read & Create MarieCommandCamera data object
   *
   * @input_name COMMAND_CAMERA
   * @input_type Input MarieCommandCamera
   * @input_description input ComandCamera, nilObject if not used
   *
   * @input_name BRIGHTNESS
   * @input_type int
   * @input_description input brightness, nilObject if not used
   *
   * @input_name CONTRAST
   * @input_type int
   * @input_description input contrast, nilObject if not used
   * 
   * @input_name PAN
   * @input_type int 
   * @input_description input pan, nilObject if not used
   *
   * @input_name TILT
   * @input_type int
   * @input_description input tilt, nilObject if not used
   *
   * @input_name ZOOM
   * @input_type input zoom, nilObject if not used
   * @input_description 
   * 
   * @output_name COMMAND_CAMERA
   * @output_type MarieCommandCamera
   * @output_description Output CommandCamera (with new pan,tilt,zoom, brightness, contrast)
   *
   * @output_name BRIGHTNESS
   * @output_type int
   * @output_description output brightness
   *
   * @output_name CONTRAST
   * @output_type int 
   * @output_description output contrast
   * 
   * @output_name PAN
   * @output_type int
   * @output_description output pan
   *
   * @output_name TILT
   * @output_type int
   * @output_description output tilt
   *
   * @output_name ZOOM
   * @output_type zoom
   * @output_description output zoom
   * 
  END*/
  
  
  using namespace std;

  DECLARE_TYPE2(CommandCamera::ID,MarieCommandCamera)
  
  MarieCommandCamera::MarieCommandCamera()
    : MarieObject("MarieCommandCamera",ParameterSet()), m_isNode(false)
  {
  
  }
  
  MarieCommandCamera::MarieCommandCamera (const CommandCamera &command)
    : MarieObject("MarieCommandCamera",ParameterSet()), CommandCamera(command),
    m_isNode(false) 
  {
  
  }
  
  MarieCommandCamera::MarieCommandCamera(string nodeName, ParameterSet params)
    : MarieObject(nodeName,params), m_isNode(true) 
  {
    //used as a BufferedNode, create inputs & outputs
                       
     //inputs
     m_commandInID = addInput("COMMAND_CAMERA");            
     m_brightnessInID = addInput("BRIGHTNESS");
     m_contrastInID = addInput("CONTRAST");
     m_panInID = addInput("PAN");
     m_tiltInID = addInput("TILT");
     m_zoomInID = addInput("ZOOM");
     
     //outputs
     m_commandOutID = addOutput("COMMAND_CAMERA");
     m_brightnessOutID = addOutput("BRIGHTNESS");   
     m_contrastOutID = addOutput("CONTRAST");
     m_panOutID = addOutput("PAN");
     m_tiltOutID = addOutput("TILT");
     m_zoomOutID = addOutput("ZOOM");     
  }
  
  void MarieCommandCamera::calculate(int output_id, int count, Buffer &out)
  {
      if (m_isNode)
      {
         MarieCommandCamera *command = NULL;
         int pan, tilt, zoom, contrast, brightness;
         
         //get all inputs       
         ObjectRef commandInValue = getInput(m_commandInID,count);
         ObjectRef brightnessInValue = getInput(m_brightnessInID,count);
         ObjectRef contrastInValue = getInput(m_contrastInID,count);
         ObjectRef panInValue = getInput(m_panInID,count);
         ObjectRef tiltInValue = getInput(m_tiltInID,count);
         ObjectRef zoomInValue = getInput(m_zoomInID,count);
      
         //create command object
         if (!commandInValue->isNil())
         {
            RCPtr<MarieCommandCamera> inputCommand = commandInValue;
            //copy input command
            command = new MarieCommandCamera(*inputCommand);
            command->getPTZ(pan,tilt,zoom);
            brightness = command->getBrightness();
            contrast = command->getContrast();
         }
         else
         {
            command = new MarieCommandCamera();
            pan = tilt = zoom = contrast = brightness = -1;
         }
         
         //get pan
         if (!panInValue->isNil())
         {
            RCPtr<Int> panPtr = panInValue;
            pan = *panPtr;
         }       
         //get tilt
         if (!tiltInValue->isNil())
         {
            RCPtr<Int> tiltPtr = tiltInValue;
            tilt = *tiltPtr;
         }
         //get zoom
         if (!zoomInValue->isNil())
         {
            RCPtr<Int> zoomPtr = zoomInValue;
            zoom = *zoomPtr;
         }
         //get contrast
         if (!contrastInValue->isNil())
         {
            RCPtr<Int> contrastPtr = contrastInValue;
            contrast = *contrastPtr;
         }
         //get brightness
         if (!brightnessInValue->isNil())
         {
            RCPtr<Int> brightnessPtr = brightnessInValue;
            brightness = *brightnessPtr;
         }
         
         //set PTZ
         command->setPTZ(pan,tilt,zoom);
         //set contrast
         command->setContrast(contrast);
         //cet brightness
         command->setBrightness(brightness);
         
         //output values
         (*outputs[m_brightnessOutID].buffer)[count] = ObjectRef(Int::alloc(brightness));
         (*outputs[m_commandOutID].buffer)[count] = ObjectRef(command); 
         (*outputs[m_contrastOutID].buffer)[count] = ObjectRef(Int::alloc(contrast));    
         (*outputs[m_panOutID].buffer)[count] = ObjectRef(Int::alloc(pan)); 
         (*outputs[m_tiltOutID].buffer)[count] = ObjectRef(Int::alloc(tilt)); 
         (*outputs[m_zoomOutID].buffer)[count] = ObjectRef(Int::alloc(zoom)); 
      }
      else
      {
         out[count] = nilObject;
      }    
  }
  
  void MarieCommandCamera::printOn(ostream &out) const 
  {    
     MarieXMLDataFactory factory;
     
     //writing XML data
     string value = factory.toString((CommandCamera&) (*this));
     
     out.write(value.c_str(), value.size());
  
  }
  
  
  void MarieCommandCamera::readFrom(istream &in) 
  {
    throw new GeneralException("readFrom not supported. Use the MarieLoad Node to read data from the stream.",__FILE__,__LINE__);
  }
  
  void MarieCommandCamera::copyDataAbstract(DataAbstract *data)
  {
     
     if (data)
     {
        CommandCamera *command = dynamic_cast<CommandCamera*>(data);
        if (command)
        {
           //((CommandCamera*)this)->operator=(*command);
           this->CommandCamera::operator=(*command);
        }
        else
        {
           throw new GeneralException(string("Unable to cast into CommandCamera Abstract : ") + data->getID(),__FILE__,__LINE__);
        }         
     }
           
  }
}

--- NEW FILE: MarieCommandBehavior.cpp ---
/*
*    MARIE - Mobile and Autonomous Robotics Integration Environment
*    Copyright (C) 2004 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 "MarieCommandBehavior.h"
#include <sstream>
#include <string>

#include "MarieXMLDataFactory.h"
#include "Behavior.h"
#include "Vector.h"

namespace marie
{
  DECLARE_NODE(MarieCommandBehavior)
  /*Node
   *
   * @name MarieCommandBehavior
   * @category MARIE:COMMAND
   * @description Save an object
   *
   * @input_name BEHAVIOR_COMMAND
   * @input_type MarieCommandBehavior
   * @input_description Input BehaviorCommand data object
   *
   * @input_name BEHAVIOR_NAME
   * @input_type String
   * @input_description The behavior name that will be activated/deactivated
   * 
   * @input_name BEHAVIOR_ACTIVATION
   * @input_type Bool
   * @input_description true if the specified behavior is activated
   *
   * @output_name BEHAVIOR_COMMAND
   * @output_description Output BehaviorCommand data object
   * @output_type MarieCommandBahavior
   *
   * @output_name ROBOTFLOW_ACTIVATION 
   * @output_description RobotFlow activation vector
   * @output_type Vector<int>
   * 
  END*/
  
  
  using namespace std;

  DECLARE_TYPE2(CommandBehavior::ID,MarieCommandBehavior)
  
  MarieCommandBehavior::MarieCommandBehavior()
    : MarieObject("MarieCommandBehavior",ParameterSet()), m_isNode(false) 
  {
  
  }
  
  MarieCommandBehavior::MarieCommandBehavior (const CommandBehavior &command)
    : MarieObject("MarieCommandBehavior",ParameterSet()), CommandBehavior(command),
    m_isNode(false)  
  {
  
  }
  
  MarieCommandBehavior::MarieCommandBehavior(string nodeName, ParameterSet params)
    : MarieObject(nodeName,params), m_isNode(true) 
  {
    //used as a BufferedNode, create inputs & outputs
    
     //inputs
     m_commandInID = addInput("BEHAVIOR_COMMAND");
     m_behaviorNameInID = addInput("BEHAVIOR_NAME");
     m_behaviorActivationInID = addInput("BEHAVIOR_ACTIVATION");
     
     //outputs
     m_commandOutID = addOutput("BEHAVIOR_COMMAND");
     m_RobotFlowActivationOutID = addOutput("ROBOTFLOW_ACTIVATION");                                           
  }
  
  void MarieCommandBehavior::calculate(int output_id, int count, Buffer &out)
  {
     if (m_isNode)
     {      
         MarieCommandBehavior *command = NULL;
     
         ObjectRef commandInValue = getInput(m_commandInID,count);
         ObjectRef behaviorNameValue = getInput(m_behaviorNameInID,count);
         ObjectRef behaviorActivationValue = getInput(m_behaviorActivationInID,count);
     
         //create command object
         if (!commandInValue->isNil())
         {
            RCPtr<MarieCommandBehavior> behaviorPtr = getInput(m_commandInID,count);       
            command = new MarieCommandBehavior(*behaviorPtr);
         }
         else
         {
            command = new MarieCommandBehavior();
         }
         
         //look for activation for the desired behavior                     
         if (!behaviorNameValue->isNil() && !behaviorActivationValue->isNil())
         {
            RCPtr<String> behaviorNamePtr = behaviorNameValue;
            RCPtr<Bool> behaviorActivationPtr = behaviorActivationValue;          
            command->setActivation(*behaviorNamePtr,*behaviorActivationPtr);
         }
                                      
         //create behavior activation vector (RobotFlow)
         Vector<int> *vect = Vector<int>::alloc(Behavior::get_behavior_size());
  
         //get known behavior names
         vector<string>& behaviorNames = Behavior::get_behaviors();
        
         //initialize to false (not activated)
         for (int i = 0; i < vect->size(); i++) 
         {
            //TODO check if behavior exists
            (*vect)[i] = (int) command->getActivation(behaviorNames[i]);    
         }
     
         (*outputs[m_commandOutID].buffer)[count] = ObjectRef(command);        
         (*outputs[m_RobotFlowActivationOutID].buffer)[count] = ObjectRef(vect);  
     }
  }
  
  void MarieCommandBehavior::printOn(ostream &out) const 
  {
       
     MarieXMLDataFactory factory;
     
     //writing XML data
     string value = factory.toString((CommandBehavior&) (*this));
     
     out.write(value.c_str(), value.size());
  }
  
  
  void MarieCommandBehavior::readFrom(istream &in) 
  {
    throw new GeneralException("readFrom not supported. Use the MarieLoad Node to read data from the stream.",__FILE__,__LINE__);
  }
  
  void MarieCommandBehavior::copyDataAbstract(DataAbstract *data)
  {
     
     if (data)
     {
        CommandBehavior *command = dynamic_cast<CommandBehavior*>(data);
        if (command)
        {
           //((CommandBehavior*)this)->operator=(*command);
           this->CommandBehavior::operator=(*command);
        }
        else
        {
           throw new GeneralException(string("Unable to cast into CommandBehavior Abstract : ") + data->getID(),__FILE__,__LINE__);
        }      
     }
    
  }
}


--- NEW FILE: MarieDataSonar.cpp ---
/*
*    MARIE - Mobile and Autonomous Robotics Integration Environment
*    Copyright (C) 2004 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 "MarieDataSonar.h"
#include <sstream>
#include <string>
#include "Vector.h"

#include "MarieXMLDataFactory.h"
namespace marie
{
  DECLARE_NODE(MarieDataSonar)
  /*Node
   *
   * @name MarieDataSonar
   * @category MARIE:DATA
   * @description Save an object
   *
   * @input_name DATA_SONAR
   * @input_type MarieDataLaser
   * @input_description MarieDataLaser object
   *
   * @output_name RANGES
   * @output_description  Range contained in the MarieDataSonar object
   * @output_type Vector<int>
   *
  END*/
  
  DECLARE_TYPE2(DataSonar::ID,MarieDataSonar)
  
  using namespace std;

  MarieDataSonar::MarieDataSonar()
    : MarieObject("MarieDataSonar",ParameterSet()), m_isNode(false)
  {
  
  }
  
  MarieDataSonar::MarieDataSonar (const DataSonar &data)
    : MarieObject("MarieDataSonar",ParameterSet()), DataSonar(data),
    m_isNode(false)
  {
  
  }
  
  MarieDataSonar::MarieDataSonar(string nodeName, ParameterSet params)
    : MarieObject(nodeName,params), m_isNode(true)
  {
    //used as a BufferedNode, create inputs & outputs
    m_inputID = addInput("DATA_SONAR");
    m_rangeID = addOutput("RANGES");
  }
  
  void MarieDataSonar::calculate(int output_id, int count, Buffer &out)
  {
     if (m_isNode)
     {
        RCPtr<MarieDataSonar> sonarObject = getInput(m_inputID,count);
     
        unsigned int size = sonarObject->getNbSonar();
        const unsigned int *ranges = sonarObject->getRanges();
        
        //copy range data into a vector
        Vector<int> *vect = Vector<int>::alloc(size);
        
        for (unsigned int i = 0; i < size; i++)
        {
           (*vect)[i] = (int) ranges[i];      
        }
        
        //output vector
        out[count] = ObjectRef(vect);
     }
     else
     {
        out[count] = nilObject;
     }
  }
  
  void MarieDataSonar::printOn(ostream &out) const 
  {
      
     MarieXMLDataFactory factory;
     
     //writing XML data
     string value = factory.toString((DataSonar&) (*this));
     
     out.write(value.c_str(), value.size());
  }
  
  
  void MarieDataSonar::readFrom(istream &in) 
  {
    throw new GeneralException("readFrom not supported. Use the MarieLoad Node to read data from the stream.",__FILE__,__LINE__);
  }
  
  void MarieDataSonar::copyDataAbstract(DataAbstract *data) {
  
     if (data)
     {
        DataSonar *my_data = dynamic_cast<DataSonar*>(data);
        if (my_data)
        {
           //((DataSonar*)this)->operator=(*my_data);
           this->DataSonar::operator=(*my_data);         
        }
        else
        {
           throw new GeneralException(string("Unable to cast into DataSonar Abstract : ") + data->getID(),__FILE__,__LINE__);
        }      
     }
           
  }
}

--- NEW FILE: MarieObject.cpp ---
/*
*    MARIE - Mobile and Autonomous Robotics Integration Environment
*    Copyright (C) 2004 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
*/
//
// C++ Implementation: MarieObject
//
// Description: 
//
//
// Author: Dominic Letourneau <do...@la...>, (C) 2004
//
// Copyright: See COPYING file that comes with this distribution
//
//

namespace marie
{
}

--- NEW FILE: MarieLoad.cpp ---
/*
*    MARIE - Mobile and Autonomous Robotics Integration Environment
*    Copyright (C) 2004 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 "MarieLoad.h"
#include "BufferedNode.h"
#include "Buffer.h"
#include "operators.h"
#include <sstream>
#include "Stream.h"
#include "MarieObject.h"

namespace marie
{ 
  DECLARE_NODE(MarieLoad)
  /*Node
   *
   * @name MarieLoad
   * @category MARIE:IO
   * @description Save an object
   *
   * @input_name STREAM
   * @input_type Stream
   * @input_description The input stream
   *
   * @output_name OUTPUT
   * @output_description Object
   * @output_type any
   *
  END*/


  MarieLoad::MarieLoad(string nodeName, ParameterSet params)
    : BufferedNode(nodeName, params)
  {
    m_outputID = addOutput("OUTPUT");
    m_inputID = addInput("STREAM");
  }
  
  void MarieLoad::calculate(int output_id, int count, Buffer &out)
  {  
  
     try {
     
        //default output to nilObject
        ObjectRef objValue = nilObject;
        
        //get input stream
        RCPtr<IOStream> input_stream = getInput(m_inputID,count);
        
        //instruct marie that we are gonna do a "pull"
        (*input_stream) << "<pull>";
        
        //make sure we are flushing data ?
        //input_stream->flush();
                       
        stringstream my_stream;
        
        while(1)
        {
           char c;                           
           (*input_stream).read(&c,1);
           
           my_stream << c;
           if (my_stream.str().find("</MARIE>") != string::npos) 
           {  
       if (my_stream.str().find("<?xml version=\"1.0\"?><!DOCTYPE MARIE []><MARIE>NULL</MARIE>") != string::npos) {
         cerr<<"MarieLoad : Return empty object"<<endl;
         out[count] = nilObject;
         return;
       }
       else {
              break;
       }
           }  
        }
                                                     
        DataAbstract *data = m_factory.createData(my_stream.str());
                                                  
        if (data) {                
           //create FlowDesigner type object
           objValue = Object::newObject(data->getID());         
                             
           //copy data from newly created object type
           RCPtr<MarieObject> marieObjectPtr = objValue;
           
           //copy internal data         
           marieObjectPtr->copyDataAbstract(data);                  
           
           //delete unused data
           delete data;         
        }
                          
        //output the newly created object
        out[count] = objValue;   
     }
     catch (BaseException *e)
     {
        e->print(cerr);
        delete e;
     }
     catch (...)
     {
        cerr<<"MarieLoad : Unknown exception occured"<<endl;
     }      
  }
}

--- NEW FILE: MarieDataOdometry.cpp ---
/*
*    MARIE - Mobile and Autonomous Robotics Integration Environment
*    Copyright (C) 2004 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 "MarieDataOdometry.h"
#include <sstream>
#include <string>

#include "MarieXMLDataFactory.h"
namespace marie
{
  DECLARE_NODE(MarieDataOdometry)
  /*Node
   *
   * @name MarieDataOdometry
   * @category MARIE:DATA
   * @description Read content of MarieDataOdometry
   *
   * @input_name ODOMETRY_DATA
   * @input_type MarieDataOdometry
   * @input_description The Odometry object
   *
   * @output_name XPOS
   * @output_description X axis position
   * @output_type int
   * 
   * @output_name YPOS
   * @output_description Y axis position
   * @output_type int
   *
   * @output_name ZPOS
   * @output_description Z axis position
   * @output_type int
   *
   * @output_name YAW
   * @output_description YAW angle 
   * @output_type int
   *
   * @output_name PITCH
   * @output_description PITCH angle 
   * @output_type int
   * 
   * @output_name ROLL
   * @output_description ROLL angle
   * @output_type int
   *
   * @output_name LINEAR_SPEED
   * @output_description linear speed (X-Y)
   * @output_type int
   * 
   * @output_name SIDE_SPEED
   * @output_description translational speed (X-Y)
   * @output_type int
   *
   * @output_name ROTATION_SPEED
   * @output_description rotation speed (X-Y)
   * @output_type int
   *
  END*/
  
  
  using namespace std;

  DECLARE_TYPE2(DataOdometry::ID,MarieDataOdometry)
  
  MarieDataOdometry::MarieDataOdometry()
    : MarieObject("MarieDataOdometry",ParameterSet()), m_isNode(false) 
  {
  
  }
  
  MarieDataOdometry::MarieDataOdometry (const DataOdometry &data)
    : MarieObject("MarieDataOdometry",ParameterSet()), DataOdometry(data),
    m_isNode(false) 
  {
  
  }
  
  MarieDataOdometry::MarieDataOdometry(string nodeName, ParameterSet params)
    : MarieObject(nodeName,params), m_isNode(true) 
  {
     //used as a BufferedNode, create inputs & outputs
       
     //input
     m_inputID = addInput("ODOMETRY_DATA");
     
     //outputs
     m_XposID = addOutput("XPOS");
     m_YposID = addOutput("YPOS");
     m_ZposID = addOutput("ZPOS");
     m_yawID = addOutput("YAW");
     m_pitchID = addOutput("PITCH");
     m_rollID = addOutput("ROLL");
     m_linSpeedID = addOutput("LINEAR_SPEED");
     m_sideSpeedID = addOutput("SIDE_SPEED");
     m_rotSpeedID = addOutput("ROTATION_SPEED");   
  }
  
  void MarieDataOdometry::calculate(int output_id, int count, Buffer &out)
  {
     if (m_isNode)
     {
        RCPtr<MarieDataOdometry> odometryObject = getInput(m_inputID,count);
        
        int x,y,z,yaw,pitch,roll, linspeed, sidespeed,rotspeed;
        odometryObject->getPosition(x,y,z);
        odometryObject->getOrientation(yaw,pitch,roll);
        odometryObject->getSpeed(linspeed,sidespeed,rotspeed);
        
        //output data
        (*outputs[m_XposID].buffer)[count] = ObjectRef(Int::alloc(x));
        (*outputs[m_YposID].buffer)[count] = ObjectRef(Int::alloc(y));
        (*outputs[m_ZposID].buffer)[count] = ObjectRef(Int::alloc(z));      
        (*outputs[m_yawID].buffer)[count] = ObjectRef(Int::alloc(yaw));
        (*outputs[m_pitchID].buffer)[count] = ObjectRef(Int::alloc(pitch));
        (*outputs[m_rollID].buffer)[count] = ObjectRef(Int::alloc(roll));      
        (*outputs[m_linSpeedID].buffer)[count] = ObjectRef(Int::alloc(linspeed));
        (*outputs[m_sideSpeedID].buffer)[count] = ObjectRef(Int::alloc(sidespeed));
        (*outputs[m_rotSpeedID].buffer)[count] = ObjectRef(Int::alloc(rotspeed));      
     }
     else
     {
        out[count] = nilObject;
     }
  }
  
  void MarieDataOdometry::printOn(ostream &out) const 
  {
      
     MarieXMLDataFactory factory;
     
     //writing XML data
     string value = factory.toString((DataOdometry&) (*this));
     
     out.write(value.c_str(), value.size());
  
  }
  
  
  void MarieDataOdometry::readFrom(istream &in) 
  {     
    throw new GeneralException("readFrom not supported. Use the MarieLoad Node to read data from the stream.",__FILE__,__LINE__);
  }
  
  void MarieDataOdometry::copyDataAbstract(DataAbstract *data)
  {
   
     if (data)
     {
        DataOdometry *my_data = dynamic_cast<DataOdometry*>(data);
        if (my_data)
        {
           //((DataOdometry*)this)->operator=(*my_data);
           this->DataOdometry::operator=(*my_data);
        }
        else
        {
           throw new GeneralException(string("Unable to cast into DataOdometry Abstract : ") + data->getID(),__FILE__,__LINE__);
        }      
     }
           
  }
}

--- NEW FILE: MarieDataLaser.cpp ---
/*
*    MARIE - Mobile and Autonomous Robotics Integration Environment
*    Copyright (C) 2004 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 "MarieDataLaser.h"
#include <sstream>
#include <string>
#include "MarieXMLDataFactory.h"
#include "Vector.h"

namespace marie
{
  DECLARE_NODE(MarieDataLaser)
  /*Node
   *
   * @name MarieDataLaser
   * @category MARIE:DATA
   * @description Save an object
   *
   * @input_name DATA_LASER
   * @input_type MarieDataLaser
   * @input_description MarieDataLaser object
   *
   * @output_name RANGES
   * @output_description  Range contained in the MarieDataLaser object
   * @output_type Vector<int>
   *
  END*/
  
  DECLARE_TYPE2(DataLaser::ID,MarieDataLaser)

  using namespace std;

  MarieDataLaser::MarieDataLaser()
    : MarieObject("MarieDataLaser",ParameterSet()), m_isNode(false) 
  {
  
  }
  
  MarieDataLaser::MarieDataLaser (const DataLaser &data)
    : MarieObject("MarieDataLaser",ParameterSet()), DataLaser(data),
    m_isNode(false)
  {
  
  }
  
  MarieDataLaser::MarieDataLaser(string nodeName, ParameterSet params)
    : MarieObject(nodeName,params) 
  {
    //used as a BufferedNode, create inputs & outputs
    m_inputID = addInput("DATA_LASER");
    m_outputID = addOutput("RANGES");  
  }
  
  void MarieDataLaser::calculate(int output_id, int count, Buffer &out)
  {
     if (m_isNode) 
     {
        RCPtr<MarieDataLaser> dataLaserObject = getInput(m_inputID,count);
     
        unsigned int size = dataLaserObject->getNbLaser();
        const unsigned int *ranges = dataLaserObject->getRanges();
        
        //copy range data into a vector
        Vector<int> *vect = Vector<int>::alloc(size);
        
        for (unsigned int i = 0; i < size; i++)
        {
           (*vect)[i] = (int) ranges[i];      
        }
        
        //output vector
        out[count] = ObjectRef(vect);
     }
     else 
     {
        out[count] = nilObject;
     }
  }
  
  void MarieDataLaser::printOn(ostream &out) const 
  {
      
     MarieXMLDataFactory factory;
     
     //writing XML data
     string value = factory.toString((DataLaser&) (*this));
     
     out.write(value.c_str(), value.size());
  
  }
  
  
  void MarieDataLaser::readFrom(istream &in) 
  {
    throw new GeneralException("readFrom not supported. Use the MarieLoad Node to read data from the stream.",__FILE__,__LINE__);
  }
  
  void MarieDataLaser::copyDataAbstract(DataAbstract *data)
  {
     
     if (data)
     {
        DataLaser *my_data = dynamic_cast<DataLaser*>(data);
        if (my_data)
        {
           //((DataLaser*)this)->operator=(*my_data);
           this->DataLaser::operator=(*my_data);
        }
        else
        {
           throw new GeneralException(string("Unable to cast into DataLaser Abstract : ") + data->getID(),__FILE__,__LINE__);
        }      
     }
                
  }
}

--- NEW FILE: MarieSave.cpp ---
/*
*    MARIE - Mobile and Autonomous Robotics Integration Environment
*    Copyright (C) 2004 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 "MarieSave.h"

#include "BufferedNode.h"
#include "Buffer.h"
#include "operators.h"
#include <sstream>
#include "MarieXMLDataFactory.h"

namespace marie
{
  DECLARE_NODE(MarieSave)
  /*Node
   *
   * @name MarieSave
   * @category MARIE:IO
   * @description Save an object
   *
   * @input_name INPUT
   * @input_description Object
   * @input_type any
   *
   * @input_name STREAM
   * @input_description Stream to write to
   * @input_type Stream
   *
   * @output_name OUTPUT
   * @output_description Same Object
   * @output_type any
   *
  END*/


  MarieSave::MarieSave(string nodeName, ParameterSet params)
    : BufferedNode(nodeName, params)
  {
    // inputs
    m_inputID = addInput("INPUT");
    m_streamID = addInput("STREAM");
    
    // outputs
    m_outputID = addOutput("OUTPUT");
    
  }
  
  void MarieSave::calculate(int output_id, int count, Buffer &out)
  {
    ObjectRef val = getInput(m_inputID,count);
    RCPtr<OStream> my_stream = getInput(m_streamID,count);
       
    //write data (XML format) to the stream
    MarieXMLDataFactory factory;
    
    DataAbstract *valPtr = dynamic_cast<DataAbstract*>(val.get());
    
    if (valPtr) 
    {    
       string val = factory.toString(*valPtr);
       (*my_stream).write(val.c_str(), val.size());
    }
    else
    {
       throw new GeneralException("Not a valid DataAbstract",__FILE__,__LINE__);   
    }
    
    //output the same object
    out[count] = val;
  }
}

--- NEW FILE: MarieCommandMotor.cpp ---
/*
*    MARIE - Mobile and Autonomous Robotics Integration Environment
*    Copyright (C) 2004 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 "MarieCommandMotor.h"
#include <sstream>
#include <string>

#include "MarieXMLDataFactory.h"

namespace marie
{
  DECLARE_NODE(MarieCommandMotor)
  /*Node
   *
   * @name MarieCommandMotor
   * @category MARIE:COMMAND
   * @description Save an object
   *
   * @input_name COMMAND_MOTOR
   * @input_type MarieCommandMotor
   * @input_description CommandMotor data structure input
   *
   * @input_name VELOCITY
   * @input_type int
   * @input_description input velocity
   *
   * @input_name ROTATION
   * @input_type int
   * @input_description input rotation
   *
   * @output_name COMMAND_MOTOR
   * @output_type MarieCommandMotor
   * @output_description CommandMotor data structure output
   *
   * @output_name VELOCITY
   * @output_type int
   * @output_description output velocity
   *
   * @output_name ROTATION
   * @output_type int
   * @output_description output rotation   
   *
  END*/
  
  
  using namespace std;

  DECLARE_TYPE2(CommandMotor::ID,MarieCommandMotor)
  
  
  MarieCommandMotor::MarieCommandMotor()
    : MarieObject("MarieCommandMotor",ParameterSet()), m_isNode(false) 
  {
  
  }
  
  MarieCommandMotor::MarieCommandMotor (const CommandMotor &command)
    : MarieObject("MarieCommandMotor",ParameterSet()), CommandMotor(command), m_isNode(false) 
  {
  
  }
  
  MarieCommandMotor::MarieCommandMotor(string nodeName, ParameterSet params)
    : MarieObject(nodeName,params), m_isNode(true)
  {
    //used as a BufferedNode, create inputs & outputs
    
    //inputs
    m_commandInID = addInput("COMMAND_MOTOR");
    m_velocityInID = addInput("VELOCITY");
    m_rotationInID = addInput("ROTATION");
    
    //outputs
    m_commandOutID = addOutput("COMMAND_MOTOR");
    m_velocityOutID = addOutput("VELOCITY");
    m_rotationOutID = addOutput("ROTATION");
  }
  
  void MarieCommandMotor::calculate(int output_id, int count, Buffer &out)
  {
     if (m_isNode)
     {
        MarieCommandMotor *command = NULL;
        int velocity, rotation;
     
        //get all inputs
        ObjectRef commandInValue = getInput(m_commandInID,count);
        ObjectRef velocityInValue = getInput(m_velocityInID,count);
        ObjectRef rotationInValue = getInput(m_rotationInID,count);
     
        //initialize new CommandMotor
        if(!commandInValue->isNil())
        {
           RCPtr<MarieCommandMotor> commandPtr = commandInValue;         
           command = new MarieCommandMotor(*commandPtr);
           velocity = command->getVelocity();
           rotation = command->getRotation();         
        }
        else
        {
           command = new MarieCommandMotor();
           velocity = rotation = -1;
        }
        
        //get velocity
        if (!velocityInValue->isNil())
        {
            RCPtr<Int> velocityPtr = velocityInValue;
            velocity = *velocityPtr;  
        }
        
        //get rotation
        if (!rotationInValue->isNil())
        {
           RCPtr<Int> rotationPtr = rotationInValue;
           rotation = *rotationPtr;
        }
        
        //set new rotation & velocity
        command->setVelocity(velocity);
        command->setRotation(rotation);
        
        //output result
        (*outputs[m_commandOutID].buffer)[count] = ObjectRef(command);
        (*outputs[m_velocityOutID].buffer)[count] = ObjectRef(Int::alloc(velocity));
        (*outputs[m_rotationOutID].buffer)[count] = ObjectRef(Int::alloc(rotation));   
     }
     else
     {
        out[count] = nilObject;
     }   
  }
  
  void MarieCommandMotor::printOn(ostream &out) const 
  {
      
     MarieXMLDataFactory factory;
     
     //writing XML data
     string value = factory.toString((CommandMotor&) (*this));
     
     out.write(value.c_str(), value.size());
  }
  
  
  void MarieCommandMotor::readFrom(istream &in) 
  {
    throw new GeneralException("readFrom not supported. Use the MarieLoad Node to read data from the stream.",__FILE__,__LINE__);
  }
  
  void MarieCommandMotor::copyDataAbstract(DataAbstract *data)
  {
     if (data)
     {
        CommandMotor *command = dynamic_cast<CommandMotor*>(data);
        if (command)
        {
           //((CommandMotor*)this)->operator=(*command);
           this->CommandMotor::operator=(*command);
        }
        else
        {
           throw new GeneralException(string("Unable to cast into CommanMotor Abstract : ") + data->getID(),__FILE__,__LINE__);
        }      
     }
  }
}

--- NEW FILE: Makefile.am ---
## Process this file with automake to produce Makefile.in. -*-Makefile-*-
# Disable automatic dependency tracking if using other tools than gcc and gmake
#AUTOMAKE_OPTIONS = no-dependencies

INCLUDES = $(ACE_INCLUDES) $(FLOWDESIGNER_INCLUDE) $(MARIE_INCLUDES) -I../include -I../../Behaviors/include

lib_LTLIBRARIES = libRFMarie.la 

libRFMarie_la_SOURCES = MarieLoad.cpp \
	MarieSave.cpp \
	MarieDataLaser.cpp \
	MarieDataSonar.cpp \
	MarieDataCamera.cpp \
	MarieDataOdometry.cpp \
	MarieCommandBehavior.cpp \
	MarieCommandCamera.cpp \
	MarieCommandMotor.cpp 

install-data-local:
	echo "Installing libRFMarie FlowDesigner toolbox"
	mkdir -p $(OVERFLOW_DATA)/$(PACKAGE)
	(perl $(OVERFLOW_BIN)/info2def.pl $(libRFMarie_la_SOURCES) > $(OVERFLOW_DATA)/$(PACKAGE)/RFMarie.def)
	(cd $(OVERFLOW_DATA)/$(PACKAGE); rm -f RFMarie.tlb; ln -s libRFMarie.so RFMarie.tlb)


libRFMarie_la_LDFLAGS = -release $(LT_RELEASE) $(ACE_LIBS) $(FLOWDESIGNER_LIB) \
	$(XML2_LIB) $(MARIE_LIBS)


--- NEW FILE: MarieDataCamera.cpp ---
/*
*    MARIE - Mobile and Autonomous Robotics Integration Environment
*    Copyright (C) 2004 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 "MarieDataCamera.h"
#include <sstream>
#include <string>

#include "MarieXMLDataFactory.h"

namespace marie
{
  DECLARE_NODE(MarieDataCamera)
  /*Node
   *
   * @name MarieDataCamera
   * @category MARIE:DATA
   * @description Save an object
   *
   * @input_name DATA_CAMERA
   * @input_type MarieDataCamera
   * @input_description MarieDataCamera Object
   *
   * @output_name PAN
   * @output_description Pan of the camera
   * @output_type int
   *
   * @output_name TILT
   * @output_description Tilt of the camera
   * @output_type int 
   *
   * @output_name ZOOM
   * @output_description Zoom of the camera
   * @output_type int
   *
   * @output_name IMAGE_WIDTH
   * @output_description image width from the camera
   * @output_type int
   *
   * @output_name IMAGE_HEIGHT
   * @output_description image height from the camera
   * @output_type int 
   *
   * @output_name IMAGE_FORMAT
   * @output_description format of the image
   * @output_type string
   *
   * @output_name IMAGE
   * @output_description the image data
   * @output_type Image
   *
  END*/
  
  DECLARE_TYPE2(DataCamera::ID,MarieDataCamera)
  
  using namespace std;

  MarieDataCamera::MarieDataCamera()
    : MarieObject("MarieDataCamera",ParameterSet()), m_isNode(false) 
  {
  
  }
  
  MarieDataCamera::MarieDataCamera (const DataCamera &data)
    : MarieObject("MarieDataCamera",ParameterSet()), DataCamera(data),
    m_isNode(false) 
  {
   
  }
  
  MarieDataCamera::MarieDataCamera(string nodeName, ParameterSet params)
    : MarieObject(nodeName,params), m_isNode(true) 
  {
    //used as a BufferedNode, create inputs & outputs
    
    //inputs
    m_inputID = addInput("DATA_CAMERA");
     
    //outputs
    m_panID = addOutput("PAN");
    m_tiltID = addOutput("TILT");
    m_zoomID = addOutput("ZOOM");
    m_imageWidthID = addOutput("IMAGE_WIDTH");
    m_imageHeightID = addOutput("IMAGE_HEIGHT");
    m_imageID = addOutput("IMAGE");
    m_imageFormatID = addOutput("IMAGE_FORMAT");
    
  }
  
  void MarieDataCamera::calculate(int output_id, int count, Buffer &out)
  {
     if (m_isNode)
     {   
        RCPtr<MarieDataCamera> dataCameraObject = getInput(m_inputID,count);
        
        int pan, tilt, zoom;      
        dataCameraObject->getPTZ(pan,tilt,zoom);
         
        unsigned int width,height;
        dataCameraObject->getImageSize(width,height);
                    
        (*outputs[m_panID].buffer)[count] = ObjectRef(Int::alloc(pan));
        (*outputs[m_tiltID].buffer)[count] = ObjectRef(Int::alloc(tilt));
        (*outputs[m_zoomID].buffer)[count] = ObjectRef(Int::alloc(zoom));
        
        (*outputs[m_imageWidthID].buffer)[count] = ObjectRef(Int::alloc(width));
        (*outputs[m_imageHeightID].buffer)[count] = ObjectRef(Int::alloc(height));
              
        (*outputs[m_imageFormatID].buffer)[count] = ObjectRef(new String(dataCameraObject->getImageFormat()));
        
        //TODO output image 
        (*outputs[m_imageID].buffer)[count] = nilObject;
     }
     else
     {
        out[count] = nilObject;
     }
  }
  
  void MarieDataCamera::printOn(ostream &out) const 
  {
      
     MarieXMLDataFactory factory;
     
     //writing XML data
     string value = factory.toString((DataCamera&) (*this));
     
     out.write(value.c_str(), value.size());
  
  }
  
  
  void MarieDataCamera::readFrom(istream &in) 
  {
    throw new GeneralException("readFrom not supported. Use the MarieLoad Node to read data from the stream.",__FILE__,__LINE__);
  }
  
  void MarieDataCamera::copyDataAbstract(DataAbstract *data)
  {
     
     if (data)
     {
        DataCamera *my_data = dynamic_cast<DataCamera*>(data);
        if (my_data)
        {
           //((DataCamera*)this)->operator=(*my_data);
           this->DataCamera::operator=(*my_data);
        }
        else
        {
           throw new GeneralException(string("Unable to cast into DataCamera Abstract : ") + data->getID(),__FILE__,__LINE__);
        }      
     }
     
     
  }
}