IOComponent.hpp

#ifndef ORO_IO_COMPONENT_HPP
#define ORO_IO_COMPONENT_HPP

#include <rtt/dev/AnalogInput.hpp>
#include <rtt/dev/DigitalInput.hpp>
#include <rtt/dev/AnalogOutput.hpp>
#include <rtt/dev/DigitalOutput.hpp>

#include <map>
#include <utility>
#include <vector>
#include <string>
#include <sstream>
#include <boost/tuple/tuple.hpp>
#include <rtt/Logger.hpp>
#include <rtt/TaskContext.hpp>
#include <rtt/DataPort.hpp>
#include <rtt/Property.hpp>
#include <rtt/RTT.hpp>

#include <ocl/OCL.hpp>

namespace OCL
{

    class IOComponent
        : public TaskContext
    {
    public:
        IOComponent(const std::string& name="IOComponent")
            :  TaskContext( name ),
               max_inchannels("MaximumInChannels","The maximum number of virtual analog input channels", 32),
               max_outchannels("MaximumOutChannels","The maximum number of virtual analog output channels", 32),
               inChannelPort( "InputValues", std::vector<double>( max_outchannels.get(), 0.0)  ),
               outChannelPort("OutputValues"),
               workvect(32),
               usingInChannels(0),
               usingOutChannels(0)
        {
            this->configure();

            this->ports()->addPort( &inChannelPort,
                                    "A DataPort containing the values read by this Component from hardware." );
            this->ports()->addPort( &outChannelPort,
                                    "A DataPort containing the values which this Component will write to hardware." );

            this->properties()->addProperty( &max_outchannels );
            this->properties()->addProperty( &max_inchannels );

            this->createAPI();
        }

        virtual bool configureHook()
        {
            if ( max_inchannels.get() > max_outchannels.get() )
                workvect.resize( max_inchannels.get() );
            else
                workvect.resize( max_outchannels.get() );

            inchannels.resize(max_inchannels.get(), 0 );
            outchannels.resize(max_outchannels.get(), 0 );
            chan_meas.resize(max_inchannels.get(), 0.0);
            chan_out.resize(max_outchannels.get(), 0);

            return true;
        }

        virtual void updateHook()
        {
            /*
             * Acces Analog device drivers
             */
            std::for_each( a_in.begin(), a_in.end(), bind( &IOComponent::write_a_in_to_do, this, _1 ) );
            std::for_each( ai_interface.begin(), ai_interface.end(), bind( &IOComponent::read_ai, this, _1 ) );

            if (usingInChannels) {
                for (unsigned int i=0; i < inchannels.size(); ++i)
                    chan_meas[i] = inchannels[i] ? inchannels[i]->value() : 0 ;

                // writeout.
                inChannelPort.Set( chan_meas );
            }

            /*
             * Acces Analog device drivers and Drives
             */
            std::for_each( a_out.begin(), a_out.end(), bind( &IOComponent::write_to_aout, this, _1 ) );
            std::for_each( ao_interface.begin(), ao_interface.end(), bind( &IOComponent::write_ao, this, _1 ) );

            // gather results.
            if ( usingOutChannels ) {
                outChannelPort.Get( chan_out );

                // writeout.
                for (unsigned int i=0; i < outchannels.size(); ++i)
                    if ( outchannels[i] )
                        outchannels[i]->value( chan_out[i] );
            }
        }

        bool addAnalogInInterface( const std::string& Portname, const std::string& devicename)
        {
            Logger::In("addAnalogInInterface");
            AnalogInInterface* input =  AnalogInInterface::nameserver.getObject(devicename);
            if ( !input ) {
                log(Error) << "AnalogIn: "<< devicename <<" not found."<<endlog();
                return false;
            }
            if ( ai_interface.count(Portname) != 0 ){
                log(Error) << "Portname: "<< Portname <<" already in use."<<endlog();
                return false;
            }
            if ( isRunning() ) {
                log(Error) << "Can not add interfaces when running."<<endlog();
                return false;
            }

            ai_interface[Portname] =
                boost::make_tuple(input, new WriteDataPort<std::vector<double> >(Portname, std::vector<double>(input->nbOfChannels())) );

            this->ports()->addPort( boost::get<1>(ai_interface[Portname]), "Analog Input value.");

            this->exportPorts();
            return true;
        }

        bool removeAnalogInInterface(const std::string& Portname)
        {
            Logger::In("removeAnalogInInterface");
            if ( ai_interface.count(Portname) == 0 ){
                log(Error) << "Portname: "<< Portname <<" unknown."<<endlog();
                return false;
            }
            if ( isRunning() ) {
                log(Error) << "Can not remove interfaces when running."<<endlog();
                return false;
            }

            delete boost::get<1>(ai_interface[Portname]);
            this->removeObject( Portname );
            ai_interface.erase(Portname);
            return true;
        }

        bool addAnalogInput( const std::string& Portname, const std::string& devicename, int channel)
        {
            if ( a_in.count(Portname) != 0  || this->isRunning() )
                return false;

            AnalogInInterface* input =  AnalogInInterface::nameserver.getObject(devicename);
            if ( !input ) {
                log(Error) << "AnalogIn: "<< devicename <<" not found."<<endlog();
                return false;
            }

            a_in[Portname] =
                boost::make_tuple( new AnalogInput( input, channel ),
                            new WriteDataPort<unsigned int>(Portname+"_raw", 0),
                            new WriteDataPort<double>(Portname, 0.0) );

            this->ports()->addPort( boost::get<1>(a_in[Portname]), "Analog Input raw value.");
            this->ports()->addPort( boost::get<2>(a_in[Portname]), "Analog Input value.");

            this->exportPorts();
            return true;
        }

        bool removeAnalogInput( const std::string& Portname )
        {
            if ( a_in.count(Portname) != 1 || this->isRunning() )
                return false;

            using namespace boost;
            tuple<AnalogInput*, WriteDataPort<unsigned int>*, WriteDataPort<double>* > res = a_in[Portname];
            this->ports()->removePort( Portname );
            this->ports()->removePort( Portname );
            a_in.erase(Portname);

            delete get<0>(res);
            delete get<1>(res);
            delete get<2>(res);

            this->removeObject(Portname);
            return true;
        }

        bool addInputChannel(int virt_channel, const std::string& devicename, int channel )
        {
            if ( inchannels[virt_channel] != 0 || this->isRunning() )
                return false;

            AnalogInInterface* input =  AnalogInInterface::nameserver.getObject(devicename);
            if ( !input ) {
                log(Error) << "AnalogIn: "<< devicename <<" not found."<<endlog();
                return false;
            }
            ++usingInChannels;

            inchannels[virt_channel] = new AnalogInput( input, channel ) ;

            return true;
        }

        bool removeInputChannel( int virt_channel )
        {
            if ( this->isRunning() )
                return false;

            --usingInChannels;

            inchannels[virt_channel] = 0;

            return true;
        }

        bool addDigitalInput( const std::string& name, const std::string& devicename, int channel, bool invert=false)
        {
            if ( d_in.count(name) != 0 || this->isRunning() )
                return false;

            DigitalInInterface* input =  DigitalInInterface::nameserver.getObject(devicename);
            if ( !input ) {
                log(Error) << "DigitalIn: "<< devicename <<" not found."<<endlog();
                return false;
            }

            d_in[name] = new DigitalInput( input, channel, invert );

            return true;
        }

        bool removeDigitalInput( const std::string& name )
        {
            if ( d_in.count(name) != 1 || this->isRunning() )
                return false;

            DigitalInput* res = d_in[name];

            delete res;

            d_in.erase(name);
            return true;
        }

        bool addAnalogOutInterface( const std::string& Portname, const std::string& devicename)
        {
            Logger::In("addAnalogOutInterface");
            AnalogOutInterface* output =  AnalogOutInterface::nameserver.getObject(devicename);
            if ( !output ) {
                log(Error) << "AnalogOut: "<< devicename <<" not found."<<endlog();
                return false;
            }
            if ( ao_interface.count(Portname) != 0 ){
                log(Error) << "Portname: "<< Portname <<" already in use."<<endlog();
                return false;
            }
            if ( isRunning() ) {
                log(Error) << "Can not add interfaces when running."<<endlog();
                return false;
            }

            ao_interface[Portname] =
                std::make_pair(output, new ReadDataPort<std::vector<double> >(Portname) );

            this->ports()->addPort( ao_interface[Portname].second, "Analog Output value.");

            this->exportPorts();
            return true;
        }

        bool removeAnalogOutInterface(const std::string& Portname)
        {
            Logger::In("removeAnalogOutInterface");
            if ( ao_interface.count(Portname) == 0 ){
                log(Error) << "Portname: "<< Portname <<" unknown."<<endlog();
                return false;
            }
            if ( isRunning() ) {
                log(Error) << "Can not remove interfaces when running."<<endlog();
                return false;
            }

            delete ao_interface[Portname].second;
            this->removeObject( Portname );
            ao_interface.erase(Portname);
            return true;
        }

        bool addAnalogOutput( const std::string& portname, const std::string& devicename, int channel )
        {
            if ( a_out.count(portname) != 0 || this->isRunning() )
                return false;

            AnalogOutInterface* output =  AnalogOutInterface::nameserver.getObject(devicename);
            if ( !output ) {
                log(Error) << "AnalogOut: "<< devicename <<" not found."<<endlog();
                return false;
            }

            a_out[portname] = std::make_pair( new AnalogOutput( output, channel ), new ReadDataPort<double>(portname) );

            this->ports()->addPort( a_out[portname].second, "Analog Output value." );

            this->exportPorts();
            return true;
        }

        bool removeAnalogOutput( const std::string& name )
        {
            if ( a_out.count(name) != 1 || this->isRunning() )
                return false;

            this->ports()->removePort( name );

            delete a_out[name].first;
            delete a_out[name].second;

            a_out.erase(name);
            this->removeObject(name);
            return true;
        }

        bool addOutputChannel( int virt_channel, const std::string& devicename, int channel )
        {
            if ( outchannels[virt_channel] != 0 || this->isRunning() )
                return false;

            AnalogOutInterface* output =  AnalogOutInterface::nameserver.getObject(devicename);
            if ( !output ) {
                log(Error) << "AnalogOut: "<< devicename <<" not found."<<endlog();
                return false;
            }


            outchannels[virt_channel] = new AnalogOutput( output, channel );
            ++usingOutChannels;
            return true;
        }

        bool removeOutputChannel( int virt_channel )
        {
            if ( outchannels[virt_channel] == 0 || this->isRunning() )
                return false;

            delete outchannels[virt_channel];
            --usingOutChannels;
            return true;
        }

        bool addDigitalInInterface( const std::string& name, const std::string& devicename)
        {
            if ( this->isRunning() )
                return false;

            DigitalInInterface* input =  DigitalInInterface::nameserver.getObject(devicename);
            if ( !input ) {
                log(Error) << "DigitalInput: "<< devicename <<" not found."<<endlog();
                return false;
            }

            std::stringstream name_number;
            for(unsigned int i=0; i != input->nbOfInputs(); ++i) {
                name_number << name;
                name_number << i;
                if ( d_in.count(name_number.str()) )
                    delete d_in.find(name_number.str())->second;
                d_in[name_number.str()] = new DigitalInput( input, i );
                name_number.str("");
            }

            return true;
        }

        bool addDigitalOutInterface( const std::string& name, const std::string& devicename)
        {
            if ( this->isRunning() )
                return false;

            DigitalOutInterface* output =  DigitalOutInterface::nameserver.getObject(devicename);
            if ( !output ) {
                log(Error) << "DigitalOut: "<< devicename <<" not found."<<endlog();
                return false;
            }

            std::stringstream name_number;
            for(unsigned int i=0; i != output->nbOfOutputs(); ++i) {
                name_number << name;
                name_number << i;
                if ( d_out.count(name_number.str()) )
                    delete d_out.find(name_number.str())->second;
                d_out[name_number.str()] = new DigitalOutput( output, i );
                name_number.str("");
            }

            return true;
        }

        bool removeDigitalInInterface( const std::string& name)
        {
            if ( this->isRunning() )
                return false;

            std::stringstream name_number;
            for(int i=0; i != 128; ++i) { // FIXME: magic number.
                name_number << name;
                name_number << i;
                if ( d_in.count(name_number.str()) )
                    delete d_in.find(name_number.str())->second;
                d_in.erase(name_number.str());
                name_number.str("");
            }

            return true;
        }

        bool removeDigitalOutInterface( const std::string& name)
        {
            if ( this->isRunning() )
                return false;

            std::stringstream name_number;
            for(int i=0; i != 128; ++i) { // FIXME: magic number.
                name_number << name;
                name_number << i;
                if ( d_out.count(name_number.str()) )
                    delete d_out.find(name_number.str())->second;
                d_out.erase(name_number.str());
                name_number.str("");
            }

            return true;
        }

        bool addDigitalOutput( const std::string& name, const std::string& devicename, int channel, bool invert=false)
        {
            if ( d_out.count(name) != 0 || this->isRunning() )
                return false;

            DigitalOutInterface* output =  DigitalOutInterface::nameserver.getObject(devicename);
            if ( !output ) {
                log(Error) << "DigitalOut: "<< devicename <<" not found."<<endlog();
                return false;
            }

            d_out[name] = new DigitalOutput( output, channel, invert );

            return true;
        }

        bool removeDigitalOutput( const std::string& name )
        {
            if ( d_out.count(name) != 1 || this->isRunning() )
                return false;

            delete d_out[name];

            d_out.erase(name);
            return true;
        }

        void switchOn( const std::string& name )
        {
            DOutMap::iterator it = d_out.find(name);
            if ( it == d_out.end() )
                return;
            it->second->switchOn();
        }

        bool isOn( const std::string& name ) const
        {
            DOutMap::const_iterator it = d_out.find(name);
            if ( it != d_out.end() )
                return it->second->isOn();
            DInMap::const_iterator it2 = d_in.find(name);
            if ( it2 != d_in.end() )
                return it2->second->isOn();
            return false;
        }

        void switchOff( const std::string& name )
        {
            DOutMap::const_iterator it = d_out.find(name);
            if ( it == d_out.end() )
                return;
            it->second->switchOff();
        }

        double value(const std::string& name) const
        {
            AInMap::const_iterator it = a_in.find(name);
            if ( it != a_in.end() )
                return boost::get<0>( it->second )->value();
            AOutMap::const_iterator it2 = a_out.find(name);
            if ( it2 != a_out.end() )
                return it2->second.first->value();
            return 0;
        }

        int rawValue(const std::string& name) const
        {
            AInMap::const_iterator it = a_in.find(name);
            if ( it != a_in.end() )
                return boost::get<0>( it->second )->rawValue();

            AOutMap::const_iterator it2 = a_out.find(name);
            if ( it2 != a_out.end() )
                return it2->second.first->rawValue();
            return 0;
        }

        int getInputChannels() const
        {
            return chan_meas.size();
        }

        int getOutputChannels() const
        {
            return chan_out.size();
        }
    protected:

        void createAPI();

        Property<int> max_inchannels;
        Property<int> max_outchannels;
        std::vector< AnalogInput* > inchannels;
        std::vector< AnalogOutput* > outchannels;

        std::vector<double> chan_meas;
        std::vector<double> chan_out;
        WriteDataPort< std::vector<double> > inChannelPort;
        ReadDataPort< std::vector<double> > outChannelPort;

        typedef std::map<std::string, DigitalInput* > DInMap;
        DInMap d_in;
        typedef std::map<std::string, DigitalOutput* > DOutMap;
        DOutMap d_out;

        typedef
        std::map<std::string,
                 boost::tuple< AnalogInput*,
                        WriteDataPort<unsigned int>*,
                        WriteDataPort<double>* > > AInMap;
        AInMap a_in;
        typedef std::map<std::string, std::pair<AnalogOutput*, ReadDataPort<double>* > > AOutMap;
        AOutMap a_out;

        typedef
        std::map<std::string,
                 boost::tuple< AnalogInInterface*,
                               WriteDataPort<std::vector<double> >* > > AInInterfaceMap;
        AInInterfaceMap ai_interface;
        typedef std::map<std::string, std::pair<AnalogOutInterface*, ReadDataPort<std::vector<double> >* > > AOutInterfaceMap;
        AOutInterfaceMap ao_interface;
        std::vector<double> workvect;

        int usingInChannels;
        int usingOutChannels;

        void write_a_in_to_do( const AInMap::value_type& dd )
        {
            // See boost::tuple for syntax
            boost::get<1>(dd.second)->Set( boost::get<0>(dd.second)->rawValue() );
            boost::get<2>(dd.second)->Set( boost::get<0>(dd.second)->value() );
        }

        void write_to_aout( const AOutMap::value_type& dd )
        {
            dd.second.first->value( dd.second.second->Get() );
        }

        void read_ai( const AInInterfaceMap::value_type& dd )
        {
            // See boost::tuple for syntax
            workvect.resize( boost::get<0>(dd.second)->nbOfChannels() );
            for(unsigned int i=0; i != workvect.size(); ++i) {
                AnalogInput ain( boost::get<0>(dd.second), i);
                workvect[i] = ain.value();
            }
            boost::get<1>(dd.second)->Set( workvect );
        }

        void write_ao( const AOutInterfaceMap::value_type& dd )
        {
            workvect.resize( dd.second.first->nbOfChannels() );
            dd.second.second->Get( workvect );
            for(unsigned int i=0; i != workvect.size(); ++i) {
                AnalogOutput aout( dd.second.first, i);
                aout.value( workvect[i]);
            }
        }

    };

}

#endif

---