00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019 package com.generalrobotix.ui.view;
00020
00021 import java.text.DecimalFormat;
00022 import java.util.ArrayList;
00023 import java.util.Iterator;
00024 import java.util.List;
00025 import java.util.Vector;
00026
00027 import jp.go.aist.hrp.simulator.SensorState;
00028
00029 import org.eclipse.jface.layout.AbstractColumnLayout;
00030 import org.eclipse.jface.layout.TableColumnLayout;
00031 import org.eclipse.jface.viewers.ArrayContentProvider;
00032 import org.eclipse.jface.viewers.ColumnWeightData;
00033 import org.eclipse.jface.viewers.ILabelProviderListener;
00034 import org.eclipse.jface.viewers.ITableColorProvider;
00035 import org.eclipse.jface.viewers.ITableFontProvider;
00036 import org.eclipse.jface.viewers.ITableLabelProvider;
00037 import org.eclipse.jface.viewers.TableViewer;
00038 import org.eclipse.swt.SWT;
00039 import org.eclipse.swt.events.ControlEvent;
00040 import org.eclipse.swt.events.ControlListener;
00041 import org.eclipse.swt.events.SelectionAdapter;
00042 import org.eclipse.swt.events.SelectionEvent;
00043 import org.eclipse.swt.graphics.Color;
00044 import org.eclipse.swt.graphics.Font;
00045 import org.eclipse.swt.graphics.Image;
00046 import org.eclipse.swt.graphics.Point;
00047 import org.eclipse.swt.layout.GridData;
00048 import org.eclipse.swt.layout.GridLayout;
00049 import org.eclipse.swt.widgets.Combo;
00050 import org.eclipse.swt.widgets.Composite;
00051 import org.eclipse.swt.widgets.Event;
00052 import org.eclipse.swt.widgets.Listener;
00053 import org.eclipse.swt.widgets.TableColumn;
00054
00055 import com.generalrobotix.ui.GrxBaseItem;
00056 import com.generalrobotix.ui.GrxBasePlugin;
00057 import com.generalrobotix.ui.GrxBaseView;
00058 import com.generalrobotix.ui.GrxBaseViewPart;
00059 import com.generalrobotix.ui.GrxPluginManager;
00060 import com.generalrobotix.ui.grxui.Activator;
00061 import com.generalrobotix.ui.item.GrxLinkItem;
00062 import com.generalrobotix.ui.item.GrxModelItem;
00063 import com.generalrobotix.ui.item.GrxSensorItem;
00064 import com.generalrobotix.ui.item.GrxWorldStateItem;
00065 import com.generalrobotix.ui.item.GrxWorldStateItem.CharacterStateEx;
00066 import com.generalrobotix.ui.item.GrxWorldStateItem.WorldStateEx;
00067
00068 @SuppressWarnings("serial")
00069 public class GrxRobotStatView extends GrxBaseView {
00070 public static final String TITLE = "Robot State";
00071
00072 private static final DecimalFormat FORMAT1 = new DecimalFormat(" 0.0;-0.0");
00073 private static final DecimalFormat FORMAT2 = new DecimalFormat(" 0.000;-0.000");
00074
00075 private Font plainFont_;
00076 private Font boldFont_;
00077
00078 private GrxWorldStateItem currentWorld_ = null;
00079 private GrxModelItem currentModel_ = null;
00080 private SensorState currentSensor_;
00081 private double[] currentRefAng_;
00082 private int[] currentSvStat_;
00083 private double[] currentPower_;
00084 private WorldStateEx currentState_ = null;
00085
00086 private List<GrxLinkItem> jointList_ = new ArrayList<GrxLinkItem>();
00087 private String[] forceName_;
00088 private String[] gyroName_;
00089 private String[] accName_;
00090
00091 private Combo comboModelName_;
00092 private List<GrxModelItem> modelList_;
00093
00094 private TableViewer[] viewers_;
00095 private TableViewer jointTV_;
00096 private TableViewer sensorTV_;
00097
00098
00099
00100
00101 private Color white_;
00102 private Color black_;
00103 private Color red_;
00104 private Color blue_;
00105 private Color yellow_;
00106
00107 private String osName_ = "Windows";
00108
00109 private static final int COMBO_WIDTH = 100;
00110 private int [][] columnWidth_ = new int[][]{
00111 {5,16,11,11,11,7,7,7,7,9,9},
00112 {22,13,13,13,13,13,13},
00113 {30,90,60,60,60,40,40,40,40,50,50},
00114 {110,75,75,75,75,75,75}
00115 };
00116 private Composite[] viewPanel_ = new Composite[2];
00117 private Composite mainPanel_;
00118
00119 public GrxRobotStatView(String name, GrxPluginManager manager, GrxBaseViewPart vp, Composite parent) {
00120 super(name, manager,vp,parent);
00121 osName_ = System.getProperty("os.name");
00122
00123 white_ = Activator.getDefault().getColor("white");
00124 black_ = Activator.getDefault().getColor("black");
00125 red_ = Activator.getDefault().getColor("red");
00126 blue_ = Activator.getDefault().getColor("blue");
00127 yellow_ = Activator.getDefault().getColor("yellow");
00128
00129 mainPanel_ = new Composite(composite_, SWT.NONE);
00130 GridLayout gridLayout = new GridLayout(1,false);
00131 gridLayout.marginHeight = 0;
00132 gridLayout.verticalSpacing = 0;
00133 mainPanel_.setLayout(gridLayout);
00134 modelList_ = new ArrayList<GrxModelItem>();
00135
00136 comboModelName_ = new Combo(mainPanel_,SWT.READ_ONLY);
00137 GridData gridData = new GridData();
00138 gridData.widthHint = COMBO_WIDTH;
00139 comboModelName_.setLayoutData(new GridData(GridData.FILL_HORIZONTAL));
00140
00141 comboModelName_.addSelectionListener(new SelectionAdapter(){
00142
00143 public void widgetSelected(SelectionEvent e) {
00144 GrxModelItem item = modelList_.get(comboModelName_.getSelectionIndex());
00145 if (item == null || item == currentModel_)
00146 return;
00147
00148 currentModel_ = item;
00149 setJointList();
00150 updateTableViewer();
00151 }
00152
00153 });
00154
00155 String[][] header = new String[][] {
00156 { "No", "Joint", "Angle", "Target", "Torque", "PWR", "SRV", "ARM", "T", "Pgain", "Dgain" },
00157 { "1","2","3","4","5","6","7"},
00158 };
00159 boolean showheader[] = {true, false};
00160 int[][] alignment = new int[][] {
00161 { SWT.RIGHT, SWT.LEFT, SWT.RIGHT, SWT.RIGHT, SWT.RIGHT, SWT.CENTER, SWT.CENTER, SWT.RIGHT, SWT.RIGHT, SWT.RIGHT, SWT.RIGHT },
00162 { SWT.LEFT, SWT.RIGHT, SWT.RIGHT, SWT.RIGHT, SWT.RIGHT, SWT.RIGHT, SWT.RIGHT, SWT.RIGHT },
00163 };
00164
00165 for(int i=0; i<2; i++){
00166 viewPanel_[i] = new Composite(mainPanel_, SWT.NONE);
00167 viewPanel_[i].setLayoutData(new GridData(GridData.FILL_BOTH));
00168 GridLayout gridLayout1 = new GridLayout();
00169 gridLayout1.marginHeight = 0;
00170 gridLayout1.marginWidth = 0;
00171 gridLayout1.horizontalSpacing = 0;
00172 gridLayout1.verticalSpacing = 0;
00173 viewPanel_[i].setLayout(gridLayout1);
00174 }
00175
00176 jointTV_ = new TableViewer(viewPanel_[0],SWT.BORDER|SWT.FULL_SELECTION|SWT.VIRTUAL);
00177 sensorTV_ = new TableViewer(viewPanel_[1],SWT.BORDER|SWT.FULL_SELECTION|SWT.VIRTUAL);
00178
00179 jointTV_.setContentProvider(new ArrayContentProvider());
00180 sensorTV_.setContentProvider(new ArrayContentProvider());
00181
00182 jointTV_.setLabelProvider(new JointTableLabelProvider());
00183 sensorTV_.setLabelProvider(new SensorTableLabelProvider());
00184
00185 viewers_ = new TableViewer[]{jointTV_,sensorTV_};
00186 TableColumnLayout[] layout = new TableColumnLayout[2];
00187 for(int i=0;i<viewers_.length;i++){
00188 layout[i] = new TableColumnLayout();
00189 for(int j=0;j<header[i].length;j++){
00190 TableColumn column = new TableColumn(viewers_[i].getTable(),SWT.NULL);
00191 column.setText(header[i][j]);
00192 column.setAlignment(alignment[i][j]);
00193 layout[i].setColumnData(column, new ColumnWeightData(columnWidth_[i][j],columnWidth_[i+2][j]));
00194 if(i==0)
00195 column.addControlListener(new ControlListener(){
00196 public void controlMoved(ControlEvent e) {
00197 }
00198 public void controlResized(ControlEvent e) {
00199 TableColumn column = (TableColumn)e.getSource();
00200 int i = jointTV_.getTable().indexOf(column);
00201 int width = column.getWidth();
00202 setInt("jcolumnWidth"+i, width);
00203 }
00204 });
00205 else if(i==1)
00206 column.addControlListener(new ControlListener(){
00207 public void controlMoved(ControlEvent e) {
00208 }
00209 public void controlResized(ControlEvent e) {
00210 TableColumn column = (TableColumn)e.getSource();
00211 int i = sensorTV_.getTable().indexOf(column);
00212 int width = column.getWidth();
00213 setInt("scolumnWidth"+i, width);
00214 }
00215 });
00216 }
00217 viewPanel_[i].setLayout(layout[i]);
00218 viewers_[i].getTable().setHeaderVisible(showheader[i]);
00219 viewers_[i].getTable().setLinesVisible(true);
00220 viewers_[i].getTable().setLayoutData(new GridData(GridData.FILL_BOTH));
00221 }
00222
00223
00224 setUp();
00225 manager_.registerItemChangeListener(this, GrxModelItem.class);
00226 manager_.registerItemChangeListener(this, GrxWorldStateItem.class);
00227
00228 }
00229
00230 public void setUp(){
00231 Iterator<GrxModelItem> it = modelList_.iterator();
00232 while(it.hasNext()){
00233 GrxModelItem modelItem = it.next();
00234 modelItem.deleteObserver(this);
00235 comboModelName_.remove(modelItem.getName());
00236 }
00237 modelList_ = manager_.<GrxModelItem>getSelectedItemList(GrxModelItem.class);
00238 if(!modelList_.isEmpty()){
00239 it = modelList_.iterator();
00240 while(it.hasNext()){
00241 GrxModelItem model = it.next();
00242 comboModelName_.add(model.getName());
00243 model.addObserver(this);
00244 }
00245 comboModelName_.select(0);
00246 currentModel_ = modelList_.get(0);
00247 setJointList();
00248 }else
00249 currentModel_ = null;
00250 if(currentWorld_ != null ){
00251 currentWorld_.deleteObserver(this);
00252 currentWorld_.deletePosObserver(this);
00253 }
00254 currentWorld_ = manager_.<GrxWorldStateItem>getSelectedItem(GrxWorldStateItem.class, null);
00255 if(currentWorld_!=null){
00256 currentState_ = currentWorld_.getValue();
00257 currentWorld_.addObserver(this);
00258 currentWorld_.addPosObserver(this);
00259 updatePosition(currentWorld_, currentWorld_.getPosition());
00260 }
00261 updateTableFont();
00262 updateTableViewer();
00263 }
00264
00265
00266
00267
00268
00269
00270
00271
00272 private Integer[] _createJointTVInput(){
00273 Integer[] input = new Integer[jointList_.size()];
00274 for(int i=0;i<input.length;i++){
00275 input[i] = i;
00276 }
00277 return input;
00278 }
00279
00280 private Integer[] _createSensorTVInput(){
00281 int length = 0;
00282 if(forceName_ != null){
00283 length += forceName_.length;
00284 if (forceName_.length >= 2 && forceName_[0].equals("rfsensor") && forceName_[1].equals("lfsensor")){
00285 length += 1;
00286 }
00287 }
00288 if(gyroName_ != null)
00289 length += gyroName_.length;
00290 if(accName_ != null)
00291 length += accName_.length;
00292 length += 1;
00293 Integer[] input = new Integer[length];
00294 for(int i=0;i<input.length;i++){
00295 input[i] = i;
00296 }
00297 return input;
00298 }
00299
00300 public boolean propertyChanged(String key, String value){
00301 if (super.propertyChanged(key, value)){
00302 return true;
00303 }else {
00304 if (key.startsWith("jcolumnWidth")){
00305 int i = Integer.parseInt(key.replace("jcolumnWidth", ""));
00306 int width = Integer.parseInt(value.trim());
00307 int w = jointTV_.getTable().getSize().x;
00308 int width0 = width*100/w;
00309 int width1;
00310 if(width < columnWidth_[2][i])
00311 width1 = width;
00312 else
00313 width1 = columnWidth_[2][i];
00314 ((AbstractColumnLayout) viewPanel_[0].getLayout()).setColumnData(jointTV_.getTable().getColumn(i), new ColumnWeightData(width0,width1));
00315 viewPanel_[0].layout(true);
00316 final String key_ = key;
00317 final String value_ = value;
00318 syncExec(new Runnable(){
00319 public void run(){
00320 setProperty(key_, value_);
00321 }
00322 });
00323 return true;
00324 }else if (key.startsWith("scolumnWidth")){
00325 int i = Integer.parseInt(key.replace("scolumnWidth", ""));
00326 int width = Integer.parseInt(value.trim());
00327 int w = sensorTV_.getTable().getSize().x;
00328 int width0 = width*100/w;
00329 int width1;
00330 if(width < columnWidth_[3][i])
00331 width1 = width;
00332 else
00333 width1 = columnWidth_[3][i];
00334 ((AbstractColumnLayout) viewPanel_[1].getLayout()).setColumnData(sensorTV_.getTable().getColumn(i), new ColumnWeightData(width0,width1));
00335 viewPanel_[1].layout(true);
00336 final String key_ = key;
00337 final String value_ = value;
00338 syncExec(new Runnable(){
00339 public void run(){
00340 setProperty(key_, value_);
00341 }
00342 });
00343 return true;
00344 }
00345 }
00346 return false;
00347 }
00348
00349
00350 private void setJointList(){
00351 jointList_.clear();
00352 Vector<GrxLinkItem> lInfo = currentModel_.links_;
00353 for (int i = 0; i < lInfo.size(); i++) {
00354 for (int j = 0; j < lInfo.size(); j++) {
00355 if (i == lInfo.get(j).jointId_) {
00356 jointList_.add(lInfo.get(j));
00357 break;
00358 }
00359 }
00360 }
00361 forceName_ = currentModel_.getSensorNames("Force");
00362 gyroName_ = currentModel_.getSensorNames("RateGyro");
00363 accName_ = currentModel_.getSensorNames("Acceleration");
00364 }
00365
00366 public void registerItemChange(GrxBaseItem item, int event){
00367 if(item instanceof GrxModelItem){
00368 GrxModelItem modelItem = (GrxModelItem) item;
00369 switch(event){
00370 case GrxPluginManager.SELECTED_ITEM:
00371 if(!modelList_.contains(modelItem)){
00372 modelList_.add(modelItem);
00373 comboModelName_.add(modelItem.getName());
00374 modelItem.addObserver(this);
00375 if(currentModel_ == null){
00376 currentModel_ = modelItem;
00377 comboModelName_.select(comboModelName_.indexOf(modelItem.getName()));
00378 setJointList();
00379 updateTableViewer();
00380 }
00381 }
00382 break;
00383 case GrxPluginManager.REMOVE_ITEM:
00384 case GrxPluginManager.NOTSELECTED_ITEM:
00385 if(modelList_.contains(modelItem)){
00386 int index = modelList_.indexOf(modelItem);
00387 modelList_.remove(modelItem);
00388 modelItem.deleteObserver(this);
00389 comboModelName_.remove(modelItem.getName());
00390 if(currentModel_ == modelItem){
00391 if(index < modelList_.size()){
00392 currentModel_ = modelList_.get(index);
00393 comboModelName_.select(comboModelName_.indexOf(currentModel_.getName()));
00394 setJointList();
00395 updateTableViewer();
00396 }else{
00397 index--;
00398 if(index >= 0 ){
00399 currentModel_ = modelList_.get(index);
00400 comboModelName_.select(comboModelName_.indexOf(currentModel_.getName()));
00401 setJointList();
00402 updateTableViewer();
00403 }else{
00404 currentModel_ = null;
00405 jointList_.clear();
00406 updateTableViewer();
00407 }
00408 }
00409
00410 }
00411 }
00412 break;
00413 default:
00414 break;
00415 }
00416 }else if(item instanceof GrxWorldStateItem){
00417 GrxWorldStateItem worldStateItem = (GrxWorldStateItem) item;
00418 switch(event){
00419 case GrxPluginManager.SELECTED_ITEM:
00420 if(currentWorld_ != worldStateItem){
00421 currentWorld_ = worldStateItem;
00422 currentState_ = currentWorld_.getValue();
00423 updatePosition(currentWorld_, currentWorld_.getPosition());
00424 currentWorld_.addObserver(this);
00425 currentWorld_.addPosObserver(this);
00426 }
00427 break;
00428 case GrxPluginManager.REMOVE_ITEM:
00429 case GrxPluginManager.NOTSELECTED_ITEM:
00430 if(currentWorld_ == worldStateItem){
00431 currentWorld_.deleteObserver(this);
00432 currentWorld_.deletePosObserver(this);
00433 currentWorld_ = null;
00434 currentState_ = null;
00435 updateTableViewer();
00436 }
00437 break;
00438 default:
00439 break;
00440 }
00441 }
00442 }
00443
00444 public void update(GrxBasePlugin plugin, Object... arg){
00445 if(currentWorld_ == plugin ){
00446 if((String)arg[0]=="ClearLog"){
00447 currentState_ = null;
00448 }
00449 }else if(modelList_.contains(plugin)){
00450 if( (String)arg[0] == "PropertyChange" ){
00451 if( (String)arg[1] == "name" ){
00452 int index = comboModelName_.indexOf(plugin.getOldName());
00453 comboModelName_.setItem(index, (String)arg[2]);
00454 }
00455 }
00456 }
00457 }
00458
00459 public void updatePosition(GrxBasePlugin plugin, Integer arg_pos){
00460 if(currentWorld_ != plugin)
00461 return;
00462
00463 int pos = arg_pos.intValue();
00464 currentState_ = currentWorld_.getValue(pos);
00465 _refresh();
00466 }
00467
00468 private void updateTableViewer(){
00469 if (currentModel_ == null ){
00470 jointTV_.setInput(new Integer[0]);
00471 sensorTV_.setInput(new Integer[0]);
00472 return;
00473 }
00474 currentSensor_ = null;
00475 currentRefAng_ = null;
00476 currentSvStat_ = null;
00477 currentPower_ = null;
00478 if (currentState_ != null) {
00479 CharacterStateEx charStat = currentState_.get(currentModel_.getName());
00480 if (charStat != null) {
00481 currentSensor_ = charStat.sensorState;
00482 currentRefAng_ = charStat.targetState;
00483 currentSvStat_ = charStat.servoState;
00484 currentPower_ = charStat.powerState;
00485 }
00486 }
00487 jointTV_.setInput(_createJointTVInput());
00488 sensorTV_.setInput(_createSensorTVInput());
00489 viewPanel_[0].layout();
00490 viewPanel_[1].layout();
00491
00492 Point point = jointTV_.getTable().computeSize(SWT.DEFAULT, SWT.DEFAULT);
00493 jointTV_.getTable().setSize(point);
00494 Point point1 = sensorTV_.getTable().computeSize(SWT.DEFAULT, SWT.DEFAULT);
00495 sensorTV_.getTable().setSize(point1);
00496 setScrollMinSize(0, point.y+point1.y);
00497 mainPanel_.layout(true);
00498 }
00499
00500 private void _refresh(){
00501 if (currentModel_ == null ) return;
00502 currentSensor_ = null;
00503 currentRefAng_ = null;
00504 currentSvStat_ = null;
00505 currentPower_ = null;
00506 if (currentState_ != null) {
00507 CharacterStateEx charStat = currentState_.get(currentModel_.getName());
00508 if (charStat != null) {
00509 currentSensor_ = charStat.sensorState;
00510 currentRefAng_ = charStat.targetState;
00511 currentSvStat_ = charStat.servoState;
00512 currentPower_ = charStat.powerState;
00513 }
00514 }
00515 jointTV_.refresh();
00516 sensorTV_.refresh();
00517 }
00518
00519 class JointTableLabelProvider implements ITableLabelProvider,ITableColorProvider,ITableFontProvider {
00520 final int CALIB_STATE_MASK = 0x00000001;
00521 final int SERVO_STATE_MASK = 0x00000002;
00522 final int POWER_STATE_MASK = 0x00000004;
00523 final int SERVO_ALARM_MASK = 0x0007fff8;
00524 final int SERVO_ALARM_SHIFT = 3;
00525 final int DRIVER_TEMP_MASK = 0xff000000;
00526 final int DRIVER_TEMP_SHIFT = 24;
00527
00528 public Image getColumnImage(Object element, int columnIndex) {
00529 return null;
00530 }
00531
00532 public String getColumnText(Object element, int columnIndex) {
00533 int rowIndex = ((Integer)element).intValue();
00534
00535
00536
00537
00538
00539
00540
00541
00542 switch (columnIndex) {
00543 case 0:
00544 return Integer.toString(rowIndex);
00545 case 1:
00546 if (jointList_.size() <= 0)
00547 break;
00548 return jointList_.get(rowIndex).getName();
00549 case 2:
00550 if (currentSensor_ == null)
00551 break;
00552 return FORMAT2.format(Math.toDegrees(currentSensor_.q[rowIndex]));
00553 case 3:
00554 if (currentRefAng_ == null)
00555 break;
00556 return FORMAT2.format(Math.toDegrees(currentRefAng_[rowIndex]));
00557 case 4:
00558 if (currentSensor_ == null || currentSensor_.u == null)
00559 break;
00560 return FORMAT2.format(currentSensor_.u[rowIndex]);
00561
00562 case 5:
00563 if (currentSvStat_ == null)
00564 break;
00565 if (_isPowerOn(currentSvStat_[rowIndex])){
00566 return "ON";
00567 }else{
00568 return "OFF";
00569 }
00570 case 6:
00571 if (currentSvStat_ == null)
00572 break;
00573 if (_isServoOn(currentSvStat_[rowIndex])) {
00574 return "ON";
00575 } else return "OFF";
00576 case 7:
00577 if (currentSvStat_ == null) break;
00578 int alarm = _getAlarm(currentSvStat_[rowIndex]);
00579 return String.format("%03x", alarm);
00580 case 8:
00581 if (currentSvStat_ == null) break;
00582 int temp = _getDriverTemperature(currentSvStat_[rowIndex]);
00583 if (temp == 0){
00584 return "---";
00585 }else{
00586 return String.format("%3d", temp);
00587 }
00588 case 9:
00589 break;
00590 case 10:
00591 break;
00592 default:
00593 break;
00594 }
00595 return "---";
00596 }
00597
00598 public void addListener(ILabelProviderListener listener) {
00599 }
00600
00601 public void dispose() {
00602 }
00603
00604 public boolean isLabelProperty(Object element, String property) {
00605 return false;
00606 }
00607
00608 public void removeListener(ILabelProviderListener listener) {
00609 }
00610
00611 public Color getBackground(Object element, int columnIndex) {
00612 int rowIndex = ((Integer)element).intValue();;
00613 GrxBaseItem bitem = manager_.focusedItem();
00614 if (currentModel_ != null && bitem instanceof GrxLinkItem) {
00615 if (jointList_.get(rowIndex) == (GrxLinkItem)bitem) {
00616 return yellow_;
00617 }
00618 }
00619 return white_;
00620 }
00621
00622 public Color getForeground(Object element, int columnIndex) {
00623 int rowIndex = ((Integer)element).intValue();
00624 if (currentSvStat_ == null) return black_;
00625 switch (columnIndex) {
00626 case 0:
00627 if (!_isCalibrated(currentSvStat_[rowIndex])) {
00628 return red_;
00629 }
00630 break;
00631 case 1:
00632 case 2:
00633 if (jointList_.size() <= 0)
00634 break;
00635 GrxLinkItem info = jointList_.get(rowIndex);
00636 if (info.llimit_ != null && info.ulimit_ != null && info.llimit_[0] < info.ulimit_[0]
00637 && (currentSensor_.q[rowIndex] <= info.llimit_[0] || info.ulimit_[0] <= currentSensor_.q[rowIndex])) {
00638 return red_;
00639 }
00640 break;
00641 case 5:
00642 if (_isPowerOn(currentSvStat_[rowIndex]))
00643 return blue_;
00644 break;
00645 case 6:
00646 if (_isServoOn(currentSvStat_[rowIndex]))
00647 return red_;
00648 break;
00649 case 7:
00650 if (_getAlarm(currentSvStat_[rowIndex]) != 0)
00651 return red_;
00652 break;
00653 case 8:
00654 if (_getDriverTemperature(currentSvStat_[rowIndex]) > 60){
00655 return red_;
00656 }
00657 break;
00658 default:
00659 break;
00660 }
00661
00662 return black_;
00663 }
00664
00665 public Font getFont(Object element, int columnIndex) {
00666 int rowIndex = ((Integer)element).intValue();
00667
00668 switch (columnIndex) {
00669 case 0:
00670 if (currentSvStat_ != null
00671 && !_isCalibrated(currentSvStat_[rowIndex])) {
00672 return boldFont_;
00673 }
00674 break;
00675 case 2:
00676 if(jointList_.isEmpty())
00677 break;
00678 GrxLinkItem info = jointList_.get(rowIndex);
00679 if (info != null && info.llimit_ != null && info.ulimit_ != null && info.llimit_[0] < info.ulimit_[0]
00680 && (info.jointValue_ <= info.llimit_[0] || info.ulimit_[0] <= info.jointValue_)) {
00681 return boldFont_;
00682 }
00683 break;
00684 case 3:
00685 case 4:
00686 break;
00687 case 5:
00688 if (currentSvStat_ == null)
00689 break;
00690 if (_isPowerOn(currentSvStat_[rowIndex])) {
00691 return boldFont_;
00692 }
00693 break;
00694 case 6:
00695 if (currentSvStat_ == null)
00696 break;
00697 if (_isServoOn(currentSvStat_[rowIndex])) {
00698 return boldFont_;
00699 }
00700 break;
00701 case 7:
00702 if (currentSvStat_ == null)
00703 break;
00704 if (_getAlarm(currentSvStat_[rowIndex]) != 0){
00705 return boldFont_;
00706 }
00707 break;
00708 case 8:
00709 if (currentSvStat_ == null)
00710 break;
00711 if (_getDriverTemperature(currentSvStat_[rowIndex]) > 60){
00712 return boldFont_;
00713 }
00714 break;
00715 default:
00716 break;
00717 }
00718 return plainFont_;
00719 }
00720
00721 private boolean _isServoOn(int state) {
00722 if ((state & SERVO_STATE_MASK) != 0){
00723 return true;
00724 }else{
00725 return false;
00726 }
00727 }
00728 private boolean _isPowerOn(int state) {
00729 if ((state & POWER_STATE_MASK) != 0){
00730 return true;
00731 }else{
00732 return false;
00733 }
00734 }
00735 private boolean _isCalibrated(int state) {
00736 if ((state & CALIB_STATE_MASK) != 0){
00737 return true;
00738 }else{
00739 return false;
00740 }
00741 }
00742 private int _getAlarm(int state){
00743 return (state&SERVO_ALARM_MASK)>>SERVO_ALARM_SHIFT;
00744 }
00745 private int _getDriverTemperature(int state){
00746 return (state&DRIVER_TEMP_MASK)>>DRIVER_TEMP_SHIFT;
00747 }
00748
00749
00750 }
00751
00752 class SensorTableLabelProvider implements ITableLabelProvider,ITableColorProvider,ITableFontProvider{
00753
00754 public Image getColumnImage(Object element, int columnIndex) {
00755 return null;
00756 }
00757
00758 public String getColumnText(Object element, int columnIndex) {
00759 int rowIndex = ((Integer)element).intValue();
00760 int numForce = 0;
00761 if(forceName_!=null)
00762 numForce = forceName_.length;
00763 int numAccel = 0;
00764 if(accName_!=null)
00765 numAccel = accName_.length;
00766 int numGyro = 0;
00767 if(gyroName_!=null)
00768 numGyro = gyroName_.length;
00769 boolean showzmp = numForce >= 2 && forceName_[0].equals("rfsensor") && forceName_[1].equals("lfsensor");
00770 if (columnIndex == 0) {
00771 if (rowIndex < numForce)
00772 return forceName_[rowIndex] + " [N],[Nm]";
00773 if (rowIndex < numForce+numAccel)
00774 return accName_[rowIndex-numForce] + " [m/s^2]";
00775 if (rowIndex < numForce+numAccel+numGyro)
00776 return gyroName_[rowIndex-numForce-numAccel] + " [rad/s]";
00777 if (rowIndex == numForce+numAccel+numGyro && showzmp)
00778 return "ZMP(right)[m]";
00779 return "Voltage [V]";
00780 }
00781
00782 if (currentSensor_ == null) return "---";
00783 if (rowIndex < numForce){
00784 if (currentSensor_.force == null || columnIndex >= 7){
00785 return "---";
00786 }else{
00787 return FORMAT2.format(currentSensor_.force[rowIndex][columnIndex - 1]);
00788 }
00789 }else if (rowIndex < numForce+numAccel){
00790 if (currentSensor_.accel == null || columnIndex >= 4){
00791 return "---";
00792 }else{
00793 return FORMAT2.format(currentSensor_.accel[rowIndex - numForce][columnIndex - 1]);
00794 }
00795 }else if (rowIndex < numForce+numAccel+numGyro){
00796 if (currentSensor_.rateGyro == null || columnIndex >= 4){
00797 return "---";
00798 }else{
00799 return FORMAT2.format(currentSensor_.rateGyro[rowIndex - numForce - numAccel][columnIndex - 1]);
00800 }
00801 }else if (rowIndex == numForce+numAccel+numGyro && showzmp){
00802 if (columnIndex == 1){
00803 if (currentSensor_.force[0][2]>25){
00804 return FORMAT2.format(-currentSensor_.force[0][4]/currentSensor_.force[0][2]);
00805 }else{
00806 return FORMAT2.format(0);
00807 }
00808 }else if (columnIndex == 2){
00809 if (currentSensor_.force[0][2]>25){
00810 return FORMAT2.format(currentSensor_.force[0][3]/currentSensor_.force[0][2]);
00811 }else{
00812 return FORMAT2.format(0);
00813 }
00814 }else if(columnIndex == 3){
00815 return "ZMP(left)[m]";
00816 }else if (columnIndex == 4){
00817 if (currentSensor_.force[1][2]>25){
00818 return FORMAT2.format(-currentSensor_.force[1][4]/currentSensor_.force[1][2]);
00819 }else{
00820 return FORMAT2.format(0);
00821 }
00822 }else if (columnIndex == 5){
00823 if (currentSensor_.force[1][2]>25){
00824 return FORMAT2.format(currentSensor_.force[1][3]/currentSensor_.force[1][2]);
00825 }else{
00826 return FORMAT2.format(0);
00827 }
00828 }else{
00829 return "---";
00830 }
00831 }else{
00832 if(currentPower_ == null || columnIndex >= 4)
00833 return "---";
00834 if(columnIndex == 2){
00835 return "Current[A]";
00836 }else{
00837 return FORMAT1.format(currentPower_[columnIndex/2]);
00838 }
00839 }
00840 }
00841
00842 public void addListener(ILabelProviderListener listener) {
00843 }
00844
00845 public void dispose() {
00846 }
00847
00848 public boolean isLabelProperty(Object element, String property) {
00849 return false;
00850 }
00851
00852 public void removeListener(ILabelProviderListener listener) {
00853 }
00854
00855 public Color getBackground(Object element, int columnIndex) {
00856 int rowIndex = ((Integer)element).intValue();;
00857 GrxBaseItem bitem = manager_.focusedItem();
00858 int numForce = 0;
00859 if(forceName_!=null)
00860 numForce = forceName_.length;
00861 int numAccel = 0;
00862 if(accName_!=null)
00863 numAccel = accName_.length;
00864 int numGyro = 0;
00865 if(gyroName_!=null)
00866 numGyro = gyroName_.length;
00867 if (rowIndex < numForce){
00868 if (bitem instanceof GrxSensorItem) {
00869 if (forceName_[rowIndex].equals(((GrxSensorItem)bitem).getName())) {
00870 return yellow_;
00871 }
00872 }
00873 }else if (rowIndex < numForce + numAccel){
00874 if (bitem instanceof GrxSensorItem) {
00875 if (accName_[rowIndex-numForce].equals(((GrxSensorItem)bitem).getName())) {
00876 return yellow_;
00877 }
00878 }
00879 }else if (rowIndex < numForce + numAccel + numGyro){
00880 if (bitem instanceof GrxSensorItem) {
00881 if (gyroName_[rowIndex-numForce-numAccel].equals(((GrxSensorItem)bitem).getName())) {
00882 return yellow_;
00883 }
00884 }
00885 }
00886 return white_;
00887 }
00888
00889 public Color getForeground(Object element, int columnIndex) {
00890 return black_;
00891 }
00892
00893 public Font getFont(Object element, int columnIndex) {
00894 return plainFont_;
00895 }
00896
00897 }
00898
00899 public void shutdown() {
00900 manager_.removeItemChangeListener(this, GrxModelItem.class);
00901 manager_.removeItemChangeListener(this, GrxWorldStateItem.class);
00902 if(currentWorld_ != null){
00903 currentWorld_.deleteObserver(this);
00904 currentWorld_.deletePosObserver(this);
00905 }
00906 modelList_ = manager_.<GrxModelItem>getSelectedItemList(GrxModelItem.class);
00907 if(!modelList_.isEmpty()){
00908 Iterator<GrxModelItem> it = modelList_.iterator();
00909 while(it.hasNext())
00910 it.next().deleteObserver(this);
00911
00912 }
00913 }
00914
00915 public void updateTableFont(){
00916 plainFont_ = Activator.getDefault().getFont("robot_state_plain");
00917 boldFont_ = Activator.getDefault().getFont("robot_state_bold");
00918
00919 for(int i=0;i<viewers_.length;i++){
00920 viewers_[i].getTable().setFont(Activator.getDefault().getFont("preference_table"));
00921 viewers_[i].refresh();
00922 }
00923
00924 }
00925 }