VirtualRobotRTC.cpp
Go to the documentation of this file.
1 // -*- mode: c++; c-basic-offset: 2; -*-
2 /*
3  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
4  * All rights reserved. This program is made available under the terms of the
5  * Eclipse Public License v1.0 which accompanies this distribution, and is
6  * available at http://www.eclipse.org/legal/epl-v10.html
7  * Contributors:
8  * National Institute of Advanced Industrial Science and Technology (AIST)
9  * General Robotix Inc.
10  */
16 #include"VirtualRobotRTC.h"
17 #include "Controller_impl.h"
18 
19 #include <iostream>
20 #include <boost/bind.hpp>
21 
22 using namespace std;
23 using namespace boost;
24 
25 namespace {
26  const bool CONTROLLER_BRIDGE_DEBUG = false;
27 }
28 
29 
30 void VirtualRobotRTC::registerFactory(RTC::Manager* manager, const char* componentTypeName)
31 {
32  static const char* spec[] = {
33  "implementation_id", "VirtualRobot",
34  "type_name", "VirtualRobot",
35  "description", "This component enables controller components to"
36  "access the I/O of a virtual robot in a OpenHRP simulation",
37  "version", "1.0",
38  "vendor", "AIST",
39  "category", "OpenHRP",
40  "activity_type", "DataFlowComponent",
41  "max_instance", "1",
42  "language", "C++",
43  "lang_type", "compile",
44  ""
45  };
46 
47  if(CONTROLLER_BRIDGE_DEBUG){
48  cout << "initVirtualRobotRTC()" << endl;
49  }
50 
52  profile.setDefault("type_name", componentTypeName);
53 
54  manager->registerFactory(profile,
55  RTC::Create<VirtualRobotRTC>,
56  RTC::Delete<VirtualRobotRTC>);
57 }
58 
59 
61  : RTC::DataFlowComponentBase(manager)
62 {
63  if(CONTROLLER_BRIDGE_DEBUG){
64  cout << "VirtualRobotRTC::VirtualRobotRTC" << endl;
65  }
66 
67  isOwnedByController = false;
68 }
69 
70 RTC::ReturnCode_t VirtualRobotRTC::onInitialize()
71 {
72  BridgeConf* bridgeConf = BridgeConf::instance();
73 
74  PortInfoMap::iterator it;
75 
76  for(it = bridgeConf->outPortInfos.begin(); it != bridgeConf->outPortInfos.end(); ++it){
77 #ifdef OPENRTM_VERSION_042
78  createOutPortHandler(it->second);
79 #else
80  if (!createOutPortHandler(it->second)){
81  cerr << "createOutPortHandler(" << it->second.portName << ") failed" << std::endl;
82  }
83 #endif
84  }
85 
86  for(it = bridgeConf->inPortInfos.begin(); it != bridgeConf->inPortInfos.end(); ++it){
87 #ifdef OPENRTM_VERSION_042
88  createInPortHandler(it->second);
89 #else
90  if (!createInPortHandler(it->second)){
91  cerr << "createInPortHandler(" << it->second.portName << ") failed" << std::endl;
92  }
93 #endif
94  }
95 
97  return RTC::RTC_OK;
98 }
99 
100 
102 {
103  if(CONTROLLER_BRIDGE_DEBUG){
104  cout << "VirtualRobotRTC::~VirtualRobotRTC" << endl;
105  }
106 }
107 
108 #ifdef OPENRTM_VERSION_042
109 
111 {
112  DataTypeId dataTypeId = portInfo.dataTypeId;
113 
114  if(portInfo.dataOwnerName.empty()){
115  switch(dataTypeId) {
116 
117  case JOINT_VALUE:
118  case JOINT_VELOCITY:
119  case JOINT_ACCELERATION:
120  case JOINT_TORQUE:
121  case FORCE_SENSOR:
122  case RATE_GYRO_SENSOR:
123  case ACCELERATION_SENSOR:
125  break;
126 
127  case COLOR_IMAGE:
129  break;
130 
131  case GRAYSCALE_IMAGE:
133  break;
134 
135  case DEPTH_IMAGE:
137  break;
138 
139  default:
140  break;
141  }
142  } else {
143 
144  switch(dataTypeId) {
145 
146  case JOINT_VALUE:
147  case JOINT_VELOCITY:
148  case JOINT_ACCELERATION:
149  case JOINT_TORQUE:
150  case ABS_TRANSFORM:
151  case ABS_VELOCITY:
152  case ABS_ACCELERATION:
153  case CONSTRAINT_FORCE:
155  break;
156  case ABS_TRANSFORM2:
158  break;
159 
160  case FORCE_SENSOR:
161  case RATE_GYRO_SENSOR:
162  case ACCELERATION_SENSOR:
163  case RANGE_SENSOR:
165  break;
166 
167  default:
168  break;
169  }
170  }
171 }
172 
173 
175 {
176  DataTypeId dataTypeId = portInfo.dataTypeId;
177  if(portInfo.dataOwnerName.empty()){
178  switch(dataTypeId) {
179  case JOINT_VALUE:
180  case JOINT_VELOCITY:
181  case JOINT_ACCELERATION:
182  case JOINT_TORQUE:
184  break;
185  default:
186  break;
187  }
188  }else{
189  switch(dataTypeId){
190  case JOINT_VALUE:
191  case JOINT_VELOCITY:
192  case JOINT_ACCELERATION:
193  case JOINT_TORQUE:
195  break;
196  case ABS_TRANSFORM:
197  case ABS_VELOCITY:
198  case ABS_ACCELERATION:
199  case EXTERNAL_FORCE:
201  break;
202  case ABS_TRANSFORM2:
204  break;
205  default:
206  break;
207  }
208  }
209 }
210 
211 #else
212 
214 {
215  DataTypeId dataTypeId = portInfo.dataTypeId;
216  bool ret = false;
217 
218  if(portInfo.dataOwnerName.empty()){
219  switch(dataTypeId) {
220 
221  case JOINT_VALUE:
222  case JOINT_VELOCITY:
223  case JOINT_ACCELERATION:
224  case JOINT_TORQUE:
225  case FORCE_SENSOR:
226  case RATE_GYRO_SENSOR:
227  case ACCELERATION_SENSOR:
229  break;
230 
231  case COLOR_IMAGE:
233  break;
234 
235  case GRAYSCALE_IMAGE:
237  break;
238 
239  case DEPTH_IMAGE:
241  break;
242 
243  default:
244  break;
245  }
246  } else {
247 
248  switch(dataTypeId) {
249 
250  case JOINT_VALUE:
251  case JOINT_VELOCITY:
252  case JOINT_ACCELERATION:
253  case JOINT_TORQUE:
254  case ABS_TRANSFORM:
255  case ABS_VELOCITY:
256  case ABS_ACCELERATION:
257  case CONSTRAINT_FORCE:
258  ret = registerOutPortHandler(new LinkDataOutPortHandler(portInfo));
259  break;
260  case ABS_TRANSFORM2:
262  break;
263 
264  case FORCE_SENSOR:
265  case RATE_GYRO_SENSOR:
266  case ACCELERATION_SENSOR:
267  case RANGE_SENSOR:
269  break;
270  case RATE_GYRO_SENSOR2:
272  break;
275  break;
276 
277  default:
278  break;
279  }
280  }
281  return ret;
282 }
283 
284 
286 {
287  DataTypeId dataTypeId = portInfo.dataTypeId;
288  bool ret = false;
289  if(portInfo.dataOwnerName.empty()){
290  switch(dataTypeId) {
291  case JOINT_VALUE:
292  case JOINT_VELOCITY:
293  case JOINT_ACCELERATION:
294  case JOINT_TORQUE:
296  break;
297  default:
298  break;
299  }
300  }else{
301  switch(dataTypeId){
302  case JOINT_VALUE:
303  case JOINT_VELOCITY:
304  case JOINT_ACCELERATION:
305  case JOINT_TORQUE:
306  ret = registerInPortHandler(new LinkDataInPortHandler(portInfo));
307  break;
308  case ABS_TRANSFORM2:
310  break;
311  case ABS_TRANSFORM:
312  case ABS_VELOCITY:
313  case ABS_ACCELERATION:
314  case EXTERNAL_FORCE:
315  ret = registerInPortHandler(new LinkDataInPortHandler(portInfo));
316  break;
317  default:
318  break;
319  }
320  }
321  return ret;
322 }
323 #endif
324 
326 {
327  string name(name_);
328  string::size_type index = name.rfind(".");
329  if (index != string::npos) name = name.substr(index+1);
330 
331  PortHandlerPtr portHandler;
332 
333  OutPortHandlerMap::iterator p = outPortHandlers.find(name);
334  if(p != outPortHandlers.end()){
335  portHandler = p->second;
336  } else {
337  InPortHandlerMap::iterator q = inPortHandlers.find(name);
338  if(q != inPortHandlers.end()){
339  portHandler = q->second;
340  }
341  }
342 
343  return portHandler;
344 }
345 
346 
348 {
349  for(OutPortHandlerMap::iterator it = outPortHandlers.begin(); it != outPortHandlers.end(); ++it){
350  OutPortHandlerPtr& handler = it->second;
351  handler->portRef = Port_Service_Type::_nil();
352  }
353  for(InPortHandlerMap::iterator it = inPortHandlers.begin(); it != inPortHandlers.end(); ++it){
354  InPortHandlerPtr& handler = it->second;
355  handler->portRef = Port_Service_Type::_nil();
356  }
357 
359 
360  for(CORBA::ULong i=0; i < ports->length(); ++i){
361 
362  RTC::PortProfile_var profile = ports[i]->get_port_profile();
363  PortHandlerPtr portHandler = getPortHandler(string(profile->name));
364 
365  if(portHandler){
366  portHandler->portRef = ports[i];
367  }
368  }
369 }
370 
371 
373 {
374  RTC::RTCList* rtcList = new RTC::RTCList;
375 
376  set<string> foundRtcNames;
377 
378  for(OutPortHandlerMap::iterator it = outPortHandlers.begin(); it != outPortHandlers.end(); ++it){
379  OutPortHandlerPtr& handler = it->second;
380  addConnectedRtcs(handler->portRef, *rtcList, foundRtcNames);
381  }
382  for(InPortHandlerMap::iterator it = inPortHandlers.begin(); it != inPortHandlers.end(); ++it){
383  InPortHandlerPtr& handler = it->second;
384  addConnectedRtcs(handler->portRef, *rtcList, foundRtcNames);
385  }
386 
387  return rtcList;
388 }
389 
390 
391 void VirtualRobotRTC::addConnectedRtcs(Port_Service_Ptr_Type portRef, RTC::RTCList& rtcList, std::set<std::string>& foundRtcNames)
392 {
393  RTC::PortProfile_var portProfile = portRef->get_port_profile();
394  string portName(portProfile->name);
395 
396  RTC::ConnectorProfileList_var connectorProfiles = portRef->get_connector_profiles();
397 
398  for(CORBA::ULong i=0; i < connectorProfiles->length(); ++i){
399  RTC::ConnectorProfile& connectorProfile = connectorProfiles[i];
400  Port_Service_List_Type& connectedPorts = connectorProfile.ports;
401 
402  for(CORBA::ULong j=0; j < connectedPorts.length(); ++j){
403  Port_Service_Ptr_Type connectedPortRef = connectedPorts[j];
404  RTC::PortProfile_var connectedPortProfile = connectedPortRef->get_port_profile();
405  RTC::RTObject_var connectedRtcRef = RTC::RTObject::_duplicate(connectedPortProfile->owner);
406  RTC::RTObject_var thisRef = RTC::RTObject::_duplicate(getObjRef());
407 
408  if(!CORBA::is_nil(connectedRtcRef) && !connectedRtcRef->_is_equivalent(thisRef)){
409  CORBA::ULong ii=0;
410  for(; ii<rtcList.length(); ii++){
411  if(rtcList[ii]->_is_equivalent(connectedRtcRef))
412  break;
413  }
414 
415  RTC::ComponentProfile_var componentProfile = connectedRtcRef->get_component_profile();
416  string connectedRtcName(componentProfile->instance_name);
417 
418  cout << "detected a port connection: ";
419  cout << "\"" << portName << "\" of " << getInstanceName() << " <--> \"";
420  cout << connectedPortProfile->name << "\" of " << connectedRtcName << endl;
421 
422  if(ii == rtcList.length()){
423 
424 #ifdef OPENRTM_VERSION_042
425  RTC::ExecutionContextServiceList_var execServices = connectedRtcRef->get_execution_context_services();
426 
427  for(CORBA::ULong k=0; k < execServices->length(); k++) {
428  RTC::ExecutionContextService_var execContext = execServices[k];
429 
430  ExtTrigExecutionContextService_Var_Type extTrigExecContext =
431  ExtTrigExecutionContextService_Type::_narrow(execContext);
432 
433  if(!CORBA::is_nil(extTrigExecContext)){
434  CORBA::ULong n = rtcList.length();
435  rtcList.length(n + 1);
436  rtcList[n] = connectedRtcRef;
437  foundRtcNames.insert(connectedRtcName);
438  }
439  }
440 #else
441  RTC::ExecutionContextList_var execServices = connectedRtcRef->get_owned_contexts();
442 
443  for(CORBA::ULong k=0; k < execServices->length(); k++) {
444  RTC::ExecutionContext_var execContext = execServices[k];
445 
446  ExtTrigExecutionContextService_Var_Type extTrigExecContext =
447  ExtTrigExecutionContextService_Type::_narrow(execContext);
448 
449  if(!CORBA::is_nil(extTrigExecContext)){
450  CORBA::ULong n = rtcList.length();
451  rtcList.length(n + 1);
452  rtcList[n] = connectedRtcRef;
453  foundRtcNames.insert(connectedRtcName);
454  }
455  }
456 #endif
457 
458  }
459  }
460  }
461  }
462 }
463 
464 
466 {
467  double controlTime = controller->controlTime;
468  double controlTimeStep = controller->getTimeStep();
469  for(OutPortHandlerMap::iterator it = outPortHandlers.begin(); it != outPortHandlers.end(); ++it){
470  double stepTime = it->second->stepTime;
471  if(stepTime){
472  double w = controlTime-(int)(controlTime/stepTime)*stepTime + controlTimeStep/2;
473  if(w >= stepTime) w=0;
474  if(w < controlTimeStep )
475  it->second->inputDataFromSimulator(controller);
476  }else{
477  it->second->inputDataFromSimulator(controller);
478  }
479  }
480 }
481 
482 
484 {
485  for(InPortHandlerMap::iterator it = inPortHandlers.begin(); it != inPortHandlers.end(); ++it){
486  it->second->outputDataToSimulator(controller);
487  }
488 }
489 
490 
492 {
493  double controlTime = controller->controlTime;
494  double controlTimeStep = controller->getTimeStep();
495  for(OutPortHandlerMap::iterator it = outPortHandlers.begin(); it != outPortHandlers.end(); ++it){
496  double stepTime = it->second->stepTime;
497  if(stepTime){
498  double w = controlTime-(int)(controlTime/stepTime)*stepTime + controlTimeStep/2;
499  if(w >= stepTime) w=0;
500  if(w < controlTimeStep )
501  it->second->writeDataToPort();
502  }else{
503  it->second->writeDataToPort();
504  }
505  }
506 }
507 
508 
510 {
511  for(InPortHandlerMap::iterator it = inPortHandlers.begin(); it != inPortHandlers.end(); ++it){
512  it->second->readDataFromPort(controller);
513  }
514 }
515 
517 {
518 
519 }
520 
521 bool VirtualRobotRTC::checkOutPortStepTime(double controlTimeStep)
522 {
523  bool ret = true;
524  for(OutPortHandlerMap::iterator it = outPortHandlers.begin(); it != outPortHandlers.end(); ++it){
525  double stepTime = it->second->stepTime;
526  if(stepTime && stepTime < controlTimeStep){
527  cerr << "OutPort(" << it->second->portName << ") : Output interval(" << stepTime << ") must be longer than the control interval(" << controlTimeStep << ")." << std::endl;
528  ret &= false;
529  }
530  }
531  return ret;
532 }
png_infop png_charpp int png_charpp profile
Definition: png.h:2382
bool createInPortHandler(PortInfo &portInfo)
void readDataFromInPorts(Controller_impl *controller)
PortInfoMap inPortInfos
Definition: BridgeConf.h:110
Modifications controlling boost library behavior.
Definition: ColladaUtil.h:306
void writeDataToOutPorts(Controller_impl *controller)
void outputDataToSimulator(Controller_impl *controller)
void addConnectedRtcs(Port_Service_Ptr_Type portRef, RTC::RTCList &rtcList, std::set< std::string > &foundRtcNames)
OpenRTM::ExtTrigExecutionContextService_var ExtTrigExecutionContextService_Var_Type
boost::shared_ptr< InPortHandler > InPortHandlerPtr
PortHandlerPtr getPortHandler(const std::string &name)
png_infop png_charpp name
Definition: png.h:2382
RTC::ReturnCode_t ret(RTC::Local::ReturnCode_t r)
virtual PortServiceList * get_ports()
boost::shared_ptr< OutPortHandler > OutPortHandlerPtr
w
VirtualRobotRTC(RTC::Manager *manager)
RTC::PortServiceList_var Port_Service_List_Var_Type
png_uint_32 i
Definition: png.h:2735
PortInfoMap outPortInfos
Definition: BridgeConf.h:109
DataTypeId
Definition: BridgeConf.h:34
boost::shared_ptr< PortHandler > PortHandlerPtr
static BridgeConf * instance()
Definition: BridgeConf.cpp:46
def j(str, encoding="cp932")
list index
void handler(int)
void inputDataFromSimulator(Controller_impl *controller)
DataTypeId dataTypeId
Definition: BridgeConf.h:59
const char * getInstanceName()
RTC::PortServiceList Port_Service_List_Type
bool checkOutPortStepTime(double controlTimeStep)
typedef int
Definition: png.h:1113
InPortHandlerMap inPortHandlers
static void registerFactory(RTC::Manager *manager, const char *componentTypeName)
RTC::RTCList * getConnectedRtcs()
RTC::PortService_ptr Port_Service_Ptr_Type
bool registerInPortHandler(TInPortHandler *handler)
std::vector< std::string > dataOwnerName
Definition: BridgeConf.h:60
double getTimeStep()
bool registerOutPortHandler(TOutPortHandler *handler)
bool registerFactory(coil::Properties &profile, RtcNewFunc new_func, RtcDeleteFunc delete_func)
OutPortHandlerMap outPortHandlers
RTC::ReturnCode_t onInitialize()
RTObject_ptr getObjRef() const
std::string setDefault(const std::string &key, const std::string &value)
bool createOutPortHandler(PortInfo &portInfo)


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sat Apr 13 2019 02:14:26