GrxPathPlanningView.java
Go to the documentation of this file.
00001 package com.generalrobotix.ui.view;
00002 
00003 import java.awt.Color;
00004 import java.io.IOException;
00005 import java.io.File;
00006 import java.lang.reflect.InvocationTargetException;
00007 import java.net.URL;
00008 import java.util.ArrayList;
00009 import java.util.List;
00010 import java.util.Vector;
00011 
00012 import javax.media.j3d.Appearance;
00013 import javax.media.j3d.BranchGroup;
00014 import javax.media.j3d.GeometryArray;
00015 import javax.media.j3d.Group;
00016 import javax.media.j3d.LineArray;
00017 import javax.media.j3d.Material;
00018 import javax.media.j3d.Shape3D;
00019 import javax.media.j3d.Switch;
00020 import javax.media.j3d.Transform3D;
00021 import javax.media.j3d.TransformGroup;
00022 import javax.media.j3d.TransparencyAttributes;
00023 import javax.vecmath.Color3f;
00024 import javax.vecmath.Point3d;
00025 import javax.vecmath.Vector3d;
00026 
00027 import jp.go.aist.hrp.simulator.BodyInfo;
00028 import jp.go.aist.hrp.simulator.DynamicsSimulator;
00029 import jp.go.aist.hrp.simulator.PathPlanner;
00030 import jp.go.aist.hrp.simulator.StringSequenceHolder;
00031 import jp.go.aist.hrp.simulator.WorldStateHolder;
00032 import jp.go.aist.hrp.simulator.DynamicsSimulatorPackage.LinkDataType;
00033 import jp.go.aist.hrp.simulator.PathPlannerPackage.PointArrayHolder;
00034 import jp.go.aist.hrp.simulator.PathPlannerPackage.RoadmapHolder;
00035 import jp.go.aist.hrp.simulator.PathPlannerPackage.RoadmapNode;
00036 import jp.go.aist.rtm.RTC.Manager;
00037 
00038 import org.eclipse.core.runtime.FileLocator;
00039 import org.eclipse.core.runtime.IProgressMonitor;
00040 import org.eclipse.core.runtime.Path;
00041 import org.eclipse.jface.dialogs.MessageDialog;
00042 import org.eclipse.jface.dialogs.ProgressMonitorDialog;
00043 import org.eclipse.jface.operation.IRunnableWithProgress;
00044 import org.eclipse.swt.SWT;
00045 import org.eclipse.swt.events.FocusEvent;
00046 import org.eclipse.swt.events.FocusListener;
00047 import org.eclipse.swt.events.KeyEvent;
00048 import org.eclipse.swt.events.KeyListener;
00049 import org.eclipse.swt.events.SelectionAdapter;
00050 import org.eclipse.swt.events.SelectionEvent;
00051 import org.eclipse.swt.events.SelectionListener;
00052 import org.eclipse.swt.layout.GridData;
00053 import org.eclipse.swt.layout.GridLayout;
00054 import org.eclipse.swt.widgets.Button;
00055 import org.eclipse.swt.widgets.Combo;
00056 import org.eclipse.swt.widgets.Composite;
00057 import org.eclipse.swt.widgets.Control;
00058 import org.eclipse.swt.widgets.Display;
00059 import org.eclipse.swt.widgets.Label;
00060 import org.eclipse.swt.widgets.Text;
00061 
00062 import jp.go.aist.hrp.simulator.PathConsumerComp;
00063 
00064 import com.generalrobotix.ui.GrxBaseItem;
00065 import com.generalrobotix.ui.GrxBasePlugin;
00066 import com.generalrobotix.ui.GrxBaseView;
00067 import com.generalrobotix.ui.GrxBaseViewPart;
00068 import com.generalrobotix.ui.GrxPluginManager;
00069 import com.generalrobotix.ui.grxui.Activator;
00070 import com.generalrobotix.ui.item.GrxCollisionPairItem;
00071 import com.generalrobotix.ui.item.GrxModelItem;
00072 import com.generalrobotix.ui.item.GrxPathPlanningAlgorithmItem;
00073 import com.generalrobotix.ui.item.GrxSimulationItem;
00074 import com.generalrobotix.ui.item.GrxWorldStateItem;
00075 import com.generalrobotix.ui.item.GrxWorldStateItem.WorldStateEx;
00076 import com.generalrobotix.ui.util.GrxDebugUtil;
00077 import com.generalrobotix.ui.util.MessageBundle;
00078 import com.generalrobotix.ui.view.tdview.ObjectToolBar;
00079 import com.sun.j3d.utils.geometry.Box;
00080 
00081 @SuppressWarnings("serial") //$NON-NLS-1$
00085 public class GrxPathPlanningView extends GrxBaseView {
00086     public static final String TITLE = "Path Planning"; //$NON-NLS-1$
00087 
00088         private Grx3DView view_ = null;
00089 
00090     // モデル
00091         private Combo modelSelect;
00092         
00093         // 移動アルゴリズム
00094         private Combo mobilitySelect;
00095         
00096         // 経路最適化アルゴリズム
00097         private Combo optimizerSelect;
00098         private Button optimizeButton;
00099 
00100         // 位置、角度の表示用
00101         private static final String FORMAT = "%.3f"; //$NON-NLS-1$
00102         private static final int TEXT_WIDTH = 64;
00103         // Start Position
00104         private Button btnSetStartPoint; 
00105         private Text textStartX, textStartY, textStartTheta;
00106         // End Position
00107         private Button btnSetEndPoint; 
00108         private Text textEndX, textEndY, textEndTheta;
00109 
00110         // 経路計画アルゴリズムとパラメータの選択
00111         private Combo algoSelect;
00112         private Button updatePropertyButton;
00113         
00114         // 計算開始、キャンセル
00115         private Button btnCalcStart, /*btnCalcCancel,*/btnVisible;
00116         private Label imgLabel;
00117         
00118         // 経路の高さ、可視性
00119         private Text carpetZ;
00120         
00121         // 干渉チェックのトレランス
00122         private Text tolerance_;
00123         
00124         private Button chkRebuildRoadmap_;
00125         
00126         // 経路表示
00127         private Switch switcher_;
00128         private Transform3D carpetTrans;
00129         private TransformGroup carpetTransGroup; 
00130         private BranchGroup carpet_;
00131         final double DEFAULT_CARPET_Z = 0.01;
00132         
00133         private GrxPathPlanningAlgorithmItem ppaItem_ = null;
00134 
00135         
00143         public GrxPathPlanningView(String name, GrxPluginManager manager, GrxBaseViewPart vp, Composite parent) {
00144                 super(name, manager, vp, parent);
00145 
00146                 GridLayout gl = new GridLayout(1,false);
00147                 getComposite().setLayout( gl );
00148 
00149             initGUI();
00150                 initJava3D();
00151                 
00152                 setUp();
00153                 manager_.registerItemChangeListener(this, GrxPathPlanningAlgorithmItem.class);
00154         }
00155 
00156         public void setUp(){
00157                 removeCarpet();
00158                 if(ppaItem_ != null)
00159                         ppaItem_.deleteObserver(this);
00160                 ppaItem_ = manager_.<GrxPathPlanningAlgorithmItem>getSelectedItem(GrxPathPlanningAlgorithmItem.class, null);
00161                 if(ppaItem_!=null){
00162                         update();
00163                         setppaItem();
00164                         ppaItem_.addObserver(this);
00165                 }else
00166                         setEnabled(false);
00167         }
00168         
00175         private Composite getComp(int grid, int style){
00176                 Composite comp = new Composite( getComposite(), style );
00177                 GridLayout layout = new GridLayout( grid, false );
00178                 comp.setLayout( layout );
00179                 comp.setLayoutData( new GridData(GridData.FILL_HORIZONTAL) );
00180                 return comp;
00181         }
00182         
00186         private void initGUI(){
00187             //使いまわす
00188             Composite comp;
00189                 Label l; 
00190                 GridData gd;
00191 
00192                 //---- robot select
00193         
00194                 comp = getComp( 5, SWT.NONE );
00195 
00196                 l = new Label(comp, SWT.NONE);
00197                 l.setText(MessageBundle.get("GrxPathPlanningView.label.model")); //$NON-NLS-1$
00198                 modelSelect = new Combo( comp, SWT.NONE|SWT.READ_ONLY );
00199                 modelSelect.setLayoutData( new GridData(GridData.FILL_HORIZONTAL) );
00200                 modelSelect.addSelectionListener(new SelectionListener(){
00201                         public void widgetDefaultSelected(SelectionEvent e) {
00202                         }
00203                         public void widgetSelected(SelectionEvent e) {
00204                                 ppaItem_.setProperty("model", modelSelect.getItem(modelSelect.getSelectionIndex())); //$NON-NLS-1$
00205                         }
00206                 });
00207 
00208                 l = new Label(comp, SWT.NONE);
00209                 l.setText(MessageBundle.get("GrxPathPlanningView.label.mobility")); //$NON-NLS-1$
00210                 mobilitySelect = new Combo( comp, SWT.NONE|SWT.READ_ONLY );
00211                 mobilitySelect.setLayoutData( new GridData(GridData.FILL_HORIZONTAL) );
00212                 mobilitySelect.addSelectionListener(new SelectionListener(){
00213                         public void widgetDefaultSelected(SelectionEvent e) {
00214                         }
00215                         public void widgetSelected(SelectionEvent e) {
00216                                 ppaItem_.setProperty("mobility", mobilitySelect.getItem(mobilitySelect.getSelectionIndex())); //$NON-NLS-1$
00217                         }
00218                 });
00219 
00220                 //---- start and goal
00221                 
00222                 comp = getComp( 8, SWT.NONE);
00223 
00224                 btnSetStartPoint = new Button( comp, SWT.NONE );
00225                 btnSetStartPoint.setText(MessageBundle.get("GrxPathPlanningView.button.start")); //$NON-NLS-1$
00226                 btnSetStartPoint.addSelectionListener(new SelectionAdapter(){
00227             public void widgetSelected(SelectionEvent e){
00228                 getStartPoint();
00229                         }
00230         });
00231 
00232 
00233                 l = new Label( comp, SWT.NONE );
00234                 l.setText("X:"); //$NON-NLS-1$
00235                 textStartX = new Text( comp, SWT.SINGLE | SWT.BORDER);
00236                 gd = new GridData();
00237                 gd.widthHint = TEXT_WIDTH;
00238                 textStartX.setLayoutData( gd );
00239                 textStartX.addKeyListener(new KeyListener(){
00240                         public void keyPressed(KeyEvent e) {
00241                         }
00242                         public void keyReleased(KeyEvent e) {
00243                                 Double d = Double.parseDouble(textStartX.getText());
00244                                 if (d != null){
00245                                         ppaItem_.setDbl("startX", d); //$NON-NLS-1$
00246                                 }
00247                         }
00248                 });
00249 
00250                 l = new Label( comp, SWT.NONE );
00251                 l.setText("Y:"); //$NON-NLS-1$
00252                 textStartY = new Text( comp, SWT.SINGLE | SWT.BORDER);
00253                 gd = new GridData();
00254                 gd.widthHint = TEXT_WIDTH;
00255                 textStartY.setLayoutData( gd );
00256                 textStartY.addKeyListener(new KeyListener(){
00257                         public void keyPressed(KeyEvent e) {
00258                         }
00259                         public void keyReleased(KeyEvent e) {
00260                                 Double d = Double.parseDouble(textStartY.getText());
00261                                 if (d != null){
00262                                         ppaItem_.setDbl("startY", d); //$NON-NLS-1$
00263                                 }
00264                         }
00265                 });
00266 
00267 
00268                 l = new Label( comp, SWT.NONE );
00269                 l.setText("Yaw:"); //$NON-NLS-1$
00270                 textStartTheta = new Text( comp, SWT.SINGLE | SWT.BORDER);
00271                 gd = new GridData();
00272                 gd.widthHint = TEXT_WIDTH;
00273                 textStartTheta.setLayoutData( gd );
00274                 textStartTheta.addKeyListener(new KeyListener(){
00275                         public void keyPressed(KeyEvent e) {
00276                         }
00277                         public void keyReleased(KeyEvent e) {
00278                                 Double d = Double.parseDouble(textStartTheta.getText());
00279                                 if (d != null){
00280                                         ppaItem_.setDbl("startTheta", d); //$NON-NLS-1$
00281                                 }
00282                         }
00283                 });
00284 
00285 
00286                 Button setStart = new Button( comp, SWT.NONE );
00287                 setStart.setText(MessageBundle.get("GrxPathPlanningView.button.set")); //$NON-NLS-1$
00288                 setStart.addSelectionListener(new SelectionAdapter(){
00289             public void widgetSelected(SelectionEvent e){
00290                 ppaItem_.setStartPoint();
00291                         }
00292         });
00293 
00294                 
00295                 btnSetEndPoint = new Button( comp, SWT.NONE );
00296                 btnSetEndPoint.setText(MessageBundle.get("GrxPathPlanningView.button.end")); //$NON-NLS-1$
00297                 btnSetEndPoint.addSelectionListener(new SelectionAdapter(){
00298             public void widgetSelected(SelectionEvent e){
00299                 getEndPoint();
00300                         }
00301         });
00302 
00303                 l = new Label( comp, SWT.NONE );
00304                 l.setText("X:"); //$NON-NLS-1$
00305                 textEndX = new Text( comp, SWT.SINGLE | SWT.BORDER);
00306                 gd = new GridData();
00307                 gd.widthHint = TEXT_WIDTH;
00308                 textEndX.setLayoutData( gd );
00309                 textEndX.addKeyListener(new KeyListener(){
00310                         public void keyPressed(KeyEvent e) {
00311                         }
00312                         public void keyReleased(KeyEvent e) {
00313                                 Double d = Double.parseDouble(textEndX.getText());
00314                                 if (d != null){
00315                                         ppaItem_.setDbl("goalX", d); //$NON-NLS-1$
00316                                 }
00317                         }
00318                 });
00319 
00320 
00321 
00322                 l = new Label( comp, SWT.NONE );
00323                 l.setText("Y:"); //$NON-NLS-1$
00324                 textEndY = new Text( comp, SWT.SINGLE | SWT.BORDER);
00325                 gd = new GridData();
00326                 gd.widthHint = TEXT_WIDTH;
00327                 textEndY.setLayoutData( gd );
00328                 textEndY.addKeyListener(new KeyListener(){
00329                         public void keyPressed(KeyEvent e) {
00330                         }
00331                         public void keyReleased(KeyEvent e) {
00332                                 Double d = Double.parseDouble(textEndY.getText());
00333                                 if (d != null){
00334                                         ppaItem_.setDbl("goalY", d); //$NON-NLS-1$
00335                                 }
00336                         }
00337                 });
00338 
00339                 l = new Label( comp, SWT.NONE );
00340                 l.setText("Yaw:"); //$NON-NLS-1$
00341                 textEndTheta = new Text( comp, SWT.SINGLE | SWT.BORDER);
00342                 gd = new GridData();
00343                 gd.widthHint = TEXT_WIDTH;
00344                 textEndTheta.setLayoutData( gd );
00345                 textEndTheta.addKeyListener(new KeyListener(){
00346                         public void keyPressed(KeyEvent e) {
00347                         }
00348                         public void keyReleased(KeyEvent e) {
00349                                 Double d = Double.parseDouble(textEndTheta.getText());
00350                                 if (d != null){
00351                                         ppaItem_.setDbl("goalTheta", d); //$NON-NLS-1$
00352                                 }
00353                         }
00354                 });
00355 
00356                 Button setEnd = new Button( comp, SWT.NONE );
00357                 setEnd.setText(MessageBundle.get("GrxPathPlanningView.button.set")); //$NON-NLS-1$
00358                 setEnd.addSelectionListener(new SelectionAdapter(){
00359             public void widgetSelected(SelectionEvent e){
00360                 ppaItem_.setEndPoint();
00361                         }
00362         });
00363 
00364                 // ---- robot Z position
00365                 Composite roboZComp = getComp( 4, SWT.NONE );
00366 
00367                 l = new Label(roboZComp, SWT.NONE);
00368                 l.setText(MessageBundle.get("GrxPathPlanningView.label.collision")); //$NON-NLS-1$
00369                 
00370                 tolerance_ = new Text(roboZComp, SWT.SINGLE | SWT.BORDER);
00371                 tolerance_.setText("0"); //$NON-NLS-1$
00372                 gd = new GridData();
00373                 gd.widthHint = TEXT_WIDTH;
00374                 tolerance_.setLayoutData( gd );
00375                 tolerance_.addFocusListener(new FocusListener(){
00376                         public void focusGained(FocusEvent e) {
00377                         }
00378                         public void focusLost(FocusEvent e) {
00379                                 Double d = Double.parseDouble(tolerance_.getText());
00380                                 if (d != null){
00381                                         ppaItem_.setDbl("tolerance", d); //$NON-NLS-1$
00382                                 }
00383                         }
00384                 });
00385                 
00386             chkRebuildRoadmap_ = new Button(roboZComp,SWT.CHECK);
00387             chkRebuildRoadmap_.setText(MessageBundle.get("GrxPathPlanningView.button.rebuild")); //$NON-NLS-1$
00388                 chkRebuildRoadmap_.addSelectionListener(new SelectionListener(){
00389                         public void widgetDefaultSelected(SelectionEvent e) {
00390                         }
00391                         public void widgetSelected(SelectionEvent e) {
00392                                 if (chkRebuildRoadmap_.getSelection()){
00393                                         ppaItem_.setProperty("rebuildRoadmap", "true"); //$NON-NLS-1$ //$NON-NLS-2$
00394                                 }else{
00395                                         ppaItem_.setProperty("rebuildRoadmap", "false"); //$NON-NLS-1$ //$NON-NLS-2$
00396                                 }
00397                         }
00398                 });
00399             chkRebuildRoadmap_.setSelection(true);
00400 
00401                 
00402                 //---- algorithm
00403         
00404                 Composite algoComp = getComp( 3, SWT.NONE );
00405 
00406                 l = new Label(algoComp, SWT.NONE);
00407                 l.setText(MessageBundle.get("GrxPathPlanningView.label.algorithm")); //$NON-NLS-1$
00408                 algoSelect = new Combo( algoComp, SWT.READ_ONLY );
00409                 algoSelect.setLayoutData( new GridData(GridData.FILL_HORIZONTAL) );
00410                 algoSelect.addSelectionListener(new SelectionListener(){
00411                         public void widgetDefaultSelected(SelectionEvent e) {
00412                         }
00413                         public void widgetSelected(SelectionEvent e) {
00414                                 ppaItem_.setProperty("algorithm", algoSelect.getItem(algoSelect.getSelectionIndex())); //$NON-NLS-1$
00415                         }
00416                 });
00417 
00418                 updatePropertyButton = new Button(algoComp, SWT.NONE);
00419                 updatePropertyButton.setText(MessageBundle.get("GrxPathPlanningView.button.getProperties")); //$NON-NLS-1$
00420                 updatePropertyButton.addSelectionListener(new SelectionAdapter(){
00421             public void widgetSelected(SelectionEvent e){
00422                 ppaItem_.propertyUpdate();
00423                         }
00424         });
00425                 
00426                 //---- calculate
00427 
00428                 Composite calcComp = getComp( 5, SWT.NONE );
00429 
00430                 btnCalcStart = new Button( calcComp, SWT.NONE );
00431                 btnCalcStart.setText(MessageBundle.get("GrxPathPlanningView.button.calcStart")); //$NON-NLS-1$
00432                 btnCalcStart.addSelectionListener(new SelectionAdapter(){
00433             public void widgetSelected(SelectionEvent e){
00434                 removeCarpet();
00435                 ppaItem_.startCalc();
00436                 displayCarpet();
00437                 pathGraph();
00438                         }
00439         });
00440 
00441                 imgLabel = new Label(calcComp, SWT.BORDER);
00442                 imgLabel.setImage( Activator.getDefault().getImage("grxrobot1.png" ) ); //$NON-NLS-1$
00443 
00444                 
00445                 l = new Label(calcComp, SWT.NONE);
00446                 l.setText(MessageBundle.get("GrxPathPlanningView.label.optimizer")); //$NON-NLS-1$
00447                 optimizerSelect = new Combo( calcComp, SWT.NONE|SWT.READ_ONLY );
00448                 optimizerSelect.setLayoutData( new GridData(GridData.FILL_HORIZONTAL) );
00449                 optimizerSelect.addSelectionListener(new SelectionListener(){
00450                         public void widgetDefaultSelected(SelectionEvent e) {
00451                         }
00452                         public void widgetSelected(SelectionEvent e) {
00453                                 ppaItem_.setProperty("optimizer", optimizerSelect.getItem(optimizerSelect.getSelectionIndex())); //$NON-NLS-1$
00454                         }
00455                 });
00456 
00457                 optimizeButton = new Button( calcComp, SWT.NONE );
00458                 optimizeButton.setText(MessageBundle.get("GrxPathPlanningView.button.optimize")); //$NON-NLS-1$
00459                 optimizeButton.addSelectionListener(new SelectionAdapter(){
00460             public void widgetSelected(SelectionEvent e){
00461                 ppaItem_.optimize();
00462                         removeCarpet();
00463                         displayCarpet();
00464                 pathGraph();
00465                         }
00466         });
00467 
00468 
00469                 // ---- Route View
00470                 Composite carpetComp = getComp( 5, SWT.NONE );
00471 
00472                 l = new Label(carpetComp, SWT.NONE);
00473                 l.setText(MessageBundle.get("GrxPathPlanningView.label.path")); //$NON-NLS-1$
00474                 
00475                 btnVisible = new Button( carpetComp, SWT.TOGGLE );
00476                 btnVisible.setText(MessageBundle.get("GrxPathPlanningView.button.visible")); //$NON-NLS-1$
00477                 btnVisible.setSelection(true);
00478                 btnVisible.addSelectionListener(new SelectionAdapter(){
00479             public void widgetSelected(SelectionEvent e){
00480                 boolean selection = btnVisible.getSelection();
00481                 if (selection) {
00482                         System.out.println("TRUE"); //$NON-NLS-1$
00483                         switcher_.setWhichChild(0);
00484                 } else {
00485                         System.out.println("FALSE"); //$NON-NLS-1$
00486                         switcher_.setWhichChild(Switch.CHILD_NONE);
00487                 }
00488                 
00489                         }
00490         });
00491 
00492                 l = new Label(carpetComp, SWT.NONE);
00493                 l.setText(MessageBundle.get("GrxPathPlanningView.label.carpetZ")); //$NON-NLS-1$
00494                 
00495                 carpetZ = new Text(carpetComp, SWT.SINGLE | SWT.BORDER );
00496                 carpetZ.setLayoutData( new GridData(GridData.FILL_HORIZONTAL) );
00497                 carpetZ.setText( String.valueOf(DEFAULT_CARPET_Z) );
00498         carpetZ.addKeyListener(new KeyListener(){
00499             public void keyPressed(KeyEvent e) {}
00500             public void keyReleased(KeyEvent e) {
00501                 if (e.character == SWT.CR) {
00502                         updateCarpet();
00503                 }
00504             }
00505         });
00506                 carpetZ.addFocusListener(new FocusListener(){
00507                         public void focusGained(FocusEvent e) {
00508                         }
00509                         public void focusLost(FocusEvent e) {
00510                                 Double d = Double.parseDouble(carpetZ.getText());
00511                                 if (d != null){
00512                                         ppaItem_.setDbl("carpetZ", d); //$NON-NLS-1$
00513                                 }
00514                         }
00515                 });
00516 
00517         setScrollMinSize(SWT.DEFAULT,SWT.DEFAULT);
00518         }
00519 
00520         
00521         private void initJava3D(){
00522                 carpet_ = new BranchGroup();
00523                 carpet_.setCapability(BranchGroup.ALLOW_CHILDREN_READ);
00524                 carpet_.setCapability(BranchGroup.ALLOW_CHILDREN_WRITE);
00525                 carpet_.setCapability(BranchGroup.ALLOW_CHILDREN_EXTEND);
00526                 carpet_.setCapability(BranchGroup.ALLOW_DETACH); //親(carpetTransGroup)から離れられるようにする
00527         
00528                 carpetTrans = new Transform3D();
00529                 carpetTrans.setTranslation( new Vector3d(0,0, getCarpetZPosition()) );
00530         
00531                 carpetTransGroup = new TransformGroup();
00532                 carpetTransGroup.setCapability( TransformGroup.ALLOW_TRANSFORM_READ );
00533                 carpetTransGroup.setCapability( TransformGroup.ALLOW_TRANSFORM_WRITE );
00534                 carpetTransGroup.setCapability( Group.ALLOW_CHILDREN_WRITE);
00535                 carpetTransGroup.setCapability( Group.ALLOW_CHILDREN_EXTEND);
00536                 carpetTransGroup.setTransform( carpetTrans );
00537                 carpetTransGroup.addChild( carpet_ );
00538         
00539                 switcher_ = new Switch();
00540             switcher_.setCapability(Switch.ALLOW_SWITCH_READ);
00541             switcher_.setCapability(Switch.ALLOW_SWITCH_WRITE);
00542             switcher_.addChild( carpetTransGroup );
00543             switcher_.setWhichChild(0);
00544         }
00545 
00546         
00550         private void update(){
00551             String[] algoNames = ppaItem_.getAlgorithms();
00552             algoSelect.removeAll();
00553                 for( String s: algoNames )
00554                         algoSelect.add( s );
00555         
00556                 // mobilities
00557             String[] mobilityNames = ppaItem_.getMobilityNames();
00558             mobilitySelect.removeAll();
00559                 for( String s: mobilityNames )
00560                         mobilitySelect.add( s );
00561                 
00562                 // optimizers
00563             String[] optimizerNames = ppaItem_.getOptimizerNames();
00564             optimizerSelect.removeAll();
00565                 for( String s: optimizerNames )
00566                         optimizerSelect.add( s );
00567                 
00568                 if (ppaItem_ != null){
00569                         String algorithmName = ppaItem_.getStr("algorithm", ""); //$NON-NLS-1$ //$NON-NLS-2$
00570                         if (algoSelect.indexOf(algorithmName) >= 0){
00571                                 algoSelect.select(algoSelect.indexOf(algorithmName));
00572                         }
00573                         String mobilityName = ppaItem_.getStr("mobility", ""); //$NON-NLS-1$ //$NON-NLS-2$
00574                         if (mobilitySelect.indexOf(mobilityName) >= 0){
00575                                 mobilitySelect.select(mobilitySelect.indexOf(mobilityName));
00576                         }
00577                         String optimizerName = ppaItem_.getStr("optimizer", ""); //$NON-NLS-1$ //$NON-NLS-2$
00578                         if (optimizerSelect.indexOf(optimizerName) >= 0){
00579                                 optimizerSelect.select(optimizerSelect.indexOf(optimizerName));
00580                         }
00581                 }
00582         }
00583         
00587         private void removeCarpet() {
00588                 try{
00589                 carpetTransGroup.removeChild( carpet_ );
00590                 carpet_.removeAllChildren();
00591                 carpetTransGroup.addChild( carpet_ );
00592                 }catch( Exception e ){
00593                         e.printStackTrace();
00594                 }
00595         }
00596 
00597         private String getRobotBaseLink( GrxModelItem model ) {
00598                 return model.rootLink().getName();
00599         }
00600 
00601         private void getStartPoint(){
00602                 String robotName = modelSelect.getText();
00603                 GrxModelItem robot = (GrxModelItem)(manager_.getItem( GrxModelItem.class, robotName ));
00604                 if( robot == null )
00605                         return;
00606                 double [] tr = robot.getDblAry(getRobotBaseLink(robot)+".translation",null); //$NON-NLS-1$
00607                 double [] rot = robot.getDblAry(getRobotBaseLink(robot)+".rotation",null); //$NON-NLS-1$
00608                 try{
00609                         textStartX.setText( String.format( FORMAT, tr[0]) );
00610                         textStartY.setText( String.format( FORMAT, tr[1]) );
00611                         textStartTheta.setText( String.format( FORMAT, rot[3] / (2f*Math.PI) * 360f) );
00612                         if (ppaItem_ != null){
00613                                 ppaItem_.setDbl("startX", tr[0]); //$NON-NLS-1$
00614                                 ppaItem_.setDbl("startY", tr[1]); //$NON-NLS-1$
00615                                 ppaItem_.setDbl("startTheta", rot[3]); //$NON-NLS-1$
00616                         }
00617                 }catch(Exception e){
00618                         return;
00619                 }
00620         }
00621 
00622         private void getEndPoint(){
00623                 String robotName = modelSelect.getText();
00624                 GrxModelItem robot = (GrxModelItem)(manager_.getItem( GrxModelItem.class, robotName ));
00625                 if( robot == null )
00626                         return;
00627                 double [] tr = robot.getDblAry(getRobotBaseLink(robot)+".translation",null); //$NON-NLS-1$
00628                 double [] rot = robot.getDblAry(getRobotBaseLink(robot)+".rotation",null); //$NON-NLS-1$
00629                 try{
00630                         textEndX.setText( String.format( FORMAT, tr[0]) );
00631                         textEndY.setText( String.format( FORMAT, tr[1]) );
00632                         textEndTheta.setText( String.format( FORMAT, rot[3] / (2f*Math.PI) * 360f) );
00633                         if (ppaItem_ != null){
00634                                 ppaItem_.setDbl("goalX", tr[0]); //$NON-NLS-1$
00635                                 ppaItem_.setDbl("goalY", tr[1]); //$NON-NLS-1$
00636                                 ppaItem_.setDbl("goalTheta", rot[3]); //$NON-NLS-1$
00637                         }
00638                 }catch(Exception e){
00639                         return;
00640                 }
00641         }
00642 
00643         Grx3DView get3DView()
00644         {
00645                 if( view_ == null )
00646                         view_ = (Grx3DView)manager_.getView(Grx3DView.class, false);
00647                 return view_;
00648         }
00649 
00650         private void displayCarpet(){
00651         double[][] path = ppaItem_.getPath();
00652                 for( int i=0; i+1<path.length; i++ ) {
00653                 double []p1 = path[i], p2 = path[i+1];
00654                 carpet( p1, p2 );
00655         }
00656         }
00657         
00663         private void carpet( double[] start, double[] goal ){
00664                 double carpetHeight=0.01;
00665                 double dx = goal[0] - start[0], dy = goal[1] - start[1], len = Math.sqrt( dx/2*dx/2+dy/2*dy/2 );
00666                 double theta;
00667                 if (len < 0.001){
00668                         len = 0.01;
00669                         theta = goal[2];
00670                 }else{
00671                         theta = Math.atan2( dy,dx ); 
00672                 }
00673                 //Point3d destPoint = new Point3d( x2+dx/2, y2+dy/2, getZPosition()+carpetHeight );
00674                 Point3d centerPoint = new Point3d( (start[0]+goal[0])/2, (start[1]+goal[1])/2, 0 );
00675 
00676                 // APPEARANCE
00677                 Appearance appea = new Appearance( );
00678                 Material material = new Material( );
00679                 Color3f color = new Color3f( 0, 1f, 0 );
00680                 material.setDiffuseColor( color );
00681                 appea.setMaterial( material );
00682                 appea.setTransparencyAttributes(new TransparencyAttributes( TransparencyAttributes.NICEST,0.7f ) );
00683                 // carpet block
00684                 Box box = new Box( (float)len, 0.1f, (float) carpetHeight, appea );
00685                 // 移動回転
00686                 Transform3D tr = new Transform3D();
00687                 tr.setTranslation( new Vector3d( centerPoint ) );
00688                 Transform3D rot = new Transform3D();
00689                 rot.rotZ( theta );
00690                 tr.mul(rot);
00691                 // ノードの追加
00692                 BranchGroup bg = new BranchGroup();
00693                 TransformGroup newtg = new TransformGroup( tr );
00694                 bg.addChild( newtg );
00695                 newtg.addChild( box );
00696                 carpet_.addChild( bg );
00697         }
00698 
00699 
00700     
00704     private void pathGraph(){
00705                 if ( view_ == null ){
00706                         view_ = get3DView();
00707                         if( view_ != null ){
00708                                 BranchGroup bg = new BranchGroup();
00709                             bg.addChild( switcher_ );
00710                             view_.attachUnclickable( bg );
00711                         }else{  
00712                             GrxDebugUtil.println("[GrxPathPlanningView] can't find 3DView"); //$NON-NLS-1$
00713                         }
00714                 }
00715 
00716         RoadmapNode[] tree = ppaItem_.getRoadmap();
00717         
00718         Vector<Point3d> vertex = new Vector<Point3d>();
00719 
00720         // LineArrayで使うためにvectorへ放り込む
00721         for( RoadmapNode n : tree ){
00722                 Point3d start = new Point3d( n.cfg[0], n.cfg[1], 0 ); //三つめはtheta
00723                 for( int l : n.neighbors ){
00724                         // 一方向リンクなのでダブりは考えなくてよし
00725                 Point3d to = new Point3d(tree[l].cfg[0],tree[l].cfg[1],0);
00726                 vertex.add( start );
00727                         vertex.add( to );
00728                         //GrxDebugUtil.println("[PPV]@pathGraph start"+start+" to "+to );
00729                 }
00730         }
00731 
00732         GrxDebugUtil.println("[PPV]@pathGraph size="+vertex.size()); //$NON-NLS-1$
00733         if (vertex.size() == 0) return;
00734         LineArray geometry = new LineArray(vertex.size(), GeometryArray.COORDINATES | GeometryArray.COLOR_3);
00735         geometry.setCoordinates(0, vertex.toArray(new Point3d[vertex.size()]) );
00736         for( int i=0; i<vertex.size(); i++)
00737                 geometry.setColor(i, new Color3f(Color.red));
00738 
00739                 // ノードの追加
00740         Shape3D graph = new Shape3D(geometry);
00741         graph.setPickable(false);
00742                 // 移動回転
00743                 Transform3D tr = new Transform3D();
00744                 tr.setTranslation( new Vector3d( 0,0,0 ) );
00745                 BranchGroup bg = new BranchGroup();
00746                 TransformGroup newtg = new TransformGroup( tr );
00747                 bg.addChild( newtg );
00748                 newtg.addChild( graph );
00749 
00750                 carpet_.addChild( bg );
00751         
00752         return;
00753     }
00754     
00755     
00756     public void registerItemChange(GrxBaseItem item, int event){
00757         if(item instanceof GrxPathPlanningAlgorithmItem){
00758                 GrxPathPlanningAlgorithmItem ppa = (GrxPathPlanningAlgorithmItem) item;
00759                 switch(event){
00760                 case GrxPluginManager.SELECTED_ITEM:
00761                         if(ppaItem_!= ppa){
00762                                 ppaItem_ = ppa;
00763                                 ppaItem_.addObserver(this);
00764                                 update();
00765                                 setppaItem();
00766                         }
00767                         break;
00768                 case GrxPluginManager.REMOVE_ITEM:
00769                 removeCarpet();
00770                 case GrxPluginManager.NOTSELECTED_ITEM:
00771                         if(ppaItem_==ppa){
00772                                 ppaItem_.deleteObserver(this);
00773                                 ppaItem_ = null;
00774                                 setEnabled(false);
00775                         }
00776                         break;
00777                 default:
00778                         break;
00779                 }
00780         }
00781     }
00782     
00783     private void setppaItem(){
00784         setEnabled(true);
00785                 String modelName = null;
00786                 if (ppaItem_ != null){
00787                         modelName = ppaItem_.getStr("model", null); //$NON-NLS-1$
00788                 }else{
00789                         modelName = modelSelect.getText();
00790                 }
00791                 
00792                 Object[] items = manager_.getSelectedItemList( GrxModelItem.class ).toArray();
00793                 modelSelect.removeAll();
00794                 for( Object o: items ){
00795                         GrxModelItem model = (GrxModelItem)o;
00796                         if (model.isRobot()){
00797                                 modelSelect.add( o.toString() );
00798                         }
00799                 }
00800                 if (modelName != null && modelSelect.indexOf(modelName) >= 0){
00801                         modelSelect.select(modelSelect.indexOf(modelName));
00802                 }
00803                 
00804                 if (ppaItem_ != null){
00805                         String mobilityName = ppaItem_.getStr("mobility", ""); //$NON-NLS-1$ //$NON-NLS-2$
00806                         if (mobilitySelect.indexOf(mobilityName) >= 0){
00807                                 mobilitySelect.select(mobilitySelect.indexOf(mobilityName));
00808                         }else{
00809                                 mobilitySelect.deselectAll();
00810                         }
00811                         String optimizerName = ppaItem_.getStr("optimizer", ""); //$NON-NLS-1$ //$NON-NLS-2$
00812                         if (optimizerSelect.indexOf(optimizerName) >= 0){
00813                                 optimizerSelect.select(optimizerSelect.indexOf(optimizerName));
00814                         }else{
00815                                 optimizerSelect.deselectAll();
00816                         }
00817                         String algorithmName = ppaItem_.getStr("algorithm", ""); //$NON-NLS-1$ //$NON-NLS-2$
00818                         if (algoSelect.indexOf(algorithmName) >= 0){
00819                                 algoSelect.select(algoSelect.indexOf(algorithmName));
00820                         }else{
00821                                 algoSelect.deselectAll();
00822                         }
00823                         Double sx = ppaItem_.getDbl("startX", null); //$NON-NLS-1$
00824                         if (sx != null){
00825                                 textStartX.setText(sx.toString());
00826                         }else{
00827                                 textStartX.setText(""); //$NON-NLS-1$
00828                         }
00829                         Double sy = ppaItem_.getDbl("startY", null); //$NON-NLS-1$
00830                         if (sy != null){
00831                                 textStartY.setText(sy.toString());
00832                         }else{
00833                                 textStartY.setText(""); //$NON-NLS-1$
00834                         }
00835                         Double st = ppaItem_.getDbl("startTheta", null); //$NON-NLS-1$
00836                         if (st != null){
00837                                 textStartTheta.setText(st.toString());
00838                         }else{
00839                                 textStartTheta.setText(""); //$NON-NLS-1$
00840                         }
00841                         Double ex = ppaItem_.getDbl("goalX", null); //$NON-NLS-1$
00842                         if (ex != null){
00843                                 textEndX.setText(ex.toString());
00844                         }else{
00845                                 textEndX.setText(""); //$NON-NLS-1$
00846                         }
00847                         Double ey = ppaItem_.getDbl("goalY", null); //$NON-NLS-1$
00848                         if (ey != null){
00849                                 textEndY.setText(ey.toString());
00850                         }else{
00851                                 textEndY.setText(""); //$NON-NLS-1$
00852                         }
00853                         Double et = ppaItem_.getDbl("goalTheta", null); //$NON-NLS-1$
00854                         if (et != null){
00855                                 textEndTheta.setText(et.toString());
00856                         }else{
00857                                 textEndTheta.setText(""); //$NON-NLS-1$
00858                         }
00859                         chkRebuildRoadmap_.setSelection(ppaItem_.isTrue("rebuildRoadmap", true)); //$NON-NLS-1$
00860                         tolerance_.setText(ppaItem_.getDbl("tolerance",0.0).toString()); //$NON-NLS-1$
00861                         carpetZ.setText(ppaItem_.getDbl("carpetZ",0.01).toString()); //$NON-NLS-1$
00862                 }
00863                 
00864         }
00865 
00870         double getCarpetZPosition(){
00871                 double z;
00872                 try{
00873                         z= Double.valueOf( carpetZ.getText() );
00874                 }catch(Exception e){
00875                         z=0.2;
00876                         e.printStackTrace();
00877                 }
00878                 return z;
00879         }
00880 
00885         void updateCarpet(){
00886                 carpetTrans.setTranslation( new Vector3d(0,0, getCarpetZPosition() ) );
00887                 carpetTransGroup.setTransform( carpetTrans );
00888         }
00889         
00894         public void setEnabled(boolean b){
00895                 Control[] ctl = getComposite().getChildren();
00896                 for (int i=0; i<ctl.length; i++){
00897                         ctl[i].setEnabled(b);
00898                         if (ctl[i] instanceof Composite){
00899                                 Composite cmp = (Composite)ctl[i];
00900                                 Control[] ctl2 = cmp.getChildren();
00901                                 for (int j=0; j<ctl2.length; j++){
00902                                         ctl2[j].setEnabled(b);
00903                                 }
00904                         }
00905                 }
00906         }
00907         
00908         public void update(GrxBasePlugin plugin, Object... arg) {
00909                 if(ppaItem_==plugin){
00910                         if(((String)arg[0]).equals("connected")){ //$NON-NLS-1$
00911                                 update();
00912                         }
00913                 }
00914         }
00915 
00916         public void shutdown() {
00917                 removeCarpet();
00918         manager_.removeItemChangeListener(this, GrxPathPlanningAlgorithmItem.class);
00919         if(ppaItem_!=null)
00920                 ppaItem_.deleteObserver(this);
00921         }
00922 }


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Apr 11 2019 03:30:16