18 package com.generalrobotix.ui.item;
20 import javax.media.j3d.Geometry;
21 import javax.media.j3d.IndexedTriangleArray;
22 import javax.media.j3d.QuadArray;
23 import javax.media.j3d.Shape3D;
24 import javax.media.j3d.Switch;
25 import javax.media.j3d.TransparencyAttributes;
26 import javax.media.j3d.TriangleFanArray;
27 import javax.vecmath.Point3f;
40 @SuppressWarnings({
"serial",
"unchecked" })
49 private Switch switchVisibleArea_ =
null;
51 public static final String[] sensorType = {
"Vision",
"RateGyro" ,
"Acceleration" ,
"Force" ,
"Range" };
52 protected static final String[] sensorTypeComboItem_ = sensorType;
53 protected static final String[] cameraTypeComboItem_ =
new String[] {
"NONE",
"COLOR" ,
"MONO" ,
"DEPTH" ,
"COLOR_DEPTH" ,
"MONO_DEPTH" };
61 super(name, manager, model);
65 specValues_ = info.specValues;
66 specFile_ = info.specFile;
67 translation(info.translation);
68 rotation(info.rotation);
69 int n = info.shapeIndices.length;
70 for(
int i=0;
i<
n;
i++)
71 addTransformedShapeIndex(info.shapeIndices[
i]);
72 buildShapeTransforms(info.inlinedShapeTransformMatrices);
75 type_ =
new String(
"Force");
76 specValues_ =
new float[]{-1.0f, -1.0f, -1.0f, -1.0f, -1.0f, -1.0f};
77 translation(
new double[]{0.0, 0.0, 0.0});
78 rotation(
new double[]{0.0, 0.0, 1.0, 0.0});
83 setProperty(
"type", type_);
84 setProperty(
"id", String.valueOf(id_));
85 setProperty(
"alwaysVisible",
"false");
86 setIcon(
"camera.png");
88 if (type_.equals(
"Vision")) {
89 CameraParameter prm =
new CameraParameter();
90 prm.defName =
new String(name);
91 prm.sensorName =
new String(name);
94 prm.frontClipDistance = (float)specValues_[0];
95 prm.backClipDistance = (float)specValues_[1];
96 prm.fieldOfView = (float)specValues_[2];
98 prm.type = CameraType.from_int((
int)specValues_[3]);
99 }
catch (Exception e) {
100 prm.type = CameraType.NONE;
102 prm.width = (
int)specValues_[4];
103 prm.height = (
int)specValues_[5];
104 prm.frameRate = (float)specValues_[6];
106 setDbl(
"frontClipDistance", prm.frontClipDistance, 4);
107 setDbl(
"backClipDistance", prm.backClipDistance, 4);
108 setDbl(
"fieldOfView", prm.fieldOfView, 6);
109 if (prm.type == CameraType.NONE){
110 setProperty(
"cameraType",
"NONE");
111 }
else if (prm.type == CameraType.COLOR){
112 setProperty(
"cameraType",
"COLOR");
113 }
else if (prm.type == CameraType.MONO){
114 setProperty(
"cameraType",
"MONO");
115 }
else if (prm.type == CameraType.COLOR_DEPTH){
116 setProperty(
"cameraType",
"COLOR_DEPTH");
117 }
else if (prm.type == CameraType.MONO_DEPTH){
118 setProperty(
"cameraType",
"MONO_DEPTH");
119 }
else if (prm.type == CameraType.DEPTH){
120 setProperty(
"cameraType",
"DEPTH");
122 setInt(
"width", prm.width);
123 setInt(
"height", prm.height);
124 setDbl(
"frameRate", prm.frameRate);
125 boolean offScreen =
false;
130 tg_.addChild(switchVisibleArea_);
131 }
else if(type_.equals(
"RateGyro")){
132 float[]
max =
new float[3];
133 max[0] = specValues_[0];
134 max[1] = specValues_[1];
135 max[2] = specValues_[2];
136 setFltAry(
"maxAngularVelocity", max);
137 }
else if(type_.equals(
"Acceleration")){
138 float[]
max =
new float[3];
139 max[0] = specValues_[0];
140 max[1] = specValues_[1];
141 max[2] = specValues_[2];
142 setFltAry(
"maxAcceleration", max);
143 }
else if(type_.equals(
"Force")){
144 float[] maxf =
new float[3];
145 maxf[0] = specValues_[0];
146 maxf[1] = specValues_[1];
147 maxf[2] = specValues_[2];
148 float[] maxt =
new float[3];
149 maxt[0] = specValues_[3];
150 maxt[1] = specValues_[4];
151 maxt[2] = specValues_[5];
152 setFltAry(
"maxForce", maxf);
153 setFltAry(
"maxTorque", maxt);
154 }
else if(type_.equals(
"Range")){
155 setFlt(
"scanAngle", specValues_[0]);
156 setFlt(
"scanStep", specValues_[1]);
157 setFlt(
"scanRate", specValues_[2]);
158 setFlt(
"maxDistance", specValues_[3]);
160 tg_.addChild(switchVisibleArea_);
167 GrxSensorItem s = (GrxSensorItem) o;
168 if (getOrder(type_) < getOrder(s.
type_))
184 if (type.equals(
"Force"))
186 else if (type.equals(
"RateGyro"))
188 else if (type.equals(
"Acceleration"))
190 else if (type.equals(
"Vision"))
192 else if (type.equals(
"Range"))
204 Short
id = getShort(value);
205 if (
id !=
null && id_ !=
id){
208 if (model_ !=
null) model_.notifyModified();
219 if (property.equals(
"name")){
221 }
else if(property.equals(
"translation")){
223 }
else if(property.equals(
"rotation")){
225 }
else if(property.equals(
"id")){
227 }
else if(property.equals(
"type")){
229 }
else if(property.equals(
"cameraType")){
231 }
else if(property.equals(
"alwaysVisible")){
232 if (value.startsWith(
"true")){
233 setProperty(
"alwaysVisible",
"true");
234 setVisibleArea(
true);
236 setProperty(
"alwaysVisible",
"false");
237 setVisibleArea(
false);
239 }
else if(property.equals(
"frameRate")){
240 setProperty(
"frameRate", value);
241 if (model_ !=
null) model_.notifyModified();
249 if (type_.equals(type))
return;
251 if (type.equals(
"Vision")){
252 if (specValues_ ==
null || specValues_.length != 7){
253 specValues_ =
new float[7];
254 _removeSensorSpecificProperties();
255 setProperty(
"frontClipDistance",
"0.01");
256 setProperty(
"backClipDistance",
"10.0");
257 setProperty(
"fieldOfView",
"0.0785398");
258 setProperty(
"cameraType",
"COLOR");
259 setProperty(
"width",
"320");
260 setProperty(
"height",
"240");
261 setProperty(
"frameRate",
"30.0");
263 specValues_[0] = getFlt(
"frontClipDistance",
null);
264 specValues_[1] = getFlt(
"backClipDistance",
null);
265 specValues_[2] = getFlt(
"fieldOfView",
null);
266 if (getStr(
"cameraType",
"NONE").equals(
"NONE")){
267 specValues_[3] = CameraType._NONE;
268 }
else if(getStr(
"cameraType",
"NONE").equals(
"COLOR")){
269 specValues_[3] = CameraType._COLOR;
270 }
else if(getStr(
"cameraType",
"NONE").equals(
"MONO")){
271 specValues_[3] = CameraType._MONO;
272 }
else if(getStr(
"cameraType",
"NONE").equals(
"DEPTH")){
273 specValues_[3] = CameraType._DEPTH;
274 }
else if(getStr(
"cameraType",
"NONE").equals(
"COLOR_DEPTH")){
275 specValues_[3] = CameraType._COLOR_DEPTH;
276 }
else if(getStr(
"cameraType",
"NONE").equals(
"MONO_DEPTH")){
277 specValues_[3] = CameraType._MONO_DEPTH;
279 setProperty(
"cameraType",
"NONE");
281 specValues_[4] = getInt(
"width",
null);
282 specValues_[5] = getInt(
"height",
null);
283 specValues_[6] = getFlt(
"frameRate",
null);
286 }
else if(type.equals(
"RateGyro")){
287 if (specValues_ ==
null || specValues_.length != 3 || getProperty(
"maxAngularVelocity")==
null){
288 specValues_ =
new float[]{-1.0f, -1.0f, -1.0f};
289 _removeSensorSpecificProperties();
290 setFltAry(
"maxAngularVelocity", specValues_);
292 specValues_ = getFltAry(
"maxAngularVelocity",
null);
293 }
else if(type.equals(
"Acceleration")){
294 if (specValues_ ==
null || specValues_.length != 3 || getProperty(
"maxAcceleration")==
null){
295 specValues_ =
new float[]{-1.0f, -1.0f, -1.0f};
296 _removeSensorSpecificProperties();
297 setFltAry(
"maxAcceleration", specValues_);
299 specValues_ = getFltAry(
"maxAcceleration",
null);
300 }
else if(type.equals(
"Force")){
301 if (specValues_ ==
null || specValues_.length != 6){
302 specValues_ =
new float[]{-1.0f, -1.0f, -1.0f, -1.0f, -1.0f, -1.0f};
303 _removeSensorSpecificProperties();
304 setFltAry(
"maxForce",
new float[]{-1.0f, -1.0f, -1.0f});
305 setFltAry(
"maxTorque",
new float[]{-1.0f, -1.0f, -1.0f});
307 float[] maxf = getFltAry(
"maxForce",
null);
308 float[] maxt = getFltAry(
"maxTorque",
null);
309 specValues_[0] = maxf[0];
310 specValues_[1] = maxf[1];
311 specValues_[2] = maxf[2];
312 specValues_[3] = maxt[0];
313 specValues_[4] = maxt[1];
314 specValues_[5] = maxt[2];
315 }
else if(type.equals(
"Range")){
316 if (specValues_ ==
null || specValues_.length != 3){
317 specValues_ =
new float[]{3.14159f, 0.1f, 10.0f, 10.0f};
318 _removeSensorSpecificProperties();
319 setFlt(
"scanAngle", 3.14159
f);
320 setFlt(
"scanStep", 0.1
f);
321 setFlt(
"scanRate", 10.0
f);
322 setFlt(
"maxDistance", 10.0
f);
324 specValues_[0] = getFlt(
"scanAngle", 3.14159
f);
325 specValues_[1] = getFlt(
"scanStep", 0.1
f);
326 specValues_[2] = getFlt(
"scanRate", 10.0
f);
328 System.out.println(
"GrxSensorItem.propertyChanged() : unknown sensor type : "+type_);
332 setProperty(
"type", type);
333 if (model_ !=
null) model_.notifyModified();
340 remove(
"maxAngularVelocity");
342 remove(
"maxAcceleration");
344 remove(
"frontClipDistance");
345 remove(
"backClipDistance");
346 remove(
"fieldOfView");
350 remove(
"cameraType");
355 remove(
"maxDistance");
377 if (type_.equals(
"Range")){
378 float step = specValues_[1];
379 int half = distances.length/2;
380 float maxD = getFlt(
"maxDistance", 10.0
f);
381 Point3f[] p3f =
new Point3f[distances.length*2+1];
382 p3f[0] =
new Point3f(0,0,0);
383 double startAngle = -step*half-step/2;
384 for(
int i=0;
i<distances.length;
i++){
385 double distance = distances[
i];
388 p3f[2*
i+1] =
new Point3f(
389 (
float)(-distance*Math.sin(startAngle)),
391 (
float)(-distance*Math.cos(startAngle)));
393 p3f[2*
i+2] =
new Point3f(
394 (
float)(-distance*Math.sin(startAngle)),
396 (
float)(-distance*Math.cos(startAngle)));
408 if (type_.equals(
"Range")){
409 Point3f[] p3f = _distances2points(distances);
410 if (p3f ==
null)
return;
411 Shape3D shapeNode = (Shape3D)switchVisibleArea_.getChild(0);
412 Geometry gm = (Geometry)shapeNode.getGeometry(0);
413 if (gm instanceof TriangleFanArray){
414 TriangleFanArray tri = (TriangleFanArray)gm;
415 tri.setCoordinates(0, p3f);
424 if (type_.equals(
"Range")){
425 double scanAngle = specValues_[0];
426 double step = specValues_[1];
427 float d = specValues_[3];
429 int half = (
int)(length/2);
430 Point3f[] p3f =
new Point3f[length*2+1];
431 p3f[0] =
new Point3f(0,0,0);
432 double startAngle = -step*half-step/2;
434 p3f[2*
i+1] =
new Point3f(
435 (
float)(-d*Math.sin(startAngle)),
437 (
float)(-d*Math.cos(startAngle)));
439 p3f[2*
i+2] =
new Point3f(
440 (
float)(-d*Math.sin(startAngle)),
442 (
float)(-d*Math.cos(startAngle)));
444 int[] stripVertexCounts = { p3f.length };
445 TriangleFanArray tri =
new TriangleFanArray(p3f.length,
446 TriangleFanArray.COORDINATES,
448 tri.setCapability(QuadArray.ALLOW_COORDINATE_READ);
449 tri.setCapability(QuadArray.ALLOW_COORDINATE_WRITE);
450 tri.setCoordinates(0, p3f);
451 javax.media.j3d.Appearance
app =
new javax.media.j3d.Appearance();
452 app.setTransparencyAttributes(
453 new TransparencyAttributes(TransparencyAttributes.FASTEST, 0.5f)
455 Shape3D s3d =
new Shape3D(tri,
app);
456 s3d.setCapability(Shape3D.ALLOW_GEOMETRY_READ);
457 s3d.setCapability(Shape3D.ALLOW_GEOMETRY_WRITE);
464 float enlarge = 0.001f;
465 float f = (float)prm.backClipDistance*(1.0f+enlarge);
466 float n = (float)prm.frontClipDistance*(1.0f-enlarge);
467 double theta = prm.fieldOfView;
468 float aspect = ((float)prm.height)/prm.width;
469 float nx = (float)Math.tan(theta/2)*
n;
470 float ny = nx*aspect;
471 float fx = (float)Math.tan(theta/2)*
f;
472 float fy = fx*aspect;
475 new Point3f(nx,ny,-n),
476 new Point3f(-nx,ny,-n),
477 new Point3f(-nx,-ny,-n),
478 new Point3f(nx,-ny,-n),
479 new Point3f(fx,fy,-f),
480 new Point3f(-fx,fy,-f),
481 new Point3f(-fx,-fy,-f),
482 new Point3f(fx,-fy,-f),
485 int vertIndices[] = {0,1,2,0,2,3,1,0,4,1,4,5,0,3,7,0,7,4,5,2,1,5,6,2,6,7,3,6,3,2,5,4,7,5,7,6};
486 IndexedTriangleArray tri =
487 new IndexedTriangleArray(p3f.length,
488 IndexedTriangleArray.COORDINATES,
491 tri.setCoordinates(0, p3f);
492 tri.setCoordinateIndices(0,vertIndices);
493 javax.media.j3d.Appearance
app =
new javax.media.j3d.Appearance();
494 app.setTransparencyAttributes(
495 new TransparencyAttributes(TransparencyAttributes.FASTEST, 0.5f)
497 Shape3D s3d =
new Shape3D(tri,
app);
506 if (switchVisibleArea_ !=
null){
507 switchVisibleArea_.setWhichChild(b? Switch.CHILD_ALL:Switch.CHILD_NONE);
512 return switchVisibleArea_ !=
null && switchVisibleArea_.getWhichChild() == Switch.CHILD_ALL;
525 if (isFalse(
"alwaysVisible")) setVisibleArea(b);
543 String old = getStr(
"cameraType",
"NONE");
544 if(!type_.equals(
"Vision") || old.equals(cameraType))
return;
546 if(cameraType.equals(
"NONE")){
547 specValues_[3] = CameraType._NONE;
548 }
else if(cameraType.equals(
"COLOR")){
549 specValues_[3] = CameraType._COLOR;
550 }
else if(cameraType.equals(
"MONO")){
551 specValues_[3] = CameraType._MONO;
552 }
else if(cameraType.equals(
"DEPTH")){
553 specValues_[3] = CameraType._DEPTH;
554 }
else if(cameraType.equals(
"COLOR_DEPTH")){
555 specValues_[3] = CameraType._COLOR_DEPTH;
556 }
else if(cameraType.equals(
"MONO_DEPTH")){
557 specValues_[3] = CameraType._MONO_DEPTH;
559 System.out.println(
"GrxSensorItem.propertyChanged() : unknown camera type : " + cameraType);
562 setProperty(
"cameraType", cameraType);
563 if (model_ !=
null) model_.notifyModified();
568 if(key.matches(
"type")) {
570 }
else if(type_.equals(
"Vision") && key.equals(
"cameraType")){
573 if(key.matches(
"alwaysVisible")){
576 return super.GetValueEditType(key);
boolean propertyChanged(String property, String value)
check validity of new value of property and update if valid
png_infop png_charp png_int_32 png_int_32 int * type
void setVisibleArea(boolean b)
make visible area visible/invisible
#define null
our own NULL pointer
GrxSensorItem(String name, GrxPluginManager manager, GrxModelItem model, SensorInfo info)
constructor
Point3f [] _distances2points(double[] distances)
convert array of distance into array of 3D point
png_infop png_charpp name
RTC::ReturnCode_t ret(RTC::Local::ReturnCode_t r)
item corresponds to a robot model
void cameraType(String cameraType)
png_bytep png_bytep png_size_t length
ValueEditType GetValueEditType(String key)
int getOrder(String type)
get sensor type as integer value
BranchGroup getBranchGroup()
void updateShapeOfVisibleArea(double[] distances)
update shape of visible area(only used for RangeSensor)
CameraParameter getCameraParameter()
void _removeSensorSpecificProperties()
void setFocused(boolean b)
set/unset fucus on this item
GrxSensorItem clone()
Override clone method.
プラグイン管理クラス GrxUIの核になるクラス。プラグインのロード等の、初期化を実行する。 プラグインとそ...
static Switch _makeSwitchNode(Shape3D shape)
static int max(int a, int b)
Shape3D _createShapeOfVisibleArea()
create shape of visible area