← Back to branch summary

~hrg/hrg-packaging/openhrp3

« back to all changes in this revision

Viewing changes to server/ControllerBridge.old/Controller_impl.cpp

  • Committer: nakaoka
  • Date: 2008-07-06 15:26:51 UTC
  • Revision ID: git-v1:fbb3ce756bbcedb021fed5d474b58ef092ea8b42
コンパイルに cmake を使用するようにし、MakefileをCMakeLists.txt で置き換えた。

Show diffs side-by-side

added added

removed removed

Lines of Context:
server/ControllerBridge.old/Controller_impl.cpp
1
 
/**
2
 
   \file
3
 
   \author Shin'ichiro Nakaoka
4
 
*/
5
 
 
6
 
#include "Controller_impl.h"
7
 
 
8
 
#include <string>
9
 
#include <iostream>
10
 
#include <rtm/Manager.h>
11
 
#include <rtm/RTObject.h>
12
 
#include <rtm/NVUtil.h>
13
 
 
14
 
#include <ORBwrap.h>
15
 
 
16
 
#include "VirtualRobotRTC.h"
17
 
 
18
 
using namespace std;
19
 
using namespace boost;
20
 
using namespace OpenHRP;
21
 
using namespace OpenHRP::ControllerBridge;
22
 
 
23
 
namespace {
24
 
  const bool CONTROLLER_BRIDGE_DEBUG = false;
25
 
}
26
 
 
27
 
 
28
 
Controller_impl::Controller_impl(BridgeConf* bridgeConf, const char* robotName, VirtualRobotRTC* virtualRobotRTC)
29
 
  : bridgeConf(bridgeConf),
30
 
    robotName(robotName),
31
 
    virtualRobotRTC(virtualRobotRTC)
32
 
{
33
 
  if(CONTROLLER_BRIDGE_DEBUG){
34
 
    cout << "Controller_impl::Controller_impl" << endl;
35
 
  }
36
 
 
37
 
  virtualRobotRTC->isOwnedByController = true;
38
 
 
39
 
  setupRtcs();
40
 
  setupRtcConnections();
41
 
}
42
 
 
43
 
 
44
 
Controller_impl::~Controller_impl()
45
 
{
46
 
  if(CONTROLLER_BRIDGE_DEBUG){
47
 
    cout << "Controller_impl::~Controller_impl" << endl;
48
 
  }
49
 
 
50
 
  delete naming;
51
 
 
52
 
  virtualRobotRTC->isOwnedByController = false;
53
 
  //virtualRobotRTC->exit();
54
 
}
55
 
 
56
 
 
57
 
void Controller_impl::makePortMap(PortMap& portMap, RTC::RTObject_ptr rtc)
58
 
{
59
 
  RTC::PortList_var ports = rtc->get_ports();
60
 
  for(CORBA::ULong i=0; i < ports->length(); ++i){
61
 
    RTC::PortProfile_var profile = ports[i]->get_port_profile();
62
 
    portMap[string(profile->name)] = ports[i];
63
 
  }
64
 
}
65
 
 
66
 
 
67
 
void Controller_impl::setupRtcs()
68
 
{
69
 
  RTC::Manager& rtcManager = RTC::Manager::instance();
70
 
 
71
 
  string nameServer = rtcManager.getConfig()["corba.nameservers"];
72
 
  cout << "setting naming" << endl;
73
 
  int comPos = nameServer.find(",");
74
 
  if (comPos < 0){
75
 
    comPos = nameServer.length();
76
 
  }
77
 
  nameServer = nameServer.substr(0, comPos);
78
 
  naming = new RTC::CorbaNaming(rtcManager.getORB(), nameServer.c_str());
79
 
 
80
 
  cout << "setup RT components" << endl;
81
 
 
82
 
  for(size_t i=0; i < bridgeConf->portConnections.size(); ++i){
83
 
 
84
 
    PortConnection& connection = bridgeConf->portConnections[i];
85
 
 
86
 
    CORBA::Object_ptr rtcRef = 0;
87
 
    string rtcName;
88
 
 
89
 
    if(connection.controllerInstanceName.empty()){
90
 
      if(!bridgeConf->moduleInfoList.empty()){
91
 
        RTC::RtcBase* rtcServant = bridgeConf->moduleInfoList.front().rtcServant;
92
 
        rtcName = rtcServant->getInstanceName();
93
 
        rtcRef = rtcServant->getObjRef();
94
 
      }
95
 
    } else {
96
 
      rtcName = connection.controllerInstanceName;
97
 
      rtcRef = naming->resolve(rtcName.c_str());
98
 
    }
99
 
 
100
 
    if(!rtcRef || CORBA::is_nil(rtcRef)){
101
 
      cout << rtcName << " is not found." << endl;
102
 
    } else {
103
 
 
104
 
      RtcInfoMap::iterator p = rtcInfoMap.find(rtcName);
105
 
      if(p == rtcInfoMap.end()){
106
 
 
107
 
        cout << rtcName << " is found." << endl;
108
 
 
109
 
        RtcInfoPtr rtcInfo(new RtcInfo());
110
 
        rtcInfo->rtcRef.setObject(rtcRef);
111
 
        makePortMap(rtcInfo->portMap, rtcInfo->rtcRef._ptr());
112
 
        rtcInfo->timeRate = 1.0;
113
 
 
114
 
        rtcInfoMap.insert(make_pair(rtcName, rtcInfo));
115
 
 
116
 
        RTC::ExecutionContextServiceList_var eclist = rtcInfo->rtcRef->get_execution_context_services();
117
 
        for(CORBA::ULong i=0; i < eclist->length(); ++i){
118
 
          if(!CORBA::is_nil(eclist[i])){
119
 
            rtcInfo->execContext.setObject(eclist[i]);
120
 
            cout << "The execution context of " << rtcName << " is detected" << endl;
121
 
            break;
122
 
          }
123
 
        }
124
 
      }
125
 
    }
126
 
  }
127
 
}
128
 
 
129
 
 
130
 
void Controller_impl::setupRtcConnections()
131
 
{
132
 
  for(size_t i=0; i < bridgeConf->portConnections.size(); ++i){
133
 
 
134
 
    const PortConnection& connection = bridgeConf->portConnections[i];
135
 
 
136
 
    string controllerInstanceName = connection.controllerInstanceName;
137
 
    if(controllerInstanceName.empty()){
138
 
      if(!bridgeConf->moduleInfoList.empty()){
139
 
        RTC::RtcBase* rtcServant = bridgeConf->moduleInfoList.front().rtcServant;
140
 
        controllerInstanceName = rtcServant->getInstanceName();
141
 
      }
142
 
    }
143
 
 
144
 
    RtcInfoMap::iterator p = rtcInfoMap.find(controllerInstanceName);
145
 
    if(p != rtcInfoMap.end()){
146
 
 
147
 
      RtcInfoPtr rtcInfo = p->second;
148
 
 
149
 
      cout << "connect " << virtualRobotRTC->getInstanceName() << ":" << connection.robotPortName;
150
 
      cout << " <--> " << controllerInstanceName << ":" << connection.controllerPortName << endl;
151
 
 
152
 
      PortMap::iterator q = rtcInfo->portMap.find(connection.controllerPortName);
153
 
      if(q == rtcInfo->portMap.end()){
154
 
        cout << controllerInstanceName << " does not have a port ";
155
 
        cout << connection.controllerPortName << endl;
156
 
      } else {
157
 
        RTC::Port_ptr controllerPortRef = q->second;
158
 
        
159
 
        PortHandlerPtr robotPortHandler = virtualRobotRTC->getPortHandler(connection.robotPortName);
160
 
        if(!robotPortHandler){
161
 
          cout << "The robot does not have a port named " << connection.robotPortName << endl;
162
 
        } else {
163
 
          bool connected = false;
164
 
          RTC::Port_ptr robotPortRef = robotPortHandler->portRef;
165
 
 
166
 
          if(!CORBA::is_nil(robotPortRef)){
167
 
            if(dynamic_pointer_cast<OutPortHandler>(robotPortHandler)){
168
 
              connected = connectPorts(robotPortRef, controllerPortRef);
169
 
            } else {
170
 
              connected = connectPorts(controllerPortRef, robotPortRef);
171
 
            }
172
 
          }
173
 
          if(connected){
174
 
            cout << " ... ok" << endl;
175
 
          } else {
176
 
            cout << " ... failed." << endl;
177
 
          }
178
 
        }
179
 
      }
180
 
    }
181
 
  }
182
 
}
183
 
 
184
 
 
185
 
bool Controller_impl::connectPorts(RTC::Port_ptr outPort, RTC::Port_ptr inPort)
186
 
{
187
 
  // connect ports
188
 
  RTC::ConnectorProfile cprof;
189
 
  cprof.connector_id = "";
190
 
  cprof.name = CORBA::string_dup("connector0");
191
 
  cprof.ports.length(2);
192
 
  cprof.ports[0] = RTC::Port::_duplicate(inPort);
193
 
  cprof.ports[1] = RTC::Port::_duplicate(outPort);
194
 
 
195
 
  CORBA_SeqUtil::push_back(cprof.properties,
196
 
                           NVUtil::newNV("dataport.interface_type",
197
 
                                         "CORBA_Any"));
198
 
  CORBA_SeqUtil::push_back(cprof.properties,
199
 
                           NVUtil::newNV("dataport.dataflow_type",
200
 
                                         "Push"));
201
 
  CORBA_SeqUtil::push_back(cprof.properties,
202
 
                           NVUtil::newNV("dataport.subscription_type",
203
 
                                         "Flush"));
204
 
  cout << "set " << endl;
205
 
  RTC::ReturnCode_t result = inPort->connect(cprof);
206
 
 
207
 
  return (result == RTC::RTC_OK);
208
 
}
209
 
 
210
 
 
211
 
void Controller_impl::setDynamicsSimulator(DynamicsSimulator_ptr dynamicsSimulator)
212
 
{
213
 
  this->dynamicsSimulator = DynamicsSimulator::_duplicate(dynamicsSimulator);
214
 
}
215
 
 
216
 
 
217
 
void Controller_impl::setViewSimulator(ViewSimulator_ptr viewSimulator)
218
 
{
219
 
  this->viewSimulator = ViewSimulator::_duplicate(viewSimulator);
220
 
}
221
 
 
222
 
 
223
 
void Controller_impl::start()
224
 
{
225
 
  if(CONTROLLER_BRIDGE_DEBUG){
226
 
    cout << "Controller_impl::onStart" << endl;
227
 
  }
228
 
 
229
 
  if(!CORBA::is_nil(viewSimulator)) {
230
 
    viewSimulator->getCameraSequenceOf(robotName.c_str(), cameras);
231
 
  }
232
 
 
233
 
  for(RtcInfoMap::iterator p = rtcInfoMap.begin(); p != rtcInfoMap.end(); ++p){
234
 
    RtcInfoPtr& rtcInfo = p->second;
235
 
    rtcInfo->timeRateCounter = 1.0;
236
 
    rtcInfo->execContext->activate_component(rtcInfo->rtcRef._ptr());
237
 
  }
238
 
}
239
 
 
240
 
 
241
 
SensorState& Controller_impl::getCurrentSensorState()
242
 
{
243
 
  if(!sensorStateUpdated){
244
 
    dynamicsSimulator->getCharacterSensorState(robotName.c_str(), sensorState);
245
 
    sensorStateUpdated = true;
246
 
  }
247
 
 
248
 
  return sensorState;
249
 
}
250
 
 
251
 
 
252
 
DblSequence* Controller_impl::getLinkDataFromSimulator
253
 
(const std::string& linkName, DynamicsSimulator::LinkDataType linkDataType)
254
 
{
255
 
  DblSequence_var data;
256
 
  dynamicsSimulator->getCharacterLinkData(robotName.c_str(), linkName.c_str(), linkDataType, data.out());
257
 
  return data._retn();
258
 
}
259
 
 
260
 
 
261
 
DblSequence* Controller_impl::getSensorDataFromSimulator(const std::string& sensorName)
262
 
{
263
 
  DblSequence_var data;
264
 
  dynamicsSimulator->getCharacterSensorValues(robotName.c_str(), sensorName.c_str(), data.out());
265
 
  return data._retn();
266
 
}
267
 
 
268
 
 
269
 
ImageData* Controller_impl::getCameraImageFromSimulator(int cameraId)
270
 
{
271
 
  ImageData_var imageData = cameras[cameraId]->getImageData();
272
 
  return imageData._retn();
273
 
}
274
 
    
275
 
 
276
 
void Controller_impl::input()
277
 
{
278
 
  if(CONTROLLER_BRIDGE_DEBUG){
279
 
    cout << "Controller_impl::onInput" << endl;
280
 
  }
281
 
 
282
 
  sensorStateUpdated = false;
283
 
 
284
 
  virtualRobotRTC->inputDataFromSimulator(this);
285
 
}
286
 
 
287
 
 
288
 
DblSequence& Controller_impl::getJointDataSeqRef(DynamicsSimulator::LinkDataType linkDataType)
289
 
{
290
 
  return outputJointValueSeqInfos[linkDataType].values;
291
 
}
292
 
 
293
 
 
294
 
void Controller_impl::flushJointDataSeqToSimulator(DynamicsSimulator::LinkDataType linkDataType)
295
 
{
296
 
  JointValueSeqInfoMap::iterator p = outputJointValueSeqInfos.find(linkDataType);
297
 
  if(p != outputJointValueSeqInfos.end()){
298
 
    JointValueSeqInfo& info = p->second;
299
 
    if(!info.flushed){
300
 
      dynamicsSimulator->setCharacterAllLinkData(robotName.c_str(), linkDataType, info.values);
301
 
      info.flushed = true;
302
 
    }
303
 
  }
304
 
}
305
 
 
306
 
 
307
 
void Controller_impl::output()
308
 
{
309
 
  if(CONTROLLER_BRIDGE_DEBUG){
310
 
    cout << "Controller_impl::output" << endl;
311
 
  }
312
 
 
313
 
  for(JointValueSeqInfoMap::iterator p = outputJointValueSeqInfos.begin();
314
 
      p != outputJointValueSeqInfos.end(); ++p){
315
 
    JointValueSeqInfo& info = p->second;
316
 
    info.flushed = false;
317
 
  }
318
 
 
319
 
  virtualRobotRTC->outputDataToSimulator(this);
320
 
}
321
 
 
322
 
 
323
 
void Controller_impl::control()
324
 
{
325
 
  if(CONTROLLER_BRIDGE_DEBUG){
326
 
    cout << "Controller_impl::control" << endl;
327
 
  }
328
 
 
329
 
  virtualRobotRTC->writeDataToOutPorts();
330
 
 
331
 
  for(RtcInfoMap::iterator p = rtcInfoMap.begin(); p != rtcInfoMap.end(); ++p){
332
 
    RtcInfoPtr& rtcInfo = p->second;
333
 
    rtcInfo->timeRateCounter += rtcInfo->timeRate;
334
 
    if(rtcInfo->timeRateCounter >= 1.0){
335
 
      rtcInfo->execContext->tick();
336
 
      rtcInfo->timeRateCounter -= 1.0;
337
 
    }
338
 
  }
339
 
 
340
 
  virtualRobotRTC->readDataFromInPorts(this);
341
 
}
342
 
 
343
 
 
344
 
void Controller_impl::stop()
345
 
{
346
 
 
347
 
}
348
 
 
349
 
 
350
 
void Controller_impl::destroy()
351
 
{
352
 
  if(CONTROLLER_BRIDGE_DEBUG){
353
 
    cout << "Controller_impl::destroy()" << endl;
354
 
  }
355
 
 
356
 
  PortableServer::POA_var poa = _default_POA();
357
 
  PortableServer::ObjectId_var id = poa->servant_to_id(this);
358
 
  poa->deactivate_object(id);
359
 
}
360
 
 
361
 
 
362
 
ControllerFactory_impl::ControllerFactory_impl(RTC::Manager* rtcManager, BridgeConf* bridgeConf)
363
 
  : rtcManager(rtcManager),
364
 
    bridgeConf(bridgeConf)
365
 
{
366
 
  if(CONTROLLER_BRIDGE_DEBUG){
367
 
    cout << "ControllerFactory_impl::ControllerFactory_impl()" << endl;
368
 
  }
369
 
 
370
 
  VirtualRobotRTC::registerFactory(rtcManager, bridgeConf->getVirtualRobotRtcTypeName());
371
 
 
372
 
  currentVirtualRobotRTC = createVirtualRobotRTC();
373
 
}
374
 
 
375
 
 
376
 
ControllerFactory_impl::~ControllerFactory_impl()
377
 
{
378
 
  if(CONTROLLER_BRIDGE_DEBUG){
379
 
    cout << "ControllerFactory_impl::~ControllerFactory_impl()" << endl;
380
 
  }
381
 
 
382
 
  PortableServer::POA_var poa = _default_POA();
383
 
  PortableServer::ObjectId_var id = poa->servant_to_id(this);
384
 
  poa->deactivate_object(id);
385
 
}
386
 
 
387
 
 
388
 
Controller_ptr ControllerFactory_impl::create(const char* robotName)
389
 
{
390
 
  if(CONTROLLER_BRIDGE_DEBUG){
391
 
    cout << "ControllerFactory_impl::createController()" << endl;
392
 
  }
393
 
 
394
 
  Controller_impl* controller = 0;
395
 
 
396
 
  if(!currentVirtualRobotRTC || currentVirtualRobotRTC->isOwnedByController){
397
 
    currentVirtualRobotRTC = createVirtualRobotRTC();
398
 
  }
399
 
 
400
 
  if(currentVirtualRobotRTC){
401
 
    try{
402
 
      controller = new Controller_impl(bridgeConf, robotName, currentVirtualRobotRTC);
403
 
      _default_POA()->activate_object(controller);
404
 
      
405
 
    } catch(CORBA_SystemException& ex){
406
 
      cerr << ex._rep_id() << endl;
407
 
      cerr << "exception in createController" << endl;
408
 
    } catch(std::invalid_argument& ex){
409
 
      cerr << "invalid argument : " << ex.what() << endl;
410
 
    } catch(...){
411
 
      cerr << "unknown exception in ControllerFactory_impl::create()" <<  endl;
412
 
    }
413
 
  }
414
 
  
415
 
  return controller->_this();
416
 
}
417
 
 
418
 
 
419
 
VirtualRobotRTC* ControllerFactory_impl::createVirtualRobotRTC()
420
 
{
421
 
  RTC::RtcBase* rtc = rtcManager->createComponent(bridgeConf->getVirtualRobotRtcTypeName());
422
 
  return dynamic_cast<VirtualRobotRTC*>(rtc);
423
 
}
424
 
 
425
 
 
426
 
void ControllerFactory_impl::shutdown()
427
 
{
428
 
  if(CONTROLLER_BRIDGE_DEBUG){
429
 
    cout << "ControllerFactory_impl::shutdown()" << endl;
430
 
  }
431
 
 
432
 
  rtcManager->terminate();
433
 
}