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