1 package cz.cuni.amis.pogamut.usar2004.communication.messages.usarinfomessages;
2
3 import java.util.*;
4 import javax.vecmath.*;
5 import cz.cuni.amis.pogamut.base.communication.worldview.event.*;
6 import cz.cuni.amis.pogamut.base.communication.worldview.object.*;
7 import cz.cuni.amis.pogamut.base.communication.translator.event.*;
8 import cz.cuni.amis.pogamut.base3d.worldview.object.*;
9 import cz.cuni.amis.pogamut.usar2004.communication.messages.*;
10 import cz.cuni.amis.pogamut.usar2004.communication.messages.datatypes.*;
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31 public class GeometryMessage extends GBEvent implements IWorldEvent, IWorldChangeEvent
32 {
33 public GeometryMessage(String Type, String Name, double WheelBase, double WheelRadius, double WheelSeparation, Point3d COG, Vector3d Dimensions)
34 {
35 this.Type = Type;
36 this.Name = Name;
37 this.WheelBase = WheelBase;
38 this.WheelRadius = WheelRadius;
39 this.WheelSeparation = WheelSeparation;
40 this.CenterOfGravity = COG;
41 this.Dimensions = Dimensions;
42 }
43
44
45
46 public static final String PROTOTYPE = "GEO {Type text} {Name text} {WheelBase 0} {WheelRadius 0} {WheelSeparation 0} {CenterOfGravity 0,0,0} {Dimensions 0,0,0}";
47
48 protected String Type = null;
49
50
51
52
53
54
55
56
57 public String getType()
58 {
59 return Type;
60 }
61
62 protected String Name = null;
63
64
65
66
67
68
69 public String getName()
70 {
71 return Name;
72 }
73 protected Vector3d Dimensions = null;
74
75
76
77
78
79
80
81
82 public Vector3d getDimensions()
83 {
84 return Dimensions;
85 }
86 protected Point3d CenterOfGravity = null;
87
88
89
90
91
92
93
94 public Point3d getCenterOfGravity()
95 {
96 return CenterOfGravity;
97 }
98 protected double WheelRadius = 0;
99
100
101
102
103
104
105 public double getWheelRadius()
106 {
107 return WheelRadius;
108 }
109 protected double WheelSeparation = 0;
110
111
112
113
114
115
116
117
118 public double getWheelSeparation()
119 {
120 return WheelSeparation;
121 }
122 protected double WheelBase = 0;
123
124
125
126
127
128
129
130
131 public double getWheelBase()
132 {
133 return WheelBase;
134 }
135
136
137
138
139
140 protected List<SensorMount> SenEffGeo = new ArrayList<SensorMount>();
141
142
143
144
145
146
147
148
149
150
151 public List<SensorMount> getSenEffGeo()
152 {
153 return SenEffGeo;
154 }
155
156 protected List<Integer> Links = new ArrayList<Integer>();
157
158
159
160
161
162
163
164
165
166
167
168
169
170 public List<Integer> getLinks()
171 {
172 return Links;
173 }
174 protected List<Integer> ParentLinks = new ArrayList<Integer>();
175
176
177
178
179
180
181
182
183
184
185
186
187
188 public List<Integer> getParentLinks()
189 {
190 return ParentLinks;
191 }
192 protected List<Location> Locations = new ArrayList<Location>();
193
194
195
196
197
198
199
200
201
202
203
204
205
206 public List<Location> getLocations()
207 {
208 return Locations;
209 }
210 protected List<Rotation> Orientations = new ArrayList<Rotation>();
211
212
213
214
215
216
217
218
219
220
221
222
223
224 public List<Rotation> getOrientations()
225 {
226 return Orientations;
227 }
228
229
230
231
232 public GeometryMessage(GeometryMessage original)
233 {
234 this.Type = original.Type;
235 this.CenterOfGravity = original.CenterOfGravity;
236 this.Dimensions = original.Dimensions;
237 this.Links.addAll(original.Links);
238 this.Locations.addAll(original.Locations);
239 this.Name = original.Name;
240 this.Orientations.addAll(original.Orientations);
241 this.ParentLinks.addAll(original.ParentLinks);
242 this.SenEffGeo.addAll(original.SenEffGeo);
243 this.WheelBase = original.WheelBase;
244 this.WheelRadius = original.WheelRadius;
245 this.WheelSeparation = original.WheelSeparation;
246 }
247
248
249
250
251
252 public GeometryMessage()
253 {
254 }
255
256
257
258 private IWorldObject orig = null;
259
260 @Override
261 public String toString()
262 {
263
264 StringBuilder buf = new StringBuilder();
265 buf.append(super.toString() + " | "
266 + "Type = "
267 + String.valueOf(Type) + " | "
268 + "Name = "
269 + String.valueOf(Name) + " | "
270 + "WheelBase = "
271 + String.valueOf(WheelBase) + " | "
272 + "WheelRadius = "
273 + String.valueOf(WheelRadius) + " | "
274 + "WheelSeparation = "
275 + String.valueOf(WheelSeparation) + " | ");
276 if(Dimensions != null)
277 {
278 buf.append("Dimensions = ").append(String.valueOf(Dimensions)).append(" | ");
279 }
280
281
282 if(CenterOfGravity != null)
283 {
284 StringBuilder append = buf.append("CenterOfGravity = ").append(String.valueOf(CenterOfGravity)).append(" | ");
285 }
286 if(!Links.isEmpty())
287 {
288 for(Integer i : Links)
289 {
290 buf.append("Link = ").append(i.toString()).append(" ");
291 }
292 buf.append(" | ");
293 }
294
295 if(!Locations.isEmpty())
296 {
297 for(Location i : Locations)
298 {
299 buf.append("Location = ").append(i.x).append(",").append(i.y).append(",").append(i.z).append(" ");
300 }
301 buf.append(" | ");
302 }
303
304 if(!ParentLinks.isEmpty())
305 {
306 for(Integer i : ParentLinks)
307 {
308 buf.append("ParentLink = ").append(i.toString()).append(" ");
309 }
310 buf.append(" | ");
311 }
312
313 if(!Orientations.isEmpty())
314 {
315 for(Rotation i : Orientations)
316 {
317 buf.append("Orientation = ").append(i.yaw).append(",").append(i.roll).append(",").append(i.pitch).append(" ");
318 }
319 buf.append(" | ");
320 }
321
322 if(!SenEffGeo.isEmpty())
323 {
324 buf.append("Sensor/Effecter Geo: ");
325 for(SensorMount i : SenEffGeo)
326 {
327 buf.append("Location = ").append(i.getLocation().x).append(",").append(i.getLocation().y).append(",").append(i.getLocation().z).append(" ");
328 buf.append("Name = ").append(i.getName()).append(" ");
329 buf.append("Mount = ").append(i.getMount()).append(" ");
330 buf.append("Orientation = ").append(i.getOrientation().yaw).append(",").append(i.getOrientation().roll).append(",").append(i.getOrientation().pitch).append(" ");
331 }
332 buf.append(" | ");
333 }
334 return buf.toString();
335
336 }
337
338
339
340
341
342
343 public String toHtmlString()
344 {
345 StringBuilder buf = new StringBuilder();
346 buf.append(super.toString()
347 + "<b>Type</b> : "
348 + String.valueOf(Type)
349 + " <br/> "
350 + "<b>Name</b> : "
351 + String.valueOf(Name)
352 + " <br/> "
353 + "<b>WheelBase</b> : "
354 + String.valueOf(WheelBase)
355 + " <br/> "
356 + "<b>WheelRadius</b> : "
357 + String.valueOf(WheelRadius)
358 + " <br/> "
359 + "<b>WheelSeparation</b> : "
360 + String.valueOf(WheelSeparation)
361 + " <br/> ");
362
363 if(Dimensions != null)
364 {
365 buf.append("<b>Dimensions</b> : ").append(String.valueOf(Dimensions)).append(" <br/> ");
366
367 }
368
369 if(CenterOfGravity != null)
370 {
371 buf.append("<b>CenterOfGravity</b> : ").append(String.valueOf(CenterOfGravity)).append(" <br/> ");
372 }
373
374 if(!Links.isEmpty())
375 {
376 for(Integer i : Links)
377 {
378 buf.append("<b>Link</b> : ").append(i.toString()).append(" <br/> ");
379 }
380 }
381
382 if(!Locations.isEmpty())
383 {
384 for(Location i : Locations)
385 {
386 buf.append("<b>Location</b> : ").append(i.x).append(",").append(i.y).append(",").append(i.z).append(" <br/> ");
387 }
388 }
389
390 if(!ParentLinks.isEmpty())
391 {
392 for(Integer i : ParentLinks)
393 {
394 buf.append("<b>ParentLink</b> : ").append(i.toString()).append(" <br/> ");
395 }
396 }
397
398 if(!Orientations.isEmpty())
399 {
400 for(Rotation i : Orientations)
401 {
402 buf.append("<b>Orientation</b> : ").append(i.yaw).append(",").append(i.roll).append(",").append(i.pitch).append(" <br/> ");
403 }
404 }
405
406 if(!SenEffGeo.isEmpty())
407 {
408 buf.append("<b>Sensor/Effecter Geo</b> : ");
409 for(SensorMount i : SenEffGeo)
410 {
411 buf.append("<b>Location</b> : ").append(i.getLocation().x).append(",").append(i.getLocation().y).append(",").append(i.getLocation().z).append(" <br/> ");
412 buf.append("<b>Name</b> : ").append(i.getName()).append(" <br/> ");
413 buf.append("<b>Mount</b> : ").append(i.getMount()).append(" <br/> ");
414 buf.append("<b>Orientation</b> : ").append(i.getOrientation().yaw).append(",").append(i.getOrientation().roll).append(",").append(i.getOrientation().pitch).append(" <br/> ");
415 }
416 }
417
418 return buf.toString();
419 }
420
421 @Override
422 public long getSimTime()
423 {
424 throw new UnsupportedOperationException("Not supported yet.");
425 }
426 }