Controller_impl.h

Go to the documentation of this file.

// -*- mode: c++; indent-tabs-mode: t; tab-width: 4; c-basic-offset: 4; -*-
/*
 * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
 * All rights reserved. This program is made available under the terms of the
 * Eclipse Public License v1.0 which accompanies this distribution, and is
 * available at http://www.eclipse.org/legal/epl-v10.html
 * Contributors:
 * National Institute of Advanced Industrial Science and Technology (AIST)
 * General Robotix Inc.
 */
#ifndef OPENHRP_CONTROLLER_BRIDGE_CONTROLLER_IMPL_H_INCLUDED
#define OPENHRP_CONTROLLER_BRIDGE_CONTROLLER_IMPL_H_INCLUDED
 
#include <string>
#include <map>
#include <rtm/RTC.h>
#include <rtm/RTObject.h>
#include <rtm/CorbaNaming.h>
#include <rtm/idl/RTCStub.h>
 
#include <hrpCorba/Controller.hh>
#include <hrpCorba/ViewSimulator.hh>
#include <hrpCorba/DynamicsSimulator.hh>
 
#include "BridgeConf.h"
 
#include "config.h"
 
using namespace OpenHRP;
 
class BridgeConf;
class VirtualRobotRTC;
 
class Controller_impl
        : virtual public POA_OpenHRP::Controller
{
public:
        Controller_impl(RTC::Manager* rtcManager, BridgeConf* bridgeConf);
        ~Controller_impl();
 
        SensorState& getCurrentSensorState();
        DblSequence* getLinkDataFromSimulator
        (const std::string& linkName, DynamicsSimulator::LinkDataType linkDataType);
        DblSequence* getSensorDataFromSimulator(const std::string& sensorName);
        ImageData* getCameraImageFromSimulator(int cameraId);
        DblSequence& getJointDataSeqRef(DynamicsSimulator::LinkDataType linkDataType);
        void flushJointDataSeqToSimulator(DynamicsSimulator::LinkDataType linkDataType);
    void flushLinkDataToSimulator(const std::string& linkName,
                                        DynamicsSimulator::LinkDataType linkDataType,
                                                                        const DblSequence& linkData);
 
        virtual void setDynamicsSimulator(DynamicsSimulator_ptr dynamicsSimulator);
        virtual void setViewSimulator(ViewSimulator_ptr viewSimulator);
        void setTimeStep(CORBA::Double _timeStep){
        timeStep = _timeStep;
    }
    double getTimeStep(){
        return timeStep;
    }
 
        virtual void start();
        virtual void control();
        virtual void input();
        virtual void output();
        virtual void stop();
        virtual void destroy();
 
    virtual void shutdown();
    virtual omniObjRef* _do_get_interface(){return _this();}
    virtual void setModelName(const char* localModelName){ modelName = localModelName;}
    virtual void initialize();
    double controlTime;
 
private:
        BridgeConf* bridgeConf;
        RTC::Manager* rtcManager;
 
        std::string modelName;
        VirtualRobotRTC* virtualRobotRTC;
 
        typedef std::map<std::string, Port_Service_Var_Type> PortMap;
 
        struct RtcInfo
        {
                RTC::RTObject_var rtcRef;
                PortMap portMap;
                ExtTrigExecutionContextService_Var_Type execContext;
                double timeRate;
                double timeRateCounter;
        };
        typedef boost::shared_ptr<RtcInfo> RtcInfoPtr;
 
        typedef std::map<std::string, RtcInfoPtr> RtcInfoMap;
        RtcInfoMap rtcInfoMap;
    typedef std::vector<RtcInfoPtr> RtcInfoVector;
    RtcInfoVector rtcInfoVector;
 
        RTC::CorbaNaming* naming;
 
        DynamicsSimulator_var dynamicsSimulator;
        ViewSimulator_var viewSimulator;
 
        SensorState_var sensorState;
        bool sensorStateUpdated;
 
        struct JointValueSeqInfo {
                bool flushed;
                DblSequence values;
        };
 
        typedef std::map<DynamicsSimulator::LinkDataType, JointValueSeqInfo> JointValueSeqInfoMap;
        JointValueSeqInfoMap outputJointValueSeqInfos;
 
        CameraSequence_var cameras;
        Camera::CameraParameter_var cparam;
 
        void detectRtcs();
        void makePortMap(RtcInfoPtr& rtcInfo);
    Controller_impl::RtcInfoPtr addRtcVectorWithConnection(RTC::RTObject_var rtcRef);
        void setupRtcConnections();
        int connectPorts(Port_Service_Ptr_Type outPort, Port_Service_Ptr_Type inPort);
 
    void activeComponents();
    void deactiveComponents();
    void disconnectRtcConnections(PortMap& refPortMap);
    double timeStep;
    bool bRestart;
    void restart();
};
 
#endif