GrxPathPlanningView.java
Go to the documentation of this file.
1 package com.generalrobotix.ui.view;
2 
3 import java.awt.Color;
4 import java.io.IOException;
5 import java.io.File;
6 import java.lang.reflect.InvocationTargetException;
7 import java.net.URL;
8 import java.util.ArrayList;
9 import java.util.List;
10 import java.util.Vector;
11 
12 import javax.media.j3d.Appearance;
13 import javax.media.j3d.BranchGroup;
14 import javax.media.j3d.GeometryArray;
15 import javax.media.j3d.Group;
16 import javax.media.j3d.LineArray;
17 import javax.media.j3d.Material;
18 import javax.media.j3d.Shape3D;
19 import javax.media.j3d.Switch;
20 import javax.media.j3d.Transform3D;
21 import javax.media.j3d.TransformGroup;
22 import javax.media.j3d.TransparencyAttributes;
23 import javax.vecmath.Color3f;
24 import javax.vecmath.Point3d;
25 import javax.vecmath.Vector3d;
26 
27 import jp.go.aist.hrp.simulator.BodyInfo;
28 import jp.go.aist.hrp.simulator.DynamicsSimulator;
29 import jp.go.aist.hrp.simulator.PathPlanner;
30 import jp.go.aist.hrp.simulator.StringSequenceHolder;
31 import jp.go.aist.hrp.simulator.WorldStateHolder;
32 import jp.go.aist.hrp.simulator.DynamicsSimulatorPackage.LinkDataType;
33 import jp.go.aist.hrp.simulator.PathPlannerPackage.PointArrayHolder;
34 import jp.go.aist.hrp.simulator.PathPlannerPackage.RoadmapHolder;
35 import jp.go.aist.hrp.simulator.PathPlannerPackage.RoadmapNode;
36 import jp.go.aist.rtm.RTC.Manager;
37 
38 import org.eclipse.core.runtime.FileLocator;
39 import org.eclipse.core.runtime.IProgressMonitor;
40 import org.eclipse.core.runtime.Path;
41 import org.eclipse.jface.dialogs.MessageDialog;
42 import org.eclipse.jface.dialogs.ProgressMonitorDialog;
43 import org.eclipse.jface.operation.IRunnableWithProgress;
44 import org.eclipse.swt.SWT;
45 import org.eclipse.swt.events.FocusEvent;
46 import org.eclipse.swt.events.FocusListener;
47 import org.eclipse.swt.events.KeyEvent;
48 import org.eclipse.swt.events.KeyListener;
49 import org.eclipse.swt.events.SelectionAdapter;
50 import org.eclipse.swt.events.SelectionEvent;
51 import org.eclipse.swt.events.SelectionListener;
52 import org.eclipse.swt.layout.GridData;
53 import org.eclipse.swt.layout.GridLayout;
54 import org.eclipse.swt.widgets.Button;
55 import org.eclipse.swt.widgets.Combo;
56 import org.eclipse.swt.widgets.Composite;
57 import org.eclipse.swt.widgets.Control;
58 import org.eclipse.swt.widgets.Display;
59 import org.eclipse.swt.widgets.Label;
60 import org.eclipse.swt.widgets.Text;
61 
62 import jp.go.aist.hrp.simulator.PathConsumerComp;
63 
64 import com.generalrobotix.ui.GrxBaseItem;
65 import com.generalrobotix.ui.GrxBasePlugin;
66 import com.generalrobotix.ui.GrxBaseView;
67 import com.generalrobotix.ui.GrxBaseViewPart;
68 import com.generalrobotix.ui.GrxPluginManager;
69 import com.generalrobotix.ui.grxui.Activator;
70 import com.generalrobotix.ui.item.GrxCollisionPairItem;
71 import com.generalrobotix.ui.item.GrxModelItem;
72 import com.generalrobotix.ui.item.GrxPathPlanningAlgorithmItem;
73 import com.generalrobotix.ui.item.GrxSimulationItem;
74 import com.generalrobotix.ui.item.GrxWorldStateItem;
75 import com.generalrobotix.ui.item.GrxWorldStateItem.WorldStateEx;
76 import com.generalrobotix.ui.util.GrxDebugUtil;
77 import com.generalrobotix.ui.util.MessageBundle;
78 import com.generalrobotix.ui.view.tdview.ObjectToolBar;
79 import com.sun.j3d.utils.geometry.Box;
80 
81 @SuppressWarnings("serial") //$NON-NLS-1$
85 public class GrxPathPlanningView extends GrxBaseView {
86  public static final String TITLE = "Path Planning"; //$NON-NLS-1$
87 
88  private Grx3DView view_ = null;
89 
90  // モデル
91  private Combo modelSelect;
92 
93  // 移動アルゴリズム
94  private Combo mobilitySelect;
95 
96  // 経路最適化アルゴリズム
97  private Combo optimizerSelect;
98  private Button optimizeButton;
99 
100  // 位置、角度の表示用
101  private static final String FORMAT = "%.3f"; //$NON-NLS-1$
102  private static final int TEXT_WIDTH = 64;
103  // Start Position
104  private Button btnSetStartPoint;
105  private Text textStartX, textStartY, textStartTheta;
106  // End Position
107  private Button btnSetEndPoint;
108  private Text textEndX, textEndY, textEndTheta;
109 
110  // 経路計画アルゴリズムとパラメータの選択
111  private Combo algoSelect;
112  private Button updatePropertyButton;
113 
114  // 計算開始、キャンセル
115  private Button btnCalcStart, /*btnCalcCancel,*/btnVisible;
116  private Label imgLabel;
117 
118  // 経路の高さ、可視性
119  private Text carpetZ;
120 
121  // 干渉チェックのトレランス
122  private Text tolerance_;
123 
124  private Button chkRebuildRoadmap_;
125 
126  // 経路表示
127  private Switch switcher_;
128  private Transform3D carpetTrans;
129  private TransformGroup carpetTransGroup;
130  private BranchGroup carpet_;
131  final double DEFAULT_CARPET_Z = 0.01;
132 
133  private GrxPathPlanningAlgorithmItem ppaItem_ = null;
134 
135 
143  public GrxPathPlanningView(String name, GrxPluginManager manager, GrxBaseViewPart vp, Composite parent) {
144  super(name, manager, vp, parent);
145 
146  GridLayout gl = new GridLayout(1,false);
147  getComposite().setLayout( gl );
148 
149  initGUI();
150  initJava3D();
151 
152  setUp();
153  manager_.registerItemChangeListener(this, GrxPathPlanningAlgorithmItem.class);
154  }
155 
156  public void setUp(){
157  removeCarpet();
158  if(ppaItem_ != null)
159  ppaItem_.deleteObserver(this);
160  ppaItem_ = manager_.<GrxPathPlanningAlgorithmItem>getSelectedItem(GrxPathPlanningAlgorithmItem.class, null);
161  if(ppaItem_!=null){
162  update();
163  setppaItem();
164  ppaItem_.addObserver(this);
165  }else
166  setEnabled(false);
167  }
168 
175  private Composite getComp(int grid, int style){
176  Composite comp = new Composite( getComposite(), style );
177  GridLayout layout = new GridLayout( grid, false );
178  comp.setLayout( layout );
179  comp.setLayoutData( new GridData(GridData.FILL_HORIZONTAL) );
180  return comp;
181  }
182 
186  private void initGUI(){
187  //使いまわす
188  Composite comp;
189  Label l;
190  GridData gd;
191 
192  //---- robot select
193 
194  comp = getComp( 5, SWT.NONE );
195 
196  l = new Label(comp, SWT.NONE);
197  l.setText(MessageBundle.get("GrxPathPlanningView.label.model")); //$NON-NLS-1$
198  modelSelect = new Combo( comp, SWT.NONE|SWT.READ_ONLY );
199  modelSelect.setLayoutData( new GridData(GridData.FILL_HORIZONTAL) );
200  modelSelect.addSelectionListener(new SelectionListener(){
201  public void widgetDefaultSelected(SelectionEvent e) {
202  }
203  public void widgetSelected(SelectionEvent e) {
204  ppaItem_.setProperty("model", modelSelect.getItem(modelSelect.getSelectionIndex())); //$NON-NLS-1$
205  }
206  });
207 
208  l = new Label(comp, SWT.NONE);
209  l.setText(MessageBundle.get("GrxPathPlanningView.label.mobility")); //$NON-NLS-1$
210  mobilitySelect = new Combo( comp, SWT.NONE|SWT.READ_ONLY );
211  mobilitySelect.setLayoutData( new GridData(GridData.FILL_HORIZONTAL) );
212  mobilitySelect.addSelectionListener(new SelectionListener(){
213  public void widgetDefaultSelected(SelectionEvent e) {
214  }
215  public void widgetSelected(SelectionEvent e) {
216  ppaItem_.setProperty("mobility", mobilitySelect.getItem(mobilitySelect.getSelectionIndex())); //$NON-NLS-1$
217  }
218  });
219 
220  //---- start and goal
221 
222  comp = getComp( 8, SWT.NONE);
223 
224  btnSetStartPoint = new Button( comp, SWT.NONE );
225  btnSetStartPoint.setText(MessageBundle.get("GrxPathPlanningView.button.start")); //$NON-NLS-1$
226  btnSetStartPoint.addSelectionListener(new SelectionAdapter(){
227  public void widgetSelected(SelectionEvent e){
228  getStartPoint();
229  }
230  });
231 
232 
233  l = new Label( comp, SWT.NONE );
234  l.setText("X:"); //$NON-NLS-1$
235  textStartX = new Text( comp, SWT.SINGLE | SWT.BORDER);
236  gd = new GridData();
237  gd.widthHint = TEXT_WIDTH;
238  textStartX.setLayoutData( gd );
239  textStartX.addKeyListener(new KeyListener(){
240  public void keyPressed(KeyEvent e) {
241  }
242  public void keyReleased(KeyEvent e) {
243  Double d = Double.parseDouble(textStartX.getText());
244  if (d != null){
245  ppaItem_.setDbl("startX", d); //$NON-NLS-1$
246  }
247  }
248  });
249 
250  l = new Label( comp, SWT.NONE );
251  l.setText("Y:"); //$NON-NLS-1$
252  textStartY = new Text( comp, SWT.SINGLE | SWT.BORDER);
253  gd = new GridData();
254  gd.widthHint = TEXT_WIDTH;
255  textStartY.setLayoutData( gd );
256  textStartY.addKeyListener(new KeyListener(){
257  public void keyPressed(KeyEvent e) {
258  }
259  public void keyReleased(KeyEvent e) {
260  Double d = Double.parseDouble(textStartY.getText());
261  if (d != null){
262  ppaItem_.setDbl("startY", d); //$NON-NLS-1$
263  }
264  }
265  });
266 
267 
268  l = new Label( comp, SWT.NONE );
269  l.setText("Yaw:"); //$NON-NLS-1$
270  textStartTheta = new Text( comp, SWT.SINGLE | SWT.BORDER);
271  gd = new GridData();
272  gd.widthHint = TEXT_WIDTH;
273  textStartTheta.setLayoutData( gd );
274  textStartTheta.addKeyListener(new KeyListener(){
275  public void keyPressed(KeyEvent e) {
276  }
277  public void keyReleased(KeyEvent e) {
278  Double d = Double.parseDouble(textStartTheta.getText());
279  if (d != null){
280  ppaItem_.setDbl("startTheta", d); //$NON-NLS-1$
281  }
282  }
283  });
284 
285 
286  Button setStart = new Button( comp, SWT.NONE );
287  setStart.setText(MessageBundle.get("GrxPathPlanningView.button.set")); //$NON-NLS-1$
288  setStart.addSelectionListener(new SelectionAdapter(){
289  public void widgetSelected(SelectionEvent e){
290  ppaItem_.setStartPoint();
291  }
292  });
293 
294 
295  btnSetEndPoint = new Button( comp, SWT.NONE );
296  btnSetEndPoint.setText(MessageBundle.get("GrxPathPlanningView.button.end")); //$NON-NLS-1$
297  btnSetEndPoint.addSelectionListener(new SelectionAdapter(){
298  public void widgetSelected(SelectionEvent e){
299  getEndPoint();
300  }
301  });
302 
303  l = new Label( comp, SWT.NONE );
304  l.setText("X:"); //$NON-NLS-1$
305  textEndX = new Text( comp, SWT.SINGLE | SWT.BORDER);
306  gd = new GridData();
307  gd.widthHint = TEXT_WIDTH;
308  textEndX.setLayoutData( gd );
309  textEndX.addKeyListener(new KeyListener(){
310  public void keyPressed(KeyEvent e) {
311  }
312  public void keyReleased(KeyEvent e) {
313  Double d = Double.parseDouble(textEndX.getText());
314  if (d != null){
315  ppaItem_.setDbl("goalX", d); //$NON-NLS-1$
316  }
317  }
318  });
319 
320 
321 
322  l = new Label( comp, SWT.NONE );
323  l.setText("Y:"); //$NON-NLS-1$
324  textEndY = new Text( comp, SWT.SINGLE | SWT.BORDER);
325  gd = new GridData();
326  gd.widthHint = TEXT_WIDTH;
327  textEndY.setLayoutData( gd );
328  textEndY.addKeyListener(new KeyListener(){
329  public void keyPressed(KeyEvent e) {
330  }
331  public void keyReleased(KeyEvent e) {
332  Double d = Double.parseDouble(textEndY.getText());
333  if (d != null){
334  ppaItem_.setDbl("goalY", d); //$NON-NLS-1$
335  }
336  }
337  });
338 
339  l = new Label( comp, SWT.NONE );
340  l.setText("Yaw:"); //$NON-NLS-1$
341  textEndTheta = new Text( comp, SWT.SINGLE | SWT.BORDER);
342  gd = new GridData();
343  gd.widthHint = TEXT_WIDTH;
344  textEndTheta.setLayoutData( gd );
345  textEndTheta.addKeyListener(new KeyListener(){
346  public void keyPressed(KeyEvent e) {
347  }
348  public void keyReleased(KeyEvent e) {
349  Double d = Double.parseDouble(textEndTheta.getText());
350  if (d != null){
351  ppaItem_.setDbl("goalTheta", d); //$NON-NLS-1$
352  }
353  }
354  });
355 
356  Button setEnd = new Button( comp, SWT.NONE );
357  setEnd.setText(MessageBundle.get("GrxPathPlanningView.button.set")); //$NON-NLS-1$
358  setEnd.addSelectionListener(new SelectionAdapter(){
359  public void widgetSelected(SelectionEvent e){
360  ppaItem_.setEndPoint();
361  }
362  });
363 
364  // ---- robot Z position
365  Composite roboZComp = getComp( 4, SWT.NONE );
366 
367  l = new Label(roboZComp, SWT.NONE);
368  l.setText(MessageBundle.get("GrxPathPlanningView.label.collision")); //$NON-NLS-1$
369 
370  tolerance_ = new Text(roboZComp, SWT.SINGLE | SWT.BORDER);
371  tolerance_.setText("0"); //$NON-NLS-1$
372  gd = new GridData();
373  gd.widthHint = TEXT_WIDTH;
374  tolerance_.setLayoutData( gd );
375  tolerance_.addFocusListener(new FocusListener(){
376  public void focusGained(FocusEvent e) {
377  }
378  public void focusLost(FocusEvent e) {
379  Double d = Double.parseDouble(tolerance_.getText());
380  if (d != null){
381  ppaItem_.setDbl("tolerance", d); //$NON-NLS-1$
382  }
383  }
384  });
385 
386  chkRebuildRoadmap_ = new Button(roboZComp,SWT.CHECK);
387  chkRebuildRoadmap_.setText(MessageBundle.get("GrxPathPlanningView.button.rebuild")); //$NON-NLS-1$
388  chkRebuildRoadmap_.addSelectionListener(new SelectionListener(){
389  public void widgetDefaultSelected(SelectionEvent e) {
390  }
391  public void widgetSelected(SelectionEvent e) {
392  if (chkRebuildRoadmap_.getSelection()){
393  ppaItem_.setProperty("rebuildRoadmap", "true"); //$NON-NLS-1$ //$NON-NLS-2$
394  }else{
395  ppaItem_.setProperty("rebuildRoadmap", "false"); //$NON-NLS-1$ //$NON-NLS-2$
396  }
397  }
398  });
399  chkRebuildRoadmap_.setSelection(true);
400 
401 
402  //---- algorithm
403 
404  Composite algoComp = getComp( 3, SWT.NONE );
405 
406  l = new Label(algoComp, SWT.NONE);
407  l.setText(MessageBundle.get("GrxPathPlanningView.label.algorithm")); //$NON-NLS-1$
408  algoSelect = new Combo( algoComp, SWT.READ_ONLY );
409  algoSelect.setLayoutData( new GridData(GridData.FILL_HORIZONTAL) );
410  algoSelect.addSelectionListener(new SelectionListener(){
411  public void widgetDefaultSelected(SelectionEvent e) {
412  }
413  public void widgetSelected(SelectionEvent e) {
414  ppaItem_.setProperty("algorithm", algoSelect.getItem(algoSelect.getSelectionIndex())); //$NON-NLS-1$
415  }
416  });
417 
418  updatePropertyButton = new Button(algoComp, SWT.NONE);
419  updatePropertyButton.setText(MessageBundle.get("GrxPathPlanningView.button.getProperties")); //$NON-NLS-1$
420  updatePropertyButton.addSelectionListener(new SelectionAdapter(){
421  public void widgetSelected(SelectionEvent e){
422  ppaItem_.propertyUpdate();
423  }
424  });
425 
426  //---- calculate
427 
428  Composite calcComp = getComp( 5, SWT.NONE );
429 
430  btnCalcStart = new Button( calcComp, SWT.NONE );
431  btnCalcStart.setText(MessageBundle.get("GrxPathPlanningView.button.calcStart")); //$NON-NLS-1$
432  btnCalcStart.addSelectionListener(new SelectionAdapter(){
433  public void widgetSelected(SelectionEvent e){
434  removeCarpet();
435  ppaItem_.startCalc();
436  displayCarpet();
437  pathGraph();
438  }
439  });
440 
441  imgLabel = new Label(calcComp, SWT.BORDER);
442  imgLabel.setImage( Activator.getDefault().getImage("grxrobot1.png" ) ); //$NON-NLS-1$
443 
444 
445  l = new Label(calcComp, SWT.NONE);
446  l.setText(MessageBundle.get("GrxPathPlanningView.label.optimizer")); //$NON-NLS-1$
447  optimizerSelect = new Combo( calcComp, SWT.NONE|SWT.READ_ONLY );
448  optimizerSelect.setLayoutData( new GridData(GridData.FILL_HORIZONTAL) );
449  optimizerSelect.addSelectionListener(new SelectionListener(){
450  public void widgetDefaultSelected(SelectionEvent e) {
451  }
452  public void widgetSelected(SelectionEvent e) {
453  ppaItem_.setProperty("optimizer", optimizerSelect.getItem(optimizerSelect.getSelectionIndex())); //$NON-NLS-1$
454  }
455  });
456 
457  optimizeButton = new Button( calcComp, SWT.NONE );
458  optimizeButton.setText(MessageBundle.get("GrxPathPlanningView.button.optimize")); //$NON-NLS-1$
459  optimizeButton.addSelectionListener(new SelectionAdapter(){
460  public void widgetSelected(SelectionEvent e){
461  ppaItem_.optimize();
462  removeCarpet();
463  displayCarpet();
464  pathGraph();
465  }
466  });
467 
468 
469  // ---- Route View
470  Composite carpetComp = getComp( 5, SWT.NONE );
471 
472  l = new Label(carpetComp, SWT.NONE);
473  l.setText(MessageBundle.get("GrxPathPlanningView.label.path")); //$NON-NLS-1$
474 
475  btnVisible = new Button( carpetComp, SWT.TOGGLE );
476  btnVisible.setText(MessageBundle.get("GrxPathPlanningView.button.visible")); //$NON-NLS-1$
477  btnVisible.setSelection(true);
478  btnVisible.addSelectionListener(new SelectionAdapter(){
479  public void widgetSelected(SelectionEvent e){
480  boolean selection = btnVisible.getSelection();
481  if (selection) {
482  System.out.println("TRUE"); //$NON-NLS-1$
483  switcher_.setWhichChild(0);
484  } else {
485  System.out.println("FALSE"); //$NON-NLS-1$
486  switcher_.setWhichChild(Switch.CHILD_NONE);
487  }
488 
489  }
490  });
491 
492  l = new Label(carpetComp, SWT.NONE);
493  l.setText(MessageBundle.get("GrxPathPlanningView.label.carpetZ")); //$NON-NLS-1$
494 
495  carpetZ = new Text(carpetComp, SWT.SINGLE | SWT.BORDER );
496  carpetZ.setLayoutData( new GridData(GridData.FILL_HORIZONTAL) );
497  carpetZ.setText( String.valueOf(DEFAULT_CARPET_Z) );
498  carpetZ.addKeyListener(new KeyListener(){
499  public void keyPressed(KeyEvent e) {}
500  public void keyReleased(KeyEvent e) {
501  if (e.character == SWT.CR) {
502  updateCarpet();
503  }
504  }
505  });
506  carpetZ.addFocusListener(new FocusListener(){
507  public void focusGained(FocusEvent e) {
508  }
509  public void focusLost(FocusEvent e) {
510  Double d = Double.parseDouble(carpetZ.getText());
511  if (d != null){
512  ppaItem_.setDbl("carpetZ", d); //$NON-NLS-1$
513  }
514  }
515  });
516 
517  setScrollMinSize(SWT.DEFAULT,SWT.DEFAULT);
518  }
519 
520 
521  private void initJava3D(){
522  carpet_ = new BranchGroup();
523  carpet_.setCapability(BranchGroup.ALLOW_CHILDREN_READ);
524  carpet_.setCapability(BranchGroup.ALLOW_CHILDREN_WRITE);
525  carpet_.setCapability(BranchGroup.ALLOW_CHILDREN_EXTEND);
526  carpet_.setCapability(BranchGroup.ALLOW_DETACH); //親(carpetTransGroup)から離れられるようにする
527 
528  carpetTrans = new Transform3D();
529  carpetTrans.setTranslation( new Vector3d(0,0, getCarpetZPosition()) );
530 
531  carpetTransGroup = new TransformGroup();
532  carpetTransGroup.setCapability( TransformGroup.ALLOW_TRANSFORM_READ );
533  carpetTransGroup.setCapability( TransformGroup.ALLOW_TRANSFORM_WRITE );
534  carpetTransGroup.setCapability( Group.ALLOW_CHILDREN_WRITE);
535  carpetTransGroup.setCapability( Group.ALLOW_CHILDREN_EXTEND);
536  carpetTransGroup.setTransform( carpetTrans );
537  carpetTransGroup.addChild( carpet_ );
538 
539  switcher_ = new Switch();
540  switcher_.setCapability(Switch.ALLOW_SWITCH_READ);
541  switcher_.setCapability(Switch.ALLOW_SWITCH_WRITE);
542  switcher_.addChild( carpetTransGroup );
543  switcher_.setWhichChild(0);
544  }
545 
546 
550  private void update(){
551  String[] algoNames = ppaItem_.getAlgorithms();
552  algoSelect.removeAll();
553  for( String s: algoNames )
554  algoSelect.add( s );
555 
556  // mobilities
557  String[] mobilityNames = ppaItem_.getMobilityNames();
558  mobilitySelect.removeAll();
559  for( String s: mobilityNames )
560  mobilitySelect.add( s );
561 
562  // optimizers
563  String[] optimizerNames = ppaItem_.getOptimizerNames();
564  optimizerSelect.removeAll();
565  for( String s: optimizerNames )
566  optimizerSelect.add( s );
567 
568  if (ppaItem_ != null){
569  String algorithmName = ppaItem_.getStr("algorithm", ""); //$NON-NLS-1$ //$NON-NLS-2$
570  if (algoSelect.indexOf(algorithmName) >= 0){
571  algoSelect.select(algoSelect.indexOf(algorithmName));
572  }
573  String mobilityName = ppaItem_.getStr("mobility", ""); //$NON-NLS-1$ //$NON-NLS-2$
574  if (mobilitySelect.indexOf(mobilityName) >= 0){
575  mobilitySelect.select(mobilitySelect.indexOf(mobilityName));
576  }
577  String optimizerName = ppaItem_.getStr("optimizer", ""); //$NON-NLS-1$ //$NON-NLS-2$
578  if (optimizerSelect.indexOf(optimizerName) >= 0){
579  optimizerSelect.select(optimizerSelect.indexOf(optimizerName));
580  }
581  }
582  }
583 
587  private void removeCarpet() {
588  try{
589  carpetTransGroup.removeChild( carpet_ );
590  carpet_.removeAllChildren();
591  carpetTransGroup.addChild( carpet_ );
592  }catch( Exception e ){
593  e.printStackTrace();
594  }
595  }
596 
597  private String getRobotBaseLink( GrxModelItem model ) {
598  return model.rootLink().getName();
599  }
600 
601  private void getStartPoint(){
602  String robotName = modelSelect.getText();
603  GrxModelItem robot = (GrxModelItem)(manager_.getItem( GrxModelItem.class, robotName ));
604  if( robot == null )
605  return;
606  double [] tr = robot.getDblAry(getRobotBaseLink(robot)+".translation",null); //$NON-NLS-1$
607  double [] rot = robot.getDblAry(getRobotBaseLink(robot)+".rotation",null); //$NON-NLS-1$
608  try{
609  textStartX.setText( String.format( FORMAT, tr[0]) );
610  textStartY.setText( String.format( FORMAT, tr[1]) );
611  textStartTheta.setText( String.format( FORMAT, rot[3] / (2f*Math.PI) * 360f) );
612  if (ppaItem_ != null){
613  ppaItem_.setDbl("startX", tr[0]); //$NON-NLS-1$
614  ppaItem_.setDbl("startY", tr[1]); //$NON-NLS-1$
615  ppaItem_.setDbl("startTheta", rot[3]); //$NON-NLS-1$
616  }
617  }catch(Exception e){
618  return;
619  }
620  }
621 
622  private void getEndPoint(){
623  String robotName = modelSelect.getText();
624  GrxModelItem robot = (GrxModelItem)(manager_.getItem( GrxModelItem.class, robotName ));
625  if( robot == null )
626  return;
627  double [] tr = robot.getDblAry(getRobotBaseLink(robot)+".translation",null); //$NON-NLS-1$
628  double [] rot = robot.getDblAry(getRobotBaseLink(robot)+".rotation",null); //$NON-NLS-1$
629  try{
630  textEndX.setText( String.format( FORMAT, tr[0]) );
631  textEndY.setText( String.format( FORMAT, tr[1]) );
632  textEndTheta.setText( String.format( FORMAT, rot[3] / (2f*Math.PI) * 360f) );
633  if (ppaItem_ != null){
634  ppaItem_.setDbl("goalX", tr[0]); //$NON-NLS-1$
635  ppaItem_.setDbl("goalY", tr[1]); //$NON-NLS-1$
636  ppaItem_.setDbl("goalTheta", rot[3]); //$NON-NLS-1$
637  }
638  }catch(Exception e){
639  return;
640  }
641  }
642 
643  Grx3DView get3DView()
644  {
645  if( view_ == null )
646  view_ = (Grx3DView)manager_.getView(Grx3DView.class, false);
647  return view_;
648  }
649 
650  private void displayCarpet(){
651  double[][] path = ppaItem_.getPath();
652  for( int i=0; i+1<path.length; i++ ) {
653  double []p1 = path[i], p2 = path[i+1];
654  carpet( p1, p2 );
655  }
656  }
657 
663  private void carpet( double[] start, double[] goal ){
664  double carpetHeight=0.01;
665  double dx = goal[0] - start[0], dy = goal[1] - start[1], len = Math.sqrt( dx/2*dx/2+dy/2*dy/2 );
666  double theta;
667  if (len < 0.001){
668  len = 0.01;
669  theta = goal[2];
670  }else{
671  theta = Math.atan2( dy,dx );
672  }
673  //Point3d destPoint = new Point3d( x2+dx/2, y2+dy/2, getZPosition()+carpetHeight );
674  Point3d centerPoint = new Point3d( (start[0]+goal[0])/2, (start[1]+goal[1])/2, 0 );
675 
676  // APPEARANCE
677  Appearance appea = new Appearance( );
678  Material material = new Material( );
679  Color3f color = new Color3f( 0, 1f, 0 );
680  material.setDiffuseColor( color );
681  appea.setMaterial( material );
682  appea.setTransparencyAttributes(new TransparencyAttributes( TransparencyAttributes.NICEST,0.7f ) );
683  // carpet block
684  Box box = new Box( (float)len, 0.1f, (float) carpetHeight, appea );
685  // 移動回転
686  Transform3D tr = new Transform3D();
687  tr.setTranslation( new Vector3d( centerPoint ) );
688  Transform3D rot = new Transform3D();
689  rot.rotZ( theta );
690  tr.mul(rot);
691  // ノードの追加
692  BranchGroup bg = new BranchGroup();
693  TransformGroup newtg = new TransformGroup( tr );
694  bg.addChild( newtg );
695  newtg.addChild( box );
696  carpet_.addChild( bg );
697  }
698 
699 
700 
704  private void pathGraph(){
705  if ( view_ == null ){
706  view_ = get3DView();
707  if( view_ != null ){
708  BranchGroup bg = new BranchGroup();
709  bg.addChild( switcher_ );
710  view_.attachUnclickable( bg );
711  }else{
712  GrxDebugUtil.println("[GrxPathPlanningView] can't find 3DView"); //$NON-NLS-1$
713  }
714  }
715 
716  RoadmapNode[] tree = ppaItem_.getRoadmap();
717 
718  Vector<Point3d> vertex = new Vector<Point3d>();
719 
720  // LineArrayで使うためにvectorへ放り込む
721  for( RoadmapNode n : tree ){
722  Point3d start = new Point3d( n.cfg[0], n.cfg[1], 0 ); //三つめはtheta
723  for( int l : n.neighbors ){
724  // 一方向リンクなのでダブりは考えなくてよし
725  Point3d to = new Point3d(tree[l].cfg[0],tree[l].cfg[1],0);
726  vertex.add( start );
727  vertex.add( to );
728  //GrxDebugUtil.println("[PPV]@pathGraph start"+start+" to "+to );
729  }
730  }
731 
732  GrxDebugUtil.println("[PPV]@pathGraph size="+vertex.size()); //$NON-NLS-1$
733  if (vertex.size() == 0) return;
734  LineArray geometry = new LineArray(vertex.size(), GeometryArray.COORDINATES | GeometryArray.COLOR_3);
735  geometry.setCoordinates(0, vertex.toArray(new Point3d[vertex.size()]) );
736  for( int i=0; i<vertex.size(); i++)
737  geometry.setColor(i, new Color3f(Color.red));
738 
739  // ノードの追加
740  Shape3D graph = new Shape3D(geometry);
741  graph.setPickable(false);
742  // 移動回転
743  Transform3D tr = new Transform3D();
744  tr.setTranslation( new Vector3d( 0,0,0 ) );
745  BranchGroup bg = new BranchGroup();
746  TransformGroup newtg = new TransformGroup( tr );
747  bg.addChild( newtg );
748  newtg.addChild( graph );
749 
750  carpet_.addChild( bg );
751 
752  return;
753  }
754 
755 
756  public void registerItemChange(GrxBaseItem item, int event){
757  if(item instanceof GrxPathPlanningAlgorithmItem){
758  GrxPathPlanningAlgorithmItem ppa = (GrxPathPlanningAlgorithmItem) item;
759  switch(event){
760  case GrxPluginManager.SELECTED_ITEM:
761  if(ppaItem_!= ppa){
762  ppaItem_ = ppa;
763  ppaItem_.addObserver(this);
764  update();
765  setppaItem();
766  }
767  break;
768  case GrxPluginManager.REMOVE_ITEM:
769  removeCarpet();
770  case GrxPluginManager.NOTSELECTED_ITEM:
771  if(ppaItem_==ppa){
772  ppaItem_.deleteObserver(this);
773  ppaItem_ = null;
774  setEnabled(false);
775  }
776  break;
777  default:
778  break;
779  }
780  }
781  }
782 
783  private void setppaItem(){
784  setEnabled(true);
785  String modelName = null;
786  if (ppaItem_ != null){
787  modelName = ppaItem_.getStr("model", null); //$NON-NLS-1$
788  }else{
789  modelName = modelSelect.getText();
790  }
791 
792  Object[] items = manager_.getSelectedItemList( GrxModelItem.class ).toArray();
793  modelSelect.removeAll();
794  for( Object o: items ){
795  GrxModelItem model = (GrxModelItem)o;
796  if (model.isRobot()){
797  modelSelect.add( o.toString() );
798  }
799  }
800  if (modelName != null && modelSelect.indexOf(modelName) >= 0){
801  modelSelect.select(modelSelect.indexOf(modelName));
802  }
803 
804  if (ppaItem_ != null){
805  String mobilityName = ppaItem_.getStr("mobility", ""); //$NON-NLS-1$ //$NON-NLS-2$
806  if (mobilitySelect.indexOf(mobilityName) >= 0){
807  mobilitySelect.select(mobilitySelect.indexOf(mobilityName));
808  }else{
809  mobilitySelect.deselectAll();
810  }
811  String optimizerName = ppaItem_.getStr("optimizer", ""); //$NON-NLS-1$ //$NON-NLS-2$
812  if (optimizerSelect.indexOf(optimizerName) >= 0){
813  optimizerSelect.select(optimizerSelect.indexOf(optimizerName));
814  }else{
815  optimizerSelect.deselectAll();
816  }
817  String algorithmName = ppaItem_.getStr("algorithm", ""); //$NON-NLS-1$ //$NON-NLS-2$
818  if (algoSelect.indexOf(algorithmName) >= 0){
819  algoSelect.select(algoSelect.indexOf(algorithmName));
820  }else{
821  algoSelect.deselectAll();
822  }
823  Double sx = ppaItem_.getDbl("startX", null); //$NON-NLS-1$
824  if (sx != null){
825  textStartX.setText(sx.toString());
826  }else{
827  textStartX.setText(""); //$NON-NLS-1$
828  }
829  Double sy = ppaItem_.getDbl("startY", null); //$NON-NLS-1$
830  if (sy != null){
831  textStartY.setText(sy.toString());
832  }else{
833  textStartY.setText(""); //$NON-NLS-1$
834  }
835  Double st = ppaItem_.getDbl("startTheta", null); //$NON-NLS-1$
836  if (st != null){
837  textStartTheta.setText(st.toString());
838  }else{
839  textStartTheta.setText(""); //$NON-NLS-1$
840  }
841  Double ex = ppaItem_.getDbl("goalX", null); //$NON-NLS-1$
842  if (ex != null){
843  textEndX.setText(ex.toString());
844  }else{
845  textEndX.setText(""); //$NON-NLS-1$
846  }
847  Double ey = ppaItem_.getDbl("goalY", null); //$NON-NLS-1$
848  if (ey != null){
849  textEndY.setText(ey.toString());
850  }else{
851  textEndY.setText(""); //$NON-NLS-1$
852  }
853  Double et = ppaItem_.getDbl("goalTheta", null); //$NON-NLS-1$
854  if (et != null){
855  textEndTheta.setText(et.toString());
856  }else{
857  textEndTheta.setText(""); //$NON-NLS-1$
858  }
859  chkRebuildRoadmap_.setSelection(ppaItem_.isTrue("rebuildRoadmap", true)); //$NON-NLS-1$
860  tolerance_.setText(ppaItem_.getDbl("tolerance",0.0).toString()); //$NON-NLS-1$
861  carpetZ.setText(ppaItem_.getDbl("carpetZ",0.01).toString()); //$NON-NLS-1$
862  }
863 
864  }
865 
870  double getCarpetZPosition(){
871  double z;
872  try{
873  z= Double.valueOf( carpetZ.getText() );
874  }catch(Exception e){
875  z=0.2;
876  e.printStackTrace();
877  }
878  return z;
879  }
880 
885  void updateCarpet(){
886  carpetTrans.setTranslation( new Vector3d(0,0, getCarpetZPosition() ) );
887  carpetTransGroup.setTransform( carpetTrans );
888  }
889 
894  public void setEnabled(boolean b){
895  Control[] ctl = getComposite().getChildren();
896  for (int i=0; i<ctl.length; i++){
897  ctl[i].setEnabled(b);
898  if (ctl[i] instanceof Composite){
899  Composite cmp = (Composite)ctl[i];
900  Control[] ctl2 = cmp.getChildren();
901  for (int j=0; j<ctl2.length; j++){
902  ctl2[j].setEnabled(b);
903  }
904  }
905  }
906  }
907 
908  public void update(GrxBasePlugin plugin, Object... arg) {
909  if(ppaItem_==plugin){
910  if(((String)arg[0]).equals("connected")){ //$NON-NLS-1$
911  update();
912  }
913  }
914  }
915 
916  public void shutdown() {
917  removeCarpet();
918  manager_.removeItemChangeListener(this, GrxPathPlanningAlgorithmItem.class);
919  if(ppaItem_!=null)
920  ppaItem_.deleteObserver(this);
921  }
922 }
com.generalrobotix.ui.util.MessageBundle.get
static final String get(String key)
Definition: MessageBundle.java:50
com.generalrobotix.ui.util.GrxConfigBundle.isTrue
final boolean isTrue(String key)
check whether value associated to key includes a word "true"
Definition: GrxConfigBundle.java:436
com.generalrobotix.ui.util.GrxConfigBundle.getDblAry
final double [] getDblAry(String key, double[] defaultVal)
get double array associated to key
Definition: GrxConfigBundle.java:268
null
#define null
our own NULL pointer
Definition: IceTypes.h:57
name
png_infop png_charpp name
Definition: png.h:2382
com.generalrobotix.ui.item.GrxModelItem
item corresponds to a robot model
Definition: GrxModelItem.java:52
FORMAT
FORMAT
i
png_uint_32 i
Definition: png.h:2735
b
long b
Definition: jpegint.h:371
com.generalrobotix.ui.util.GrxConfigBundle.setDbl
final void setDbl(String key, double value)
associate double value to key
Definition: GrxConfigBundle.java:393
com.generalrobotix.ui.item.GrxModelItem.rootLink
GrxLinkItem rootLink()
get root link
Definition: GrxModelItem.java:375
com.generalrobotix.ui.view.GrxPathPlanningView.update
void update(GrxBasePlugin plugin, Object... arg)
Definition: GrxPathPlanningView.java:908
com.generalrobotix.ui.GrxBasePlugin.setProperty
Object setProperty(String key, String value)
set property value associated with a keyword
Definition: GrxBasePlugin.java:477
path
path
start
png_size_t start
Definition: png.h:1496
com.generalrobotix.ui.view.GrxPathPlanningView.registerItemChange
void registerItemChange(GrxBaseItem item, int event)
Definition: GrxPathPlanningView.java:756
com.generalrobotix.ui.util.GrxConfigBundle.getDbl
final Double getDbl(String key, Double defaultVal)
get double value associated to key
Definition: GrxConfigBundle.java:212
com.generalrobotix.ui.util.GrxConfigBundle.getStr
final String getStr(String key)
get value associated to keyword
Definition: GrxConfigBundle.java:78
com.generalrobotix.ui.GrxPluginManager
プラグイン管理クラス GrxUIの核になるクラス。プラグインのロード等の、初期化を実行する。 プラグインとそ...
Definition: GrxPluginManager.java:79
com.generalrobotix.ui.item.GrxPathPlanningAlgorithmItem.propertyUpdate
void propertyUpdate()
移動動作設計コンポーネントから現在選択している経路計画アルゴリズムに対するプロパティを取得し、選択し...
Definition: GrxPathPlanningAlgorithmItem.java:176
com.generalrobotix.ui.view.Grx3DView.attachUnclickable
void attachUnclickable(BranchGroup bg)
Definition: Grx3DView.java:1830
com.generalrobotix.ui.view.GrxPathPlanningView.carpet
void carpet(double[] start, double[] goal)
Definition: GrxPathPlanningView.java:663
com.generalrobotix.ui.view.GrxPathPlanningView.update
void update()
移動動作設計コンポーネントから経路計画アルゴリズム名、移動アルゴリズム名、経路最適化アルゴリズム名を...
Definition: GrxPathPlanningView.java:550
com.generalrobotix.ui.view.GrxPathPlanningView.GrxPathPlanningView
GrxPathPlanningView(String name, GrxPluginManager manager, GrxBaseViewPart vp, Composite parent)
constructor
Definition: GrxPathPlanningView.java:143
org
org
arg
char * arg
Definition: cdjpeg.h:136
box
Definition: jquant2.c:258

openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Sep 8 2022 02:24:03