00001 /* 00002 * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc. 00003 * All rights reserved. This program is made available under the terms of the 00004 * Eclipse Public License v1.0 which accompanies this distribution, and is 00005 * available at http://www.eclipse.org/legal/epl-v10.html 00006 * Contributors: 00007 * National Institute of Advanced Industrial Science and Technology (AIST) 00008 * General Robotix Inc. 00009 */ 00015 #ifndef OPENHRP_CONTROLLER_BRIDGE_VIRTUAL_ROBOT_PORT_HANDLER_H_INCLUDED 00016 #define OPENHRP_CONTROLLER_BRIDGE_VIRTUAL_ROBOT_PORT_HANDLER_H_INCLUDED 00017 00018 #include <boost/shared_ptr.hpp> 00019 00020 #include <rtm/idl/BasicDataType.hh> 00021 #include <rtm/idl/ExtendedDataTypes.hh> 00022 #include <rtm/RTC.h> 00023 #include <rtm/PortBase.h> 00024 #include <rtm/OutPort.h> 00025 #include <rtm/InPort.h> 00026 #include <rtm/idl/BasicDataTypeStub.h> 00027 00028 #include <hrpCorba/DynamicsSimulator.hh> 00029 00030 #include "BridgeConf.h" 00031 00032 #include "config.h" 00033 00034 using namespace OpenHRP; 00035 00036 00037 class Controller_impl; 00038 00039 class PortHandler 00040 { 00041 public: 00042 PortHandler(PortInfo& info) : portName(info.portName){} 00043 virtual ~PortHandler(); 00044 Port_Service_Var_Type portRef; 00045 std::string portName; 00046 }; 00047 00048 typedef boost::shared_ptr<PortHandler> PortHandlerPtr; 00049 00050 00051 class OutPortHandler : public PortHandler 00052 { 00053 public: 00054 OutPortHandler(PortInfo& info) : PortHandler(info){} 00055 virtual void inputDataFromSimulator(Controller_impl* controller) = 0; 00056 virtual void writeDataToPort() = 0; 00057 template<class T> void setTime(T& value, double _time) 00058 { 00059 value.tm.sec = (unsigned long)_time; 00060 value.tm.nsec = (unsigned long)((_time-value.tm.sec)*1000000000.0 + 0.5); 00061 if( value.tm.nsec >= 1000000000 ){ 00062 value.tm.sec++; 00063 value.tm.nsec -= 1000000000; 00064 } 00065 } 00066 double stepTime; 00067 }; 00068 00069 typedef boost::shared_ptr<OutPortHandler> OutPortHandlerPtr; 00070 00071 00072 class InPortHandler : public PortHandler 00073 { 00074 public: 00075 InPortHandler(PortInfo& info) : PortHandler(info){} 00076 virtual void outputDataToSimulator(Controller_impl* controller) = 0; 00077 virtual void readDataFromPort(Controller_impl* controller) = 0; 00078 }; 00079 00080 typedef boost::shared_ptr<InPortHandler> InPortHandlerPtr; 00081 00082 00083 class SensorStateOutPortHandler : public OutPortHandler 00084 { 00085 public: 00086 SensorStateOutPortHandler(PortInfo& info); 00087 virtual void inputDataFromSimulator(Controller_impl* controller); 00088 virtual void writeDataToPort(); 00089 RTC::OutPort<RTC::TimedDoubleSeq> outPort; 00090 private: 00091 RTC::TimedDoubleSeq values; 00092 DataTypeId dataTypeId; 00093 }; 00094 00095 00096 class LinkDataOutPortHandler : public OutPortHandler 00097 { 00098 public: 00099 LinkDataOutPortHandler(PortInfo& info); 00100 virtual void inputDataFromSimulator(Controller_impl* controller); 00101 virtual void writeDataToPort(); 00102 RTC::OutPort<RTC::TimedDoubleSeq> outPort; 00103 private: 00104 std::vector<std::string> linkName; 00105 DynamicsSimulator::LinkDataType linkDataType; 00106 RTC::TimedDoubleSeq value; 00107 }; 00108 00109 class AbsTransformOutPortHandler : public OutPortHandler 00110 { 00111 public: 00112 AbsTransformOutPortHandler(PortInfo& info); 00113 virtual void inputDataFromSimulator(Controller_impl* controller); 00114 virtual void writeDataToPort(); 00115 RTC::OutPort<RTC::TimedPose3D> outPort; 00116 private: 00117 std::vector<std::string> linkName; 00118 DynamicsSimulator::LinkDataType linkDataType; 00119 RTC::TimedPose3D value; 00120 }; 00121 00122 00123 class SensorDataOutPortHandler : public OutPortHandler 00124 { 00125 public: 00126 SensorDataOutPortHandler(PortInfo& info); 00127 virtual void inputDataFromSimulator(Controller_impl* controller); 00128 virtual void writeDataToPort(); 00129 RTC::OutPort<RTC::TimedDoubleSeq> outPort; 00130 private: 00131 RTC::TimedDoubleSeq value; 00132 std::vector<std::string> sensorName; 00133 }; 00134 00135 class GyroSensorOutPortHandler : public OutPortHandler 00136 { 00137 public: 00138 GyroSensorOutPortHandler(PortInfo& info); 00139 virtual void inputDataFromSimulator(Controller_impl* controller); 00140 virtual void writeDataToPort(); 00141 RTC::OutPort<RTC::TimedAngularVelocity3D> outPort; 00142 private: 00143 RTC::TimedAngularVelocity3D value; 00144 std::vector<std::string> sensorName; 00145 }; 00146 00147 class AccelerationSensorOutPortHandler : public OutPortHandler 00148 { 00149 public: 00150 AccelerationSensorOutPortHandler(PortInfo& info); 00151 virtual void inputDataFromSimulator(Controller_impl* controller); 00152 virtual void writeDataToPort(); 00153 RTC::OutPort<RTC::TimedAcceleration3D> outPort; 00154 private: 00155 RTC::TimedAcceleration3D value; 00156 std::vector<std::string> sensorName; 00157 }; 00158 00159 00160 class ColorImageOutPortHandler : public OutPortHandler 00161 { 00162 public: 00163 ColorImageOutPortHandler(PortInfo& info); 00164 virtual void inputDataFromSimulator(Controller_impl* controller); 00165 virtual void writeDataToPort(); 00166 RTC::OutPort<RTC::TimedLongSeq> outPort; 00167 private: 00168 RTC::TimedLongSeq image; 00169 int cameraId; 00170 }; 00171 00172 00173 class GrayScaleImageOutPortHandler : public OutPortHandler 00174 { 00175 public: 00176 GrayScaleImageOutPortHandler(PortInfo& info); 00177 virtual void inputDataFromSimulator(Controller_impl* controller); 00178 virtual void writeDataToPort(); 00179 RTC::OutPort<RTC::TimedOctetSeq> outPort; 00180 private: 00181 RTC::TimedOctetSeq image; 00182 int cameraId; 00183 }; 00184 00185 00186 class DepthImageOutPortHandler : public OutPortHandler 00187 { 00188 public: 00189 DepthImageOutPortHandler(PortInfo& info); 00190 virtual void inputDataFromSimulator(Controller_impl* controller); 00191 virtual void writeDataToPort(); 00192 RTC::OutPort<RTC::TimedFloatSeq> outPort; 00193 private: 00194 RTC::TimedFloatSeq image; 00195 int cameraId; 00196 }; 00197 00198 00199 class JointDataSeqInPortHandler : public InPortHandler 00200 { 00201 public: 00202 JointDataSeqInPortHandler(PortInfo& info); 00203 virtual void outputDataToSimulator(Controller_impl* controller); 00204 virtual void readDataFromPort(Controller_impl* controller); 00205 RTC::InPort<RTC::TimedDoubleSeq> inPort; 00206 private: 00207 RTC::TimedDoubleSeq values; 00208 DynamicsSimulator::LinkDataType linkDataType; 00209 }; 00210 00211 class LinkDataInPortHandler : public InPortHandler 00212 { 00213 public: 00214 LinkDataInPortHandler(PortInfo& info); 00215 virtual void outputDataToSimulator(Controller_impl* controller); 00216 virtual void readDataFromPort(Controller_impl* controller); 00217 RTC::InPort<RTC::TimedDoubleSeq> inPort; 00218 private: 00219 RTC::TimedDoubleSeq values; 00220 std::vector<std::string> linkName; 00221 DynamicsSimulator::LinkDataType linkDataType; 00222 DblSequence data; 00223 }; 00224 00225 class AbsTransformInPortHandler : public InPortHandler 00226 { 00227 public: 00228 AbsTransformInPortHandler(PortInfo& info); 00229 virtual void outputDataToSimulator(Controller_impl* controller); 00230 virtual void readDataFromPort(Controller_impl* controller); 00231 RTC::InPort<RTC::TimedPose3D> inPort; 00232 private: 00233 RTC::TimedPose3D values; 00234 std::vector<std::string> linkName; 00235 DynamicsSimulator::LinkDataType linkDataType; 00236 DblSequence data; 00237 }; 00238 00239 00240 #endif