← Back to branch summary

~hrg/hrg-packaging/openhrp3

« back to all changes in this revision

Viewing changes to server/ControllerBridge.old/VirtualRobotPortHandler.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/VirtualRobotPortHandler.cpp
1
 
/**
2
 
   \file
3
 
   \author Shin'ichiro Nakaoka
4
 
*/
5
 
 
6
 
#include "VirtualRobotPortHandler.h"
7
 
 
8
 
#include "Controller_impl.h"
9
 
 
10
 
using namespace RTC;
11
 
using namespace OpenHRP;
12
 
using namespace OpenHRP::ControllerBridge;
13
 
 
14
 
namespace {
15
 
 
16
 
  template <typename TSrc>
17
 
  void copyDblArraySeqToTimedDoubleSeq(const TSrc& src, int arraySize, TimedDoubleSeq& dest)
18
 
  {
19
 
    CORBA::ULong n = src.length();
20
 
    CORBA::ULong m = n * arraySize;
21
 
    dest.data.length(m);
22
 
    CORBA::ULong destPos = 0;
23
 
    for(CORBA::ULong i=0; i < n; ++i){
24
 
      for(int j=0; j < arraySize; ++j){
25
 
        dest.data[destPos++] = src[i][j];
26
 
      }
27
 
    }
28
 
  }
29
 
 
30
 
  template <typename TSrcSeq, typename TDestSeq>
31
 
  void copyImageData();
32
 
 
33
 
 
34
 
  DynamicsSimulator::LinkDataType toDynamicsSimulatorLinkDataType(DataTypeId id)
35
 
  {
36
 
    switch(id){
37
 
    case JOINT_VALUE:        return DynamicsSimulator::JOINT_VALUE;
38
 
    case JOINT_VELOCITY:     return DynamicsSimulator::JOINT_VELOCITY;
39
 
    case JOINT_ACCELERATION: return DynamicsSimulator::JOINT_ACCELERATION;
40
 
    case JOINT_TORQUE:       return DynamicsSimulator::JOINT_TORQUE;
41
 
    case EXTERNAL_FORCE:     return DynamicsSimulator::EXTERNAL_FORCE;
42
 
    case ABS_TRANSFORM:      return DynamicsSimulator::ABS_TRANSFORM;
43
 
    default:                 return DynamicsSimulator::INVALID_DATA_TYPE;
44
 
    }
45
 
  }
46
 
}
47
 
 
48
 
 
49
 
PortHandler::~PortHandler()
50
 
{
51
 
 
52
 
}
53
 
 
54
 
 
55
 
SensorStateOutPortHandler::SensorStateOutPortHandler(PortInfo& info)
56
 
  : outPort(info.portName.c_str(), values)
57
 
{
58
 
  dataTypeId = info.dataTypeId;
59
 
}
60
 
 
61
 
 
62
 
void SensorStateOutPortHandler::inputDataFromSimulator(Controller_impl* controller)
63
 
{
64
 
  SensorState& state = controller->getCurrentSensorState();
65
 
 
66
 
  switch(dataTypeId) {
67
 
  case JOINT_VALUE:
68
 
    values.data = state.q;
69
 
    break;
70
 
  case JOINT_VELOCITY:
71
 
    values.data = state.dq;
72
 
    break;
73
 
  case JOINT_TORQUE:
74
 
    values.data = state.u;
75
 
    break;
76
 
  case FORCE_SENSOR:
77
 
    copyDblArraySeqToTimedDoubleSeq(state.force, 6, values);
78
 
    break;
79
 
  case RATE_GYRO_SENSOR:
80
 
    copyDblArraySeqToTimedDoubleSeq(state.rateGyro, 3, values);
81
 
    break;
82
 
  case ACCELERATION_SENSOR:
83
 
    copyDblArraySeqToTimedDoubleSeq(state.accel, 3, values);
84
 
    break;
85
 
  default:
86
 
    break;
87
 
  }
88
 
}
89
 
 
90
 
 
91
 
void SensorStateOutPortHandler::writeDataToPort()
92
 
{
93
 
  outPort.write();
94
 
}
95
 
 
96
 
 
97
 
LinkDataOutPortHandler::LinkDataOutPortHandler(PortInfo& info) :
98
 
  outPort(info.portName.c_str(), value),
99
 
  linkName(info.dataOwnerName)
100
 
{
101
 
  linkDataType = toDynamicsSimulatorLinkDataType(info.dataTypeId);
102
 
}
103
 
 
104
 
 
105
 
void LinkDataOutPortHandler::inputDataFromSimulator(Controller_impl* controller)
106
 
{
107
 
  DblSequence_var data = controller->getLinkDataFromSimulator(linkName, linkDataType);
108
 
  value.data = data;
109
 
}
110
 
 
111
 
 
112
 
void LinkDataOutPortHandler::writeDataToPort()
113
 
{
114
 
  outPort.write();
115
 
}
116
 
 
117
 
 
118
 
SensorDataOutPortHandler::SensorDataOutPortHandler(PortInfo& info) :
119
 
  outPort(info.portName.c_str(), value),
120
 
  sensorName(info.dataOwnerName)
121
 
{
122
 
 
123
 
}
124
 
 
125
 
 
126
 
void SensorDataOutPortHandler::inputDataFromSimulator(Controller_impl* controller)
127
 
{
128
 
  DblSequence_var data = controller->getSensorDataFromSimulator(sensorName);
129
 
  value.data = data;
130
 
}
131
 
 
132
 
 
133
 
void SensorDataOutPortHandler::writeDataToPort()
134
 
{
135
 
  outPort.write();
136
 
}
137
 
 
138
 
 
139
 
ColorImageOutPortHandler::ColorImageOutPortHandler(PortInfo& info) :
140
 
  outPort(info.portName.c_str(), image),
141
 
  cameraId(info.dataOwnerId)
142
 
{
143
 
 
144
 
}
145
 
 
146
 
 
147
 
void ColorImageOutPortHandler::inputDataFromSimulator(Controller_impl* controller)
148
 
{
149
 
  ImageData_var imageInput = controller->getCameraImageFromSimulator(cameraId);
150
 
  image.data = imageInput->longData;
151
 
}
152
 
 
153
 
 
154
 
void ColorImageOutPortHandler::writeDataToPort()
155
 
{
156
 
  outPort.write();
157
 
}
158
 
 
159
 
 
160
 
GrayScaleImageOutPortHandler::GrayScaleImageOutPortHandler(PortInfo& info)
161
 
  : outPort(info.portName.c_str(), image),
162
 
    cameraId(info.dataOwnerId)
163
 
{
164
 
 
165
 
}
166
 
 
167
 
 
168
 
void GrayScaleImageOutPortHandler::inputDataFromSimulator(Controller_impl* controller)
169
 
{
170
 
  ImageData_var imageInput = controller->getCameraImageFromSimulator(cameraId);
171
 
  image.data = imageInput->octetData;
172
 
}
173
 
 
174
 
 
175
 
void GrayScaleImageOutPortHandler::writeDataToPort()
176
 
{
177
 
  outPort.write();
178
 
}
179
 
 
180
 
 
181
 
DepthImageOutPortHandler::DepthImageOutPortHandler(PortInfo& info) :
182
 
  outPort(info.portName.c_str(), image),
183
 
  cameraId(info.dataOwnerId)
184
 
{
185
 
 
186
 
}
187
 
 
188
 
 
189
 
void DepthImageOutPortHandler::inputDataFromSimulator(Controller_impl* controller)
190
 
{
191
 
  ImageData_var imageInput = controller->getCameraImageFromSimulator(cameraId);
192
 
  image.data = imageInput->floatData;
193
 
}
194
 
 
195
 
 
196
 
void DepthImageOutPortHandler::writeDataToPort()
197
 
{
198
 
  outPort.write();
199
 
}
200
 
 
201
 
 
202
 
JointDataSeqInPortHandler::JointDataSeqInPortHandler(PortInfo& info) :
203
 
  inPort(info.portName.c_str(), values)
204
 
{
205
 
  linkDataType = toDynamicsSimulatorLinkDataType(info.dataTypeId);
206
 
}
207
 
 
208
 
 
209
 
void JointDataSeqInPortHandler::outputDataToSimulator(Controller_impl* controller)
210
 
{
211
 
  controller->flushJointDataSeqToSimulator(linkDataType);
212
 
}
213
 
 
214
 
 
215
 
void JointDataSeqInPortHandler::readDataFromPort(Controller_impl* controller)
216
 
{
217
 
  inPort.read();
218
 
 
219
 
  DblSequence& data = controller->getJointDataSeqRef(linkDataType);
220
 
  
221
 
  CORBA::ULong n = values.data.length();
222
 
  data.length(n);
223
 
  for(CORBA::ULong i=0; i < n; ++i){
224
 
    data[i] = values.data[i];
225
 
  }
226
 
}