Controller_impl.h
Go to the documentation of this file.
00001 // -*- mode: c++; indent-tabs-mode: t; tab-width: 4; c-basic-offset: 4; -*-
00002 /*
00003  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
00004  * All rights reserved. This program is made available under the terms of the
00005  * Eclipse Public License v1.0 which accompanies this distribution, and is
00006  * available at http://www.eclipse.org/legal/epl-v10.html
00007  * Contributors:
00008  * National Institute of Advanced Industrial Science and Technology (AIST)
00009  * General Robotix Inc.
00010  */
00016 #ifndef OPENHRP_CONTROLLER_BRIDGE_CONTROLLER_IMPL_H_INCLUDED
00017 #define OPENHRP_CONTROLLER_BRIDGE_CONTROLLER_IMPL_H_INCLUDED
00018 
00019 #include <string>
00020 #include <map>
00021 #include <rtm/RTC.h>
00022 #include <rtm/RTObject.h>
00023 #include <rtm/CorbaNaming.h>
00024 #include <rtm/idl/RTCStub.h>
00025 
00026 #include <hrpCorba/Controller.hh>
00027 #include <hrpCorba/ViewSimulator.hh>
00028 #include <hrpCorba/DynamicsSimulator.hh>
00029 
00030 #include "BridgeConf.h"
00031 
00032 #include "config.h"
00033 
00034 using namespace OpenHRP;
00035 
00036 class BridgeConf;
00037 class VirtualRobotRTC;
00038 
00039 class Controller_impl
00040         : virtual public POA_OpenHRP::Controller
00041 {
00042 public:
00043         Controller_impl(RTC::Manager* rtcManager, BridgeConf* bridgeConf);
00044         ~Controller_impl();
00045 
00046         SensorState& getCurrentSensorState();
00047         DblSequence* getLinkDataFromSimulator
00048         (const std::string& linkName, DynamicsSimulator::LinkDataType linkDataType);
00049         DblSequence* getSensorDataFromSimulator(const std::string& sensorName);
00050         ImageData* getCameraImageFromSimulator(int cameraId);
00051         DblSequence& getJointDataSeqRef(DynamicsSimulator::LinkDataType linkDataType);
00052         void flushJointDataSeqToSimulator(DynamicsSimulator::LinkDataType linkDataType);
00053     void flushLinkDataToSimulator(const std::string& linkName,
00054                                         DynamicsSimulator::LinkDataType linkDataType,
00055                                                                         const DblSequence& linkData);
00056 
00057         virtual void setDynamicsSimulator(DynamicsSimulator_ptr dynamicsSimulator);
00058         virtual void setViewSimulator(ViewSimulator_ptr viewSimulator);
00059         void setTimeStep(CORBA::Double _timeStep){
00060         timeStep = _timeStep;
00061     }
00062     double getTimeStep(){
00063         return timeStep;
00064     }
00065 
00066         virtual void start();
00067         virtual void control();
00068         virtual void input();
00069         virtual void output();
00070         virtual void stop();
00071         virtual void destroy();
00072 
00073     virtual void shutdown();
00074     virtual omniObjRef* _do_get_interface(){return _this();}
00075     virtual void setModelName(const char* localModelName){ modelName = localModelName;}
00076     virtual void initialize();
00077     double controlTime;
00078 
00079 private:
00080         BridgeConf* bridgeConf;
00081         RTC::Manager* rtcManager;
00082 
00083         std::string modelName;
00084         VirtualRobotRTC* virtualRobotRTC;
00085 
00086         typedef std::map<std::string, Port_Service_Var_Type> PortMap;
00087 
00088         struct RtcInfo
00089         {
00090                 RTC::RTObject_var rtcRef;
00091                 PortMap portMap;
00092                 ExtTrigExecutionContextService_Var_Type execContext;
00093                 double timeRate;
00094                 double timeRateCounter;
00095         };
00096         typedef boost::shared_ptr<RtcInfo> RtcInfoPtr;
00097 
00098         typedef std::map<std::string, RtcInfoPtr> RtcInfoMap;
00099         RtcInfoMap rtcInfoMap;
00100     typedef std::vector<RtcInfoPtr> RtcInfoVector;
00101     RtcInfoVector rtcInfoVector;
00102 
00103         RTC::CorbaNaming* naming;
00104 
00105         DynamicsSimulator_var dynamicsSimulator;
00106         ViewSimulator_var viewSimulator;
00107 
00108         SensorState_var sensorState;
00109         bool sensorStateUpdated;
00110 
00111         struct JointValueSeqInfo {
00112                 bool flushed;
00113                 DblSequence values;
00114         };
00115 
00116         typedef std::map<DynamicsSimulator::LinkDataType, JointValueSeqInfo> JointValueSeqInfoMap;
00117         JointValueSeqInfoMap outputJointValueSeqInfos;
00118 
00119         CameraSequence_var cameras;
00120         Camera::CameraParameter_var cparam;
00121 
00122         void detectRtcs();
00123         void makePortMap(RtcInfoPtr& rtcInfo);
00124     Controller_impl::RtcInfoPtr addRtcVectorWithConnection(RTC::RTObject_var rtcRef);
00125         void setupRtcConnections();
00126         int connectPorts(Port_Service_Ptr_Type outPort, Port_Service_Ptr_Type inPort);
00127 
00128     void activeComponents();
00129     void deactiveComponents();
00130     void disconnectRtcConnections(PortMap& refPortMap);
00131     double timeStep;
00132     bool bRestart;
00133     void restart();
00134 };
00135 
00136 #endif


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Apr 11 2019 03:30:15