GrxRobotHardwareClientView.java
Go to the documentation of this file.
00001 package com.generalrobotix.ui.view;
00002 
00003 import java.util.ArrayList;
00004 import java.util.Date;
00005 
00006 import jp.go.aist.hrp.simulator.DynamicsSimulator;
00007 import jp.go.aist.hrp.simulator.DynamicsSimulatorFactory;
00008 import jp.go.aist.hrp.simulator.DynamicsSimulatorFactoryHelper;
00009 import jp.go.aist.hrp.simulator.SensorState;
00010 import jp.go.aist.hrp.simulator.WorldStateHolder;
00011 import jp.go.aist.hrp.simulator.DynamicsSimulatorPackage.IntegrateMethod;
00012 import jp.go.aist.hrp.simulator.DynamicsSimulatorPackage.LinkDataType;
00013 import jp.go.aist.hrp.simulator.DynamicsSimulatorPackage.SensorOption;
00014 
00015 import org.eclipse.jface.dialogs.MessageDialog;
00016 import org.eclipse.swt.SWT;
00017 import org.eclipse.swt.events.KeyAdapter;
00018 import org.eclipse.swt.events.KeyEvent;
00019 import org.eclipse.swt.events.SelectionEvent;
00020 import org.eclipse.swt.events.SelectionListener;
00021 import org.eclipse.swt.graphics.Image;
00022 import org.eclipse.swt.layout.GridData;
00023 import org.eclipse.swt.layout.GridLayout;
00024 import org.eclipse.swt.widgets.Button;
00025 import org.eclipse.swt.widgets.Composite;
00026 import org.eclipse.swt.widgets.Display;
00027 import org.eclipse.swt.widgets.Label;
00028 import org.eclipse.swt.widgets.Text;
00029 
00030 import OpenHRP.RobotHardwareService;
00031 import OpenHRP.RobotHardwareServiceHelper;
00032 import OpenHRP.StateHolderService;
00033 import OpenHRP.StateHolderServiceHelper;
00034 import OpenHRP.StateHolderServicePackage.CommandHolder;
00035 import OpenHRP.RobotHardwareServicePackage.RobotStateHolder;
00036 import OpenHRP.RobotHardwareServicePackage.SwitchStatus;
00037 import RTC.ComponentProfile;
00038 import RTC.ConnectorProfile;
00039 import RTC.ConnectorProfileHolder;
00040 import RTC.ExecutionContext;
00041 import RTC.LifeCycleState;
00042 import RTC.PortInterfaceProfile;
00043 import RTC.PortProfile;
00044 import RTC.PortService;
00045 import RTC.RTObject;
00046 import RTC.RTObjectHelper;
00047 
00048 import com.generalrobotix.ui.GrxBaseItem;
00049 import com.generalrobotix.ui.GrxBaseView;
00050 import com.generalrobotix.ui.GrxBaseViewPart;
00051 import com.generalrobotix.ui.GrxPluginManager;
00052 import com.generalrobotix.ui.grxui.Activator;
00053 import com.generalrobotix.ui.item.GrxModelItem;
00054 import com.generalrobotix.ui.item.GrxWorldStateItem;
00055 import com.generalrobotix.ui.item.GrxWorldStateItem.WorldStateEx;
00056 import com.generalrobotix.ui.util.GrxCorbaUtil;
00057 import com.generalrobotix.ui.util.GrxDebugUtil;
00058 import com.generalrobotix.ui.util.GrxXmlUtil;
00059 
00060 @SuppressWarnings("serial")
00064 public class GrxRobotHardwareClientView extends GrxBaseView {
00065         public static final String TITLE = "RobotHardware RTC Client";
00066         private static final int NOT_CONNECTED = 0;
00067         private static final int CONNECTING    = 1;
00068         private static final int CONNECTED     = 2;
00069 
00070         private GrxJythonPromptView jythonView_;
00071 
00072         private StateHolderService sholder_;
00073         private RobotHardwareService hwCtrl_;
00074 
00075         private DynamicsSimulator dynamics_;
00076         private GrxWorldStateItem currentItem_;
00077         private GrxModelItem currentModel_;
00078 
00079         private WorldStateHolder worldStateH_  = new WorldStateHolder();
00080         private RobotStateHolder robotStateH_  = new RobotStateHolder();
00081 
00082         private String robotType_ = "-----";
00083         private String robotHost_    = "localhost";
00084         private int    robotPort_    = 2809;
00085         private int    interval_  = 200;
00086         private String setupFile_ = "-----";
00087         private String RobotHardwareRTC_ = "RobotHardware0";
00088         private String StateHolderRTC_ = "StateHolder0";
00089 
00090         private Date initialDate_;
00091         private Date prevDate_;
00092         private int  state_       = NOT_CONNECTED;
00093         private boolean actualFlag = false;
00094         private Image startMonitorIcon_ = Activator.getDefault().getImage("sim_start.png");
00095         private Image stopMonitorIcon_  = Activator.getDefault().getImage("sim_stop.png");
00096         private Image servoOnIcon_  = Activator.getDefault().getImage("robot_servo_start.png");
00097         private Image servoOffIcon_ = Activator.getDefault().getImage("robot_servo_stop.png");
00098         private Image lampOnIcon_  = Activator.getDefault().getImage("lamp_on.png");
00099         private Image lampOffIcon_ = Activator.getDefault().getImage("lamp_off.png");
00100         private Button btnConnect_;
00101         private Button btnServo_;
00102         private Button btnSetup_;
00103         private Label  lblLamp_;
00104         private Label  lblStatus_;
00105         private Text   fldStatus_;
00106         private Composite propertyPanel_;
00107         private boolean isMonitorRunning_;
00108 
00109         private ArrayList<SetPropertyPanel> propList_ = new ArrayList<SetPropertyPanel>();
00110 
00118         public GrxRobotHardwareClientView(String name, GrxPluginManager manager, GrxBaseViewPart vp, Composite parent) {
00119                 super(name, manager, vp, parent);
00120 
00121                 composite_.setLayout(new GridLayout(1,false));
00122                 Composite layout1 = new Composite(composite_, SWT.NONE);
00123                 layout1.setLayoutData(new GridData(GridData.HORIZONTAL_ALIGN_CENTER));
00124                 GridLayout gridLayout = new GridLayout(6, false);
00125                 gridLayout.marginWidth = 5;
00126                 gridLayout.horizontalSpacing = 5;
00127                 layout1.setLayout(gridLayout);
00128                 // servo on/off button
00129                 btnServo_ = new Button(layout1, SWT.TOGGLE);
00130                 btnServo_.setImage(servoOnIcon_);
00131                 GridData btnGridData = new GridData();
00132                 btnGridData.widthHint = 32;
00133                 btnGridData.heightHint = 32;
00134                 btnServo_.setLayoutData(btnGridData);
00135                 // status label
00136                 lblStatus_ = new Label(layout1, SWT.NONE);
00137                 lblStatus_.setText("Status");
00138                 // lamp label
00139                 lblLamp_ = new Label(layout1, SWT.NONE);
00140                 lblLamp_.setImage(lampOffIcon_);
00141                 // status filed
00142                 fldStatus_ = new Text(layout1, SWT.SINGLE | SWT.BORDER); 
00143                 fldStatus_.setText("Not Connected.");
00144                 GridData textGridData = new GridData();
00145                 textGridData.widthHint = 100;
00146                 fldStatus_.setLayoutData(textGridData);
00147                 // connect button
00148                 btnConnect_ = new Button(layout1, SWT.TOGGLE);
00149                 btnConnect_.setLayoutData(btnGridData);
00150                 btnConnect_.setImage(startMonitorIcon_);
00151                 // setup button
00152                 btnSetup_ = new Button(layout1, SWT.NONE);
00153                 btnSetup_.setText("ROBOT SETUP");
00154 
00155                 propertyPanel_ = new Composite(composite_,SWT.NONE);
00156                 propertyPanel_.setLayoutData(new GridData(GridData.HORIZONTAL_ALIGN_CENTER));
00157                 GridLayout gridLayout1 = new GridLayout(1, false);
00158                 gridLayout.marginWidth = 5;
00159                 gridLayout.horizontalSpacing = 5;
00160                 propertyPanel_.setLayout(gridLayout1);
00161                 propList_.add(new SetPropertyPanel(propertyPanel_, SWT.NONE, "Robot Host",  "robotHost", true, robotHost_));
00162                 propList_.add(new SetPropertyPanel(propertyPanel_, SWT.NONE, "Robot Port",  "robotPort", true, new Integer(robotPort_).toString()));
00163                 propList_.add(new SetPropertyPanel(propertyPanel_, SWT.NONE, "Robot Name",  "ROBOT",     true, robotType_));
00164                 propList_.add(new SetPropertyPanel(propertyPanel_, SWT.NONE, "Interval[ms]","interval",  true, new Integer(interval_).toString()));
00165                 propList_.add(new SetPropertyPanel(propertyPanel_, SWT.NONE, "Setup File",  "setupFile", true, setupFile_));
00166                 propList_.add(new SetPropertyPanel(propertyPanel_, SWT.NONE, "RobotHardwareService",  "RobotHardwareServiceRTC", true, RobotHardwareRTC_));
00167                 propList_.add(new SetPropertyPanel(propertyPanel_, SWT.NONE, "StateHolderService",  "StateHolderRTC", true, StateHolderRTC_));
00168 
00169                 btnConnect_.addSelectionListener(new SelectionListener() {
00170                         public void widgetDefaultSelected(SelectionEvent e) {}
00171                         public void widgetSelected(SelectionEvent e) {
00172                                 if (btnConnect_.getSelection()) {
00173                                         startMonitor(true);
00174                                 } else {
00175                                         boolean ans = MessageDialog.openConfirm(getParent().getShell(),
00176                                                         "Do you really want to stop monitoring?",
00177                                         "Stopping Robot State Monitor");
00178 
00179                                         if (ans){
00180                                                 stopMonitor();
00181                                         }else{
00182                                                 btnConnect_.setSelection(true);
00183                                         }
00184                                 }
00185                         }
00186                 });
00187 
00188                 btnSetup_.setEnabled(false);
00189                 btnSetup_.addSelectionListener(new SelectionListener() {
00190                         public void widgetDefaultSelected(SelectionEvent e) {}
00191                         public void widgetSelected(SelectionEvent e) {
00192                                 setupFile_ = getProperty("setupFile");
00193                                 getJythonView().execFile(GrxXmlUtil.expandEnvVal(setupFile_));
00194                         }
00195                 });
00196 
00197                 btnServo_.setEnabled(false);
00198                 btnServo_.addSelectionListener(new SelectionListener() {
00199                         public void widgetDefaultSelected(SelectionEvent e) {}
00200                         public void widgetSelected(SelectionEvent e) {
00201                             if (btnServo_.getSelection()){
00202                                 servoOn();
00203                             }else{
00204                                         servoOff();
00205                             }
00206                         }
00207                 });
00208 
00209                 currentItem_ = manager_.<GrxWorldStateItem>getSelectedItem(GrxWorldStateItem.class, null);
00210                 if(currentItem_!=null){
00211                         currentItem_.addObserver(this);
00212                 }
00213                 manager_.registerItemChangeListener(this, GrxWorldStateItem.class);
00214         }
00215 
00216         public void registerItemChange(GrxBaseItem item, int event){
00217                 if(item instanceof GrxWorldStateItem){
00218                         GrxWorldStateItem worldStateItem = (GrxWorldStateItem) item;
00219                 switch(event){
00220                 case GrxPluginManager.SELECTED_ITEM:
00221                         if(currentItem_!=worldStateItem){
00222                                 currentItem_ = worldStateItem;
00223                                 currentItem_.addObserver(this);
00224                         }
00225                         break;
00226                 case GrxPluginManager.REMOVE_ITEM:
00227                 case GrxPluginManager.NOTSELECTED_ITEM:
00228                         if(currentItem_==worldStateItem){
00229                                 currentItem_.deleteObserver(this);
00230                                 currentItem_ = null;
00231                         }
00232                         break;
00233                 default:
00234                         break;
00235                 }
00236                 }
00237         }
00238         
00239         private class SetPropertyPanel extends Composite {
00240                 private String propName;        
00241                 private boolean isLocal = true;
00242                 private String defaultVal;
00243 
00244                 private Label    label;
00245                 private Text  fld;
00246                 private Button     set;
00247                 private Button  resume;
00248 
00249 
00250                 public SetPropertyPanel(Composite parent, int style, String _title, String _propName, boolean _isLocal, String _defaultVal) {
00251                         super(parent, style);
00252                         GridLayout gridLayout = new GridLayout(4, false);
00253                         gridLayout.marginWidth = 5;
00254                         gridLayout.horizontalSpacing = 5;
00255                         this.setLayout(gridLayout);
00256                         GridData labelGridData = new GridData();
00257                         labelGridData.widthHint = 150;
00258                         label = new Label(this, SWT.NONE);
00259                         label.setText(_title);
00260                         label.setLayoutData(labelGridData);
00261                         fld = new Text(this, SWT.NONE);
00262                         GridData textGridData = new GridData();
00263                         textGridData.widthHint = 120;
00264                         fld.setLayoutData(textGridData);
00265                         set = new Button(this, SWT.NONE);
00266                         set.setText("Set");
00267                         resume = new Button(this, SWT.NONE);
00268                         resume.setText("Resume");
00269                         propName = _propName;
00270                         isLocal = _isLocal;
00271                         defaultVal = _defaultVal;
00272 
00273                         fld.addKeyListener(new KeyAdapter() {
00274                                 public void keyReleased(KeyEvent e){
00275                                         if (e.keyCode == SWT.CR) {
00276                                                 set(); 
00277                                         } else {
00278                                                 boolean hasChanged = !fld.getText().equals(getValue());
00279                                                 set.setEnabled(hasChanged);
00280                                                 resume.setEnabled(hasChanged);
00281                                         }
00282                                 }
00283                         });
00284 
00285                         set.setEnabled(false);
00286                         set.addSelectionListener(new SelectionListener() {
00287                                 public void widgetDefaultSelected(SelectionEvent e) {
00288                                 }
00289                                 public void widgetSelected(SelectionEvent e) {
00290                                         set();
00291                                 }
00292                         });
00293 
00294                         resume.setEnabled(false);
00295                         resume.addSelectionListener(new SelectionListener() {
00296                                 public void widgetDefaultSelected(SelectionEvent e) {
00297                                 }
00298                                 public void widgetSelected(SelectionEvent e) {
00299                                         resume();
00300                                 }
00301                         });
00302                 }
00303 
00304                 public void setEditable(boolean isEditable) {
00305                         fld.setEditable(isEditable);
00306                         resume.setEnabled(isEditable);
00307                         set.setEnabled(isEditable);
00308                 }
00309 
00310                 public String getValue() {
00311                         String str;
00312                         if (isLocal)
00313                                 str = getProperty(propName);
00314                         else
00315                                 str = manager_.getProjectProperty(propName);
00316                         if (str == null)
00317                                 str = defaultVal;
00318                         return str;
00319                 }
00320 
00321                 public void set() {
00322                         String value = fld.getText();
00323                         if (isLocal){
00324                                 setProperty(propName, value);
00325                         }else{
00326                                 manager_.setProjectProperty(propName, value);
00327                                 System.out.println("("+propName+","+value+")"); 
00328                         }
00329                         set.setEnabled(false);
00330                         resume.setEnabled(false);
00331                 }
00332 
00333                 public void resume() {
00334                         fld.setText(getValue());
00335                         set.setEnabled(false);
00336                         resume.setEnabled(false);
00337                 }
00338         }
00339 
00340         private void startMonitor(final boolean isInteractive) {
00341                 if (isMonitorRunning_) return;
00342                 
00343                 isMonitorRunning_ = true;
00344                 btnConnect_.setImage(stopMonitorIcon_);
00345                 btnConnect_.setToolTipText("Stop Robot State Monitor");
00346                 btnConnect_.setSelection(true);
00347 
00348                 setConnectionState(CONNECTING);
00349 
00350                 //GrxGuiUtil.setEnableRecursive(false, propertyPanel_, null);
00351 
00352                 Runnable runnable = new Runnable() {
00353                         public void run(){
00354                                 Display display = composite_.getDisplay();
00355                                 if (state_ == CONNECTING){
00356                                         tryConnection(isInteractive);
00357                                         if (!display.isDisposed())
00358                                                 display.timerExec(1000, this);
00359                                 }else if (state_ == CONNECTED){
00360                                         try{
00361                                         updateRobotState();
00362                                         }catch(Exception ex){
00363                                                 ex.printStackTrace();
00364                                         }
00365                                         interval_  = getInt("interval", 200);
00366                                         if (!display.isDisposed())
00367                                                 display.timerExec(interval_, this);
00368                                 }else{
00369                                 }
00370                         }
00371                 };
00372                 Display display = composite_.getDisplay();
00373                 if (!display.isDisposed()){
00374                         display.timerExec(1, runnable);
00375                 }
00376 
00377 
00378         }
00379 
00380     private void tryConnection(boolean isInteractive){
00381         System.out.println("tryConnection()");
00382         if (currentItem_ == null) return;
00383                 try {
00384                         for (int i=0; i<propList_.size(); i++)
00385                                 propList_.get(i).resume();
00386 
00387                         // set property for connection
00388                         robotHost_ = getStr("robotHost");
00389                         if (robotHost_ != null){
00390                                 manager_.setProjectProperty("nsHost", robotHost_);
00391                         }
00392                         try {
00393                                 robotPort_ = Integer.parseInt(manager_.getProjectProperty("robotPort"));
00394                         } catch (Exception e) {
00395                                 robotPort_ = 2809; // if not set try the default port number
00396                         }
00397 
00398                         robotType_ = getStr("ROBOT");
00399 
00400                         currentModel_ = (GrxModelItem) manager_.getItem(GrxModelItem.class, robotType_);
00401                         if (currentModel_ == null){
00402                             System.out.println("Can't find robot model("+robotType_+")"); 
00403                                 return;
00404                         }
00405 
00406                         //actualFlag = isTrue("showActualState", false);
00407 
00408                         // initialize logger
00409                         currentItem_.clearLog();
00410                         currentItem_.registerCharacter(robotType_, currentModel_.getBodyInfo());
00411 
00412                         // get RobotHardwareService
00413                         System.out.println("RobotHardwareRTC_ = "+RobotHardwareRTC_);
00414                         RTObject rtc = findRTC(RobotHardwareRTC_);
00415                         ExecutionContext ec = rtc.get_owned_contexts()[0];
00416                         if (ec.get_component_state(rtc) != LifeCycleState.ACTIVE_STATE){
00417                                 ec.activate_component(rtc);
00418                         }
00419                         hwCtrl_ = RobotHardwareServiceHelper.narrow(findService(rtc, "service0"));
00420 
00421                         // initialize servo On button
00422                         if (isAnyServoOn()) {
00423                                 btnServo_.setSelection(true);
00424                                 btnServo_.setImage(servoOffIcon_);
00425                                 btnServo_.setToolTipText("Servo Off");
00426                         } else {
00427                                 btnServo_.setSelection(false);
00428                                 btnServo_.setImage(servoOnIcon_);
00429                                 btnServo_.setToolTipText("Servo On");
00430                         }
00431                         prevDate_ = new Date();
00432 
00433                         initDynamicsSimulator(false);
00434 
00435                         setConnectionState(CONNECTED);
00436 
00437                 } catch (Exception e) {
00438                         //GrxDebugUtil.printErr("", e);
00439 
00440                         if (isInteractive && currentModel_ == null) {
00441                                 MessageDialog.openInformation(getParent().getShell(),
00442                                                 "Load Model(" + robotType_ + ") first.",
00443                                 "Start Robot State Monitor");
00444                                 stopMonitor();
00445                         } else {
00446                                 setConnectionState(CONNECTING);
00447                         }
00448                 }
00449     }
00450 
00451         private void stopMonitor() {
00452             btnConnect_.setImage(startMonitorIcon_);
00453                 btnConnect_.setToolTipText("Start Robot State Monitor");
00454                 btnConnect_.setSelection(false);
00455 
00456                 setConnectionState(NOT_CONNECTED);
00457                 isMonitorRunning_ = false;
00458         }
00459 
00460         private void setConnectionState(int state) {
00461                 switch (state) {
00462                 case NOT_CONNECTED:
00463                         btnSetup_.setEnabled(false);
00464                         btnServo_.setEnabled(false);
00465                         fldStatus_.setText("Not Connected.");
00466                         lblLamp_.setImage(lampOffIcon_);
00467                         //GrxGuiUtil.setEnableRecursive(true, propertyPanel_, null);
00468                         for (int i=0; i<propList_.size(); i++) {
00469                                 propList_.get(i).set.setEnabled(false);
00470                                 propList_.get(i).resume.setEnabled(false);
00471                         }
00472 
00473                         break;
00474 
00475                 case CONNECTING:
00476                         btnSetup_.setEnabled(false);
00477                         btnServo_.setEnabled(false);
00478                         fldStatus_.setText("Connecting ...");
00479                         //GrxGuiUtil.setEnableRecursive(false, propertyPanel_, null);
00480                         break;
00481 
00482                 case CONNECTED:
00483                         btnSetup_.setEnabled(true);
00484                         btnServo_.setEnabled(true);
00485                         fldStatus_.setText("Connected.");
00486                         lblLamp_.setImage(lampOnIcon_);
00487                         break;
00488                 }
00489                 state_ = state;
00490         }
00491 
00492         public void restoreProperties() {
00493                 super.restoreProperties();
00494                 robotHost_ = getStr("robotHost");
00495                 if (robotHost_ != null){
00496                         manager_.setProjectProperty("nsHost", robotHost_);
00497 
00498                 }
00499                 startMonitor(false);
00500         }
00501 
00502         public RTObject findRTC(String name){
00503                 org.omg.CORBA.Object obj = GrxCorbaUtil.getReference(name, "rtc", robotHost_, robotPort_);
00504                 return RTObjectHelper.narrow(obj);
00505         }
00506 
00507         public org.omg.CORBA.Object findService(RTObject rtc, String service){
00508                 ComponentProfile prof = rtc.get_component_profile();
00509                 PortProfile[] port_prof = prof.port_profiles;
00510                 PortService port = null;
00511                 for (int i=0; i<port_prof.length; i++){
00512                         PortProfile pp = port_prof[i];
00513                         PortInterfaceProfile[] ifs = pp.interfaces;
00514                         for (int j=0; j<ifs.length; j++){
00515                                 PortInterfaceProfile aif = ifs[j];
00516                                 if (aif.instance_name.equals(service)){
00517                                         port = pp.port_ref;
00518                                 }
00519                         }
00520                 }
00521                 ConnectorProfile con_prof = new ConnectorProfile();
00522                 con_prof.name = "noname";
00523                 con_prof.connector_id = "";
00524                 con_prof.ports = new PortService[1];
00525                 con_prof.ports[0] = port;
00526                 con_prof.properties = new _SDOPackage.NameValue[0];
00527                 ConnectorProfileHolder con_prof_holder = new ConnectorProfileHolder();
00528                 con_prof_holder.value = con_prof;
00529                 port.connect(con_prof_holder);
00530                 String ior = con_prof_holder.value.properties[0].value.extract_string();
00531                 port.disconnect(con_prof_holder.value.connector_id);
00532                 return GrxCorbaUtil.getORB().string_to_object(ior);
00533         } 
00534 
00535         // get the data here and stuff it into currentItem_
00536     private void updateRobotState(){
00537                 //if (currentItem_ == null || !btnConnect_.isSelected()) {
00538                 if (currentItem_ == null) {
00539                         stopMonitor();
00540                         return;
00541                 }
00542                 
00543                 Date now = new Date();
00544                 long time = now.getTime();
00545                 if (prevDate_ != null) {
00546                         time -= prevDate_.getTime();
00547                 }
00548                 if (state_ == CONNECTING) {
00549                         if (time > 1000) {
00550                                 prevDate_ = now;
00551                                 if (lblLamp_.getImage() == lampOffIcon_)
00552                                         lblLamp_.setImage(lampOnIcon_);
00553                                 else
00554                                         lblLamp_.setImage(lampOffIcon_);
00555                                 startMonitor(false);
00556                         }
00557                         return;
00558                 }
00559 
00560                 if (state_ == CONNECTED && time > interval_) {
00561                         prevDate_ = now;
00562                         try {
00563                                 if (hwCtrl_ != null){
00564                                         hwCtrl_.getStatus(robotStateH_);
00565                                 }
00566 
00567                                 if(actualFlag) {
00568                                         dynamics_.setCharacterAllLinkData(robotType_, LinkDataType.JOINT_VALUE, robotStateH_.value.angle);
00569                                 } else {
00570                                         //dynamics_.setCharacterAllLinkData(robotType_, LinkDataType.JOINT_VALUE, robotStateH_.value.command);
00571                                 }
00572                                 
00573                                 try{
00574                                         if (sholder_ == null){
00575                                                 StateHolderRTC_ = getStr("StateHolderRTC", StateHolderRTC_);
00576                                                 RTObject rtc = findRTC(StateHolderRTC_);
00577                                                 sholder_ = StateHolderServiceHelper.narrow(findService(rtc, "service0"));
00578                                         }
00579                                         CommandHolder com = new CommandHolder();
00580                                         sholder_.getCommand(com);
00581                                         dynamics_.setCharacterLinkData(robotType_, currentModel_.rootLink().getName(), LinkDataType.ABS_TRANSFORM, com.value.baseTransform);
00582                                         dynamics_.setCharacterAllLinkData(robotType_, LinkDataType.JOINT_VALUE, robotStateH_.value.command);
00583                                 }catch(Exception e){
00584                                         GrxDebugUtil.printErr("failed to connect StateHolderService");
00585                                         sholder_ = null;
00586                                 }
00587 
00588                                 dynamics_.calcWorldForwardKinematics();
00589                                 dynamics_.getWorldState(worldStateH_);
00590 
00591                         } catch (Exception e) {
00592                                 GrxDebugUtil.printErr("iobclient got exception:", e);
00593                                 setConnectionState(CONNECTING);
00594                                 return;
00595                         }
00596 
00597                         SensorState ss = new SensorState();
00598                         ss.q = robotStateH_.value.angle;
00599                         ss.u = robotStateH_.value.torque;
00600                         ss.force = robotStateH_.value.force;
00601                         ss.accel = robotStateH_.value.accel;
00602                         ss.rateGyro = robotStateH_.value.rateGyro;
00603 
00604                         WorldStateEx wsx = new WorldStateEx(worldStateH_.value);
00605                         if (currentItem_.getLogSize() == 0){
00606                                 initialDate_ = now;
00607                         }
00608                         wsx.time = (now.getTime() - initialDate_.getTime()) / 1000.0;
00609                         wsx.collisions = null;
00610                         wsx.setSensorState(robotType_, ss);
00611                         wsx.setTargetState(robotType_, robotStateH_.value.command);
00612                         int [] sstate = new int[robotStateH_.value.servoState.length];
00613                         for (int i=0; i<sstate.length; i++){
00614                             sstate[i] = robotStateH_.value.servoState[i][0];
00615                         }
00616                         wsx.setServoState(robotType_, sstate);
00617                         wsx.setPowerState(robotType_, robotStateH_.value.voltage, robotStateH_.value.current);
00618                         currentItem_.addValue(wsx.time, wsx);
00619                         currentItem_.setPosition(currentItem_.getLogSize()-1);
00620                 }
00621         }
00622 
00623         private DynamicsSimulator initDynamicsSimulator(boolean update) {
00624                 if (dynamics_ != null && !update) {
00625                         return dynamics_;
00626                 }
00627                 org.omg.CORBA.Object obj = GrxCorbaUtil.getReference("DynamicsSimulatorFactory");
00628                 DynamicsSimulatorFactory dynFactory = DynamicsSimulatorFactoryHelper.narrow(obj);
00629                 if (dynamics_ != null)  {
00630                         try {
00631                                 dynamics_.destroy();
00632                         } catch(Exception e) {
00633                                 GrxDebugUtil.printErr("", e);
00634                         }
00635                 }
00636                 try {
00637                         dynamics_ = dynFactory.create();
00638                         dynamics_.registerCharacter(robotType_, currentModel_.getBodyInfo());
00639                         dynamics_.init(0.001, IntegrateMethod.EULER, SensorOption.DISABLE_SENSOR);
00640                 } catch (Exception e) {
00641                         dynamics_ = null;
00642                         e.printStackTrace();
00643                 }
00644 
00645                 return dynamics_;
00646         }
00647 
00648         private GrxJythonPromptView getJythonView() {
00649                 if (jythonView_ == null) {
00650                         jythonView_= (GrxJythonPromptView)manager_.getView(GrxJythonPromptView.class,true);
00651                 }
00652 
00653                 return jythonView_;
00654         }
00655 
00656         private void servoOn() {
00657                 if (isAnyServoOn()) {
00658                         return;
00659                 }
00660                 if(hwCtrl_ != null) {
00661                         hwCtrl_.power("all", SwitchStatus.SWITCH_ON);
00662                         try{
00663                                 Thread.sleep(1000);
00664                         }catch(Exception ex){
00665                                 ex.printStackTrace();
00666                         }
00667                 }
00668 
00669                 boolean ans = MessageDialog.openConfirm(getParent().getShell(),
00670                                 "!! Robot Motion Warning (SERVO ON) !!\n" + 
00671                                 "Confirm RELAY turned ON.\n" + 
00672                                 "Then Push [OK] to servo ON.\n", 
00673                 "Servo ON");
00674                 if (ans) {
00675                         try {
00676                                 // get StateHolderService
00677                                 StateHolderRTC_ = getStr("StateHolderRTC", StateHolderRTC_);
00678                                 RTObject rtc = findRTC(StateHolderRTC_);
00679                                 sholder_ = StateHolderServiceHelper.narrow(findService(rtc, "service0"));
00680 
00681                                 if (sholder_ != null) {
00682                                         sholder_.goActual();
00683                                         try{
00684                                                 Thread.sleep(100);
00685                                         }catch(Exception ex){
00686                                                 ex.printStackTrace();
00687                                         }
00688                                 }
00689 
00690                                 if(hwCtrl_ != null){
00691                                         hwCtrl_.servo("all", SwitchStatus.SWITCH_ON);
00692                                         try {
00693                                                 Thread.sleep(5000);
00694                                         }catch(Exception ex){
00695                                                 ex.printStackTrace();
00696                                         }
00697                                 }
00698 
00699                                 btnServo_.setImage(servoOffIcon_);
00700                                 btnServo_.setToolTipText("Servo Off");
00701                         } catch (Exception e) {
00702                                 if(hwCtrl_ != null)
00703                                         hwCtrl_.power("all", SwitchStatus.SWITCH_OFF);
00704                                 GrxDebugUtil.printErr("got exception during servo on process:", e);
00705                         }
00706                 } else {
00707                         if(hwCtrl_ != null)
00708                                 hwCtrl_.power("all", SwitchStatus.SWITCH_OFF);
00709                         btnServo_.setSelection(false);
00710                 }
00711         }
00712 
00713         private void servoOff() {
00714                 boolean ans = MessageDialog.openConfirm(getParent().getShell(),
00715                                 "!! Robot Motion Warning (SERVO OFF) !!\n\n" + 
00716                                 "Push [OK] to servo OFF.\n", 
00717                 "Servo OFF");
00718                 if (ans) {
00719                         try {
00720                                 if(hwCtrl_ != null) {
00721                                         hwCtrl_.servo("all", SwitchStatus.SWITCH_OFF);
00722                                         hwCtrl_.power("all", SwitchStatus.SWITCH_OFF);
00723                                 }
00724                                 btnServo_.setImage(servoOnIcon_);
00725                                 btnServo_.setToolTipText("Servo On");
00726                         } catch (Exception e) {
00727                                 GrxDebugUtil.printErr("got exception during servo off process:", e);
00728                         }
00729                 } else {
00730                     btnServo_.setSelection(true);
00731                 }
00732         }
00733 
00734         public boolean isAnyServoOn() {
00735                 final int SERVO_STATE_MASK = 0x2;
00736                 if(hwCtrl_ != null) {
00737                         hwCtrl_.getStatus(robotStateH_);
00738                         int[][] state = robotStateH_.value.servoState;
00739                         for (int i=0; i<currentModel_.getDOF(); i++) {
00740                                 if ((state[i][0]&SERVO_STATE_MASK) != 0) {
00741                                         return true;
00742                                 }
00743                         }
00744                 }
00745                 return false;
00746         }       
00747 }


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed Sep 6 2017 02:35:55