Choreonoid  1.5
VirtualRobotPortHandler.h
Go to the documentation of this file.
1 
6 #ifndef CNOID_OPENRTM_PLUGIN_VIRTUAL_ROBOT_PORT_HANDLER_H
7 #define CNOID_OPENRTM_PLUGIN_VIRTUAL_ROBOT_PORT_HANDLER_H
8 
9 #include <boost/shared_ptr.hpp>
10 #include <cnoid/Body>
11 #include <cnoid/BasicSensors>
12 #include <cnoid/Camera>
13 #include <cnoid/RangeCamera>
14 #include <cnoid/RangeSensor>
15 #include <cnoid/corba/CameraImage.hh>
16 #include <cnoid/corba/PointCloud.hh>
17 #include <rtm/idl/BasicDataType.hh>
18 #include <rtm/idl/ExtendedDataTypes.hh>
19 #include <rtm/idl/InterfaceDataTypes.hh>
20 #include <rtm/RTC.h>
21 #include <rtm/PortBase.h>
22 #include <rtm/OutPort.h>
23 #include <rtm/InPort.h>
24 #include <boost/thread/mutex.hpp>
25 #include <boost/thread/locks.hpp>
26 
27 
28 #include "BridgeConf.h"
29 
30 namespace cnoid {
31 
32 class BodyRTCItem;
33 
35 {
36 public:
38  virtual ~PortHandler();
39  RTC::PortService_var portRef;
40  std::string portName;
41 };
42 
43 typedef boost::shared_ptr<PortHandler> PortHandlerPtr;
44 
45 
47 {
48 public:
49  OutPortHandler(PortInfo& info, bool synchContorller = true)
50  : PortHandler(info), synchController(synchContorller){}
51  virtual void inputDataFromSimulator(BodyRTCItem* bodyRTC) = 0;
52  virtual void writeDataToPort() = 0;
53  template<class T> void setTime(T& value, double _time)
54  {
55  value.tm.sec = (unsigned long)_time;
56  value.tm.nsec = (unsigned long)((_time-value.tm.sec)*1000000000.0 + 0.5);
57  if( value.tm.nsec >= 1000000000 ){
58  value.tm.sec++;
59  value.tm.nsec -= 1000000000;
60  }
61  }
62  double stepTime;
64 };
65 
66 typedef boost::shared_ptr<OutPortHandler> OutPortHandlerPtr;
67 
68 
69 class InPortHandler : public PortHandler
70 {
71 public:
73  virtual void outputDataToSimulator(const BodyPtr& body) = 0;
74  virtual void readDataFromPort() = 0;
75 };
76 
77 typedef boost::shared_ptr<InPortHandler> InPortHandlerPtr;
78 
79 
81 {
82 public:
84  virtual void inputDataFromSimulator(BodyRTCItem* bodyRTC);
85  virtual void writeDataToPort();
86 private:
87  RTC::TimedDoubleSeq values;
88 public:
89  RTC::OutPort<RTC::TimedDoubleSeq> outPort;
90 private:
91  DataTypeId dataTypeId;
92 };
93 
94 
96 {
97 public:
99  virtual void inputDataFromSimulator(BodyRTCItem* bodyRTC);
100  virtual void writeDataToPort();
101 private:
102  RTC::TimedDoubleSeq value;
103 public:
104  RTC::OutPort<RTC::TimedDoubleSeq> outPort;
105 private:
106  std::vector<std::string> linkNames;
107  DataTypeId linkDataType;
108 };
109 
111 {
112 public:
114  virtual void inputDataFromSimulator(BodyRTCItem* bodyRTC);
115  virtual void writeDataToPort();
116 private:
117  RTC::TimedPose3D value;
118 public:
119  RTC::OutPort<RTC::TimedPose3D> outPort;
120 private:
121  std::vector<std::string> linkNames;
122  DataTypeId linkDataType;
123 };
124 
125 
127 {
128 public:
130  virtual void inputDataFromSimulator(BodyRTCItem* bodyRTC);
131  virtual void writeDataToPort();
132 private:
133  RTC::TimedDoubleSeq value;
134 public:
135  RTC::OutPort<RTC::TimedDoubleSeq> outPort;
136 private:
137  std::vector<std::string> sensorNames;
138 };
139 
141 {
142 public:
144  virtual void inputDataFromSimulator(BodyRTCItem* bodyRTC);
145  virtual void writeDataToPort();
146 private:
147  RTC::TimedAngularVelocity3D value;
148 public:
149  RTC::OutPort<RTC::TimedAngularVelocity3D> outPort;
150 private:
151  std::vector<std::string> sensorNames;
152 };
153 
155 {
156 public:
158  virtual void inputDataFromSimulator(BodyRTCItem* bodyRTC);
159  virtual void writeDataToPort();
160 private:
161  RTC::TimedAcceleration3D value;
162 public:
163  RTC::OutPort<RTC::TimedAcceleration3D> outPort;
164 private:
165  std::vector<std::string> sensorNames;
166 };
167 
169 {
170 public:
172  virtual void inputDataFromSimulator(BodyRTCItem* bodyRTC);
173  virtual void writeDataToPort();
174 private:
175  RTC::TimedBooleanSeq value;
176 public:
177  RTC::OutPort<RTC::TimedBooleanSeq> outPort;
178 private:
179  std::vector<std::string> lightNames;
180 };
181 
183 {
184 public:
185  CameraImageOutPortHandler(PortInfo& info, bool synchController);
186  virtual void inputDataFromSimulator(BodyRTCItem* bodyRTC);
187  virtual void writeDataToPort();
188  void onCameraStateChanged();
189  void initialize(Body* simulationBody);
190 private:
191  Img::TimedCameraImage value;
192 public:
193  RTC::OutPort<Img::TimedCameraImage> outPort;
194 private:
195  boost::mutex mtx;
196  Camera* camera;
197  std::string cameraName;
198  boost::shared_ptr<const Image> prevImage;
199  double controlTime;
200 };
201 
203 {
204 public:
205  CameraRangeOutPortHandler(PortInfo& info, bool synchController);
206  virtual void inputDataFromSimulator(BodyRTCItem* bodyRTC);
207  virtual void writeDataToPort();
208  void onCameraStateChanged();
209  void initialize(Body* simulationBody);
210 private:
211  PointCloudTypes::PointCloud value;
212 public:
213  RTC::OutPort<PointCloudTypes::PointCloud> outPort;
214 private:
215  boost::mutex mtx;
216  RangeCamera* rangeCamera;
217  std::string rangeCameraName;
218  boost::shared_ptr<const RangeCamera::PointData> prevPoints;
219  boost::shared_ptr<const Image> image;
220  std::string format;
221  double controlTime;
222 };
223 
225 {
226 public:
227  RangeSensorOutPortHandler(PortInfo& info, bool synchController);
228  virtual void inputDataFromSimulator(BodyRTCItem* bodyRTC);
229  virtual void writeDataToPort();
230  void onRangeSensorStateChanged();
231  void initialize(Body* simulationBody);
232 private:
233  RTC::RangeData value;
234 public:
235  RTC::OutPort<RTC::RangeData> outPort;
236 private:
237  boost::mutex mtx;
238  RangeSensor* rangeSensor;
239  std::string rangeSensorName;
240  boost::shared_ptr<const RangeSensor::RangeData> prevRangeData;
241  double controlTime;
242 };
243 
245 {
246 public:
248  virtual void outputDataToSimulator(const BodyPtr& body);
249  virtual void readDataFromPort();
250 private:
251  RTC::TimedDoubleSeq values;
252 public:
253  RTC::InPort<RTC::TimedDoubleSeq> inPort;
254 private:
255  DataTypeId linkDataType;
256 };
257 
259 {
260 public:
262  virtual void outputDataToSimulator(const BodyPtr& body);
263  virtual void readDataFromPort();
264 private:
265  RTC::TimedDoubleSeq values;
266 public:
267  RTC::InPort<RTC::TimedDoubleSeq> inPort;
268 private:
269  std::vector<std::string> linkNames;
270  DataTypeId linkDataType;
271 };
272 
274 {
275 public:
277  virtual void outputDataToSimulator(const BodyPtr& body);
278  virtual void readDataFromPort();
279 private:
280  RTC::TimedPose3D values;
281 public:
282  RTC::InPort<RTC::TimedPose3D> inPort;
283 private:
284  std::vector<std::string> linkNames;
285  DataTypeId linkDataType;
286 };
287 
289 {
290 public:
292  virtual void outputDataToSimulator(const BodyPtr& body);
293  virtual void readDataFromPort();
294 private:
295  RTC::TimedBooleanSeq values;
296 public:
297  RTC::InPort<RTC::TimedBooleanSeq> inPort;
298 private:
299  std::vector<std::string> lightNames;
300 };
301 }
302 
303 
304 #endif
Definition: VirtualRobotPortHandler.h:126
Definition: Body.h:28
PortHandler(PortInfo &info)
Definition: VirtualRobotPortHandler.h:37
Definition: VirtualRobotPortHandler.h:168
std::string portName
Definition: VirtualRobotPortHandler.h:40
Definition: VirtualRobotPortHandler.h:69
double stepTime
Definition: VirtualRobotPortHandler.h:62
RTC::OutPort< RTC::TimedAngularVelocity3D > outPort
Definition: VirtualRobotPortHandler.h:149
boost::shared_ptr< OutPortHandler > OutPortHandlerPtr
Definition: VirtualRobotPortHandler.h:66
InPortHandler(PortInfo &info)
Definition: VirtualRobotPortHandler.h:72
Definition: VirtualRobotPortHandler.h:80
Definition: VirtualRobotPortHandler.h:224
RTC::OutPort< Img::TimedCameraImage > outPort
Definition: VirtualRobotPortHandler.h:193
Definition: Camera.h:16
Definition: VirtualRobotPortHandler.h:258
RTC::OutPort< RTC::TimedDoubleSeq > outPort
Definition: VirtualRobotPortHandler.h:135
Definition: VirtualRobotPortHandler.h:182
Definition: VirtualRobotPortHandler.h:95
boost::shared_ptr< InPortHandler > InPortHandlerPtr
Definition: VirtualRobotPortHandler.h:77
Definition: VirtualRobotPortHandler.h:46
DataTypeId
Definition: BridgeConf.h:37
Definition: VirtualRobotPortHandler.h:288
Definition: RangeCamera.h:14
Definition: VirtualRobotPortHandler.h:273
RTC::OutPort< PointCloudTypes::PointCloud > outPort
Definition: VirtualRobotPortHandler.h:213
Definition: VirtualRobotPortHandler.h:34
RTC::OutPort< RTC::TimedBooleanSeq > outPort
Definition: VirtualRobotPortHandler.h:177
boost::shared_ptr< PortHandler > PortHandlerPtr
Definition: VirtualRobotPortHandler.h:43
OutPortHandler(PortInfo &info, bool synchContorller=true)
Definition: VirtualRobotPortHandler.h:49
virtual ~PortHandler()
Definition: VirtualRobotPortHandler.cpp:18
Definition: BodyRTCItem.h:24
Definition: VirtualRobotPortHandler.h:202
RTC::InPort< RTC::TimedDoubleSeq > inPort
Definition: VirtualRobotPortHandler.h:253
Defines the minimum processing for performing pasing file for STL.
Definition: AbstractSceneLoader.h:9
Definition: VirtualRobotPortHandler.h:244
RTC::PortService_var portRef
Definition: VirtualRobotPortHandler.h:39
Definition: VirtualRobotPortHandler.h:110
RTC::OutPort< RTC::RangeData > outPort
Definition: VirtualRobotPortHandler.h:235
RTC::OutPort< RTC::TimedDoubleSeq > outPort
Definition: VirtualRobotPortHandler.h:89
RTC::OutPort< RTC::TimedPose3D > outPort
Definition: VirtualRobotPortHandler.h:119
Definition: VirtualRobotPortHandler.h:140
Definition: VirtualRobotPortHandler.h:154
bool synchController
Definition: VirtualRobotPortHandler.h:63
RTC::OutPort< RTC::TimedAcceleration3D > outPort
Definition: VirtualRobotPortHandler.h:163
RTC::InPort< RTC::TimedBooleanSeq > inPort
Definition: VirtualRobotPortHandler.h:297
Definition: RangeSensor.h:16
RTC::OutPort< RTC::TimedDoubleSeq > outPort
Definition: VirtualRobotPortHandler.h:104
Definition: BridgeConf.h:60
RTC::InPort< RTC::TimedDoubleSeq > inPort
Definition: VirtualRobotPortHandler.h:267
RTC::InPort< RTC::TimedPose3D > inPort
Definition: VirtualRobotPortHandler.h:282
void setTime(T &value, double _time)
Definition: VirtualRobotPortHandler.h:53