rangesensorbelt.java

一款机器人仿真软件,功能上与microsoft robotics studio有些相似,但基于Java平台.突出特点是给出了传感器仿真,如声纳,激光等.

                                    intersect = true;
                                }
                            }
                        }
                    }
                }
                hits[s] =intersect;
                oneHasHit |= intersect; 
                if (intersect){
                    
                    measurements[s]= minDist;
                    //System.out.println ("Sensor "+s+"="+(minDist));
                }
           }
        }
        
    }

 
    /**
     * Returns the last measure collected for the individual sensor. Measurement is made from the circle perimeter.
     * @param sensorNum num of the sensor.
     * @return the range measurment.
      */
    public double getMeasurement(int sensorNum){
       return measurements[sensorNum];
   }

    /**
     * Returns the averaged measure of the sensors situated in the front quadrant: [-PI/4,PI/4].
     * @return the averaged measurment.
      */
    public double getFrontQuadrantMeasurement() {
        return (getQuadrantMeasurement(0, Math.PI / 4) + getQuadrantMeasurement(
                7*Math.PI / 4, Math.PI * 2)) / 2.0;
    }
    /**
     * Returns the averaged measure of the sensors situated in the front left quadrant: [0,PI/4].
     * @return the averaged measurment.
      */
    public double getFrontLeftQuadrantMeasurement() {
        return getQuadrantMeasurement(0, Math.PI / 2); 
    }
    /**
     * Returns the averaged measure of the sensors situated in the front right quadrant: [3PI/2,2*PI].
     * @return the averaged measurment.
      */
    public double getFrontRightQuadrantMeasurement() {
        return getQuadrantMeasurement(3*Math.PI/2.0, 2*Math.PI); 
    }
    /**
     * Returns the averaged measure of the sensors situated in the left quadrant: [PI/4,PI*3/4].
     * @return the averaged measurment.
      */
    public double getLeftQuadrantMeasurement() {
        return getQuadrantMeasurement(Math.PI / 4,3*Math.PI/4); 
    }
   
    /**
     * Returns the averaged measure of the sensors situated in the bacck left quadrant: [PI/2,PI].
     * @return the averaged measurment.
      */
    public double  getBackLeftQuadrantMeasurement() {
        return getQuadrantMeasurement(Math.PI / 2,Math.PI); 
    }
    
    /**
     * Returns the averaged measure of the sensors situated in the back quadrant: [3PI/4,4*PI/4].
     * @return the averaged measurment.
      */
   public double getBackQuadrantMeasurement() {
        return getQuadrantMeasurement(3*Math.PI / 4,5*Math.PI/4); 
    }
   /**
    * Returns the averaged measure of the sensors situated in the back right quadrant: [PI,3*PI/2].
    * @return the averaged measurment.
     */
   public double getBackRightQuadrantMeasurement() {
       return getQuadrantMeasurement(Math.PI ,3*Math.PI/2); 
   }
   /**
    * Returns the averaged measure of the sensors situated in the right quadrant: [5*PI/4,7*PI/4].
    * @return the averaged measurment.
     */public double getRightQuadrantMeasurement() {
       return getQuadrantMeasurement(5*Math.PI/4 ,7*Math.PI/4); 
   }
    /**
     * Returns the averaged measure of the sensors situated in quadrant [minAngle,maxAngle].
     * @param minAngle in radians the right limit of the quadrant.
     * @param maxAngle in radians the left limit of the quadrant.
     * @return the averaged measurment.
      */
    public double getQuadrantMeasurement(double minAngle,double maxAngle){
        double sum=0.0,n=0;
        for (int i=0;i< nbSensors;i++){
            if ((angles[i]>= minAngle) &&(angles[i]<= maxAngle)) {
                if (hits[i]){
                    n += 1.0 ;
                   sum += measurements[i];
                }
            }
        } 
        if (n==0)return Double.POSITIVE_INFINITY;
        else return sum/n;
   }
 
    /**
     * Returns number of sensor hits in the front quadrant: [-PI/4,PI/4].
     * @return the number of hits.
      */
    public int getFrontQuadrantHits() {
        return (getQuadrantHits(0, Math.PI / 4) + getQuadrantHits(
                7*Math.PI / 4, Math.PI * 2)) ;
    }
    /**
     * Returns  number of sensor hits in the front left quadrant: [0,PI/4].
     * @return the  number of hits.
      */
    public int getFrontLeftQuadrantHits() {
        return getQuadrantHits(0, Math.PI / 2); 
    }
    /**
     * Returns  number of sensor hits in the front right quadrant: [3PI/2,2*PI].
     * @return the  number of hits.
      */
    public int getFrontRightQuadrantHits() {
        return getQuadrantHits(3*Math.PI/2.0, 2*Math.PI); 
    }
    /**
     * Returns  number of sensor hits in the left quadrant: [PI/4,PI*3/4].
     * @return the  number of hits.
      */
    public int getLeftQuadrantHits() {
        return getQuadrantHits(Math.PI / 4,3*Math.PI/4); 
    }
    /**
     * Returns  number of sensor hits in the back left quadrant: [PI/2,PI].
     * @return the  number of hits.
      */
    public int  getBackLeftQuadrantHits() {
        return getQuadrantHits(Math.PI / 2,Math.PI); 
    }
    /**
     * Returns  number of sensor hits in the back  quadrant: [3PI/4,5PI/4].
     * @return the  number of hits.
      */
    public int getBackQuadrantHits() {
        return getQuadrantHits(3*Math.PI / 4,5*Math.PI/4); 
    }
    /**
     * Returns  number of sensor hits in the back right quadrant: [PI,3PI/2].
     * @return the  number of hits.
      */
    public int getBackRightQuadrantHits() {
        return getQuadrantHits(Math.PI ,3*Math.PI/2); 
    }
    /**
     * Returns  number of sensor hits in the right quadrant: [5PI/4,7PI/4].
     * @return the  number of hits.
      */
    public int getRightQuadrantHits() {
        return getQuadrantHits(5*Math.PI/4 ,7*Math.PI/4); 
    }
    /**
     * Returns number of hits  in quadrant [minAngle,maxAngle].
     * @param minAngle in radians the right limit of the quadrant.
     * @param maxAngle in radians the left limit of the quadrant.
     * @return the  number of hits.
      */
    public int getQuadrantHits(double minAngle,double maxAngle){
        int n=0;
        for (int i=0;i< nbSensors;i++){
            if ((angles[i]>= minAngle) &&(angles[i]<= maxAngle)) {
                if (hits[i]) n++;
            }
        } 
        return n;
   }
    /**
     * Returns the hit state of the sensor.
	 * @param sensorNum num of the sensor.
	 * @return true if the sensor ray has hit an obstacle 
	 */
    public boolean hasHit(int sensorNum){
       return hits[sensorNum];
   }
    /**
     * Returns true if one of the sensors has hit.
	 * @return true if one  ray has hit an obstacle 
	 */
    public boolean oneHasHit(){
       return oneHasHit;
   }
   /**
    * Return the number of individual sensor in the belt.
    * @return the number of sensors.
    */
   public int getNumSensors(){ return nbSensors;}

   /**
    * Returns the angle of this sensor.
    * @param sensorNum - num of the sensor.
   * @return the angle in radians.
   */
   public double getSensorAngle(int sensorNum){
       return angles[sensorNum];
   }

   /**
    * Returns the maximum sensing range in meters. 
    * @return the maximum range.
   */
   public float getMaxRange(){ return maxRange;}
   
   public JPanel createInspectorPanel(){
       return new RangeSensorBeltJPanel();
   }

 
   /**
    * A JPanel Inner class for displaying the sensor belt rays in 2d. 
    */
    private class RangeSensorBeltJPanel extends JPanel{
   
    		private static final long serialVersionUID = 1L;
		Font font;
        int lineSize=8;
        DecimalFormat format; 
        final static int IMAGE_SIZEX = 200;
        final static int IMAGE_SIZEY = 100;
           public RangeSensorBeltJPanel(){
            Dimension d= new Dimension(IMAGE_SIZEX,IMAGE_SIZEY);
           setPreferredSize(d);
           setMinimumSize(d);
           font = new Font("Arial",Font.PLAIN,lineSize-1);
           // display format for numbers
           format = new DecimalFormat();
           format.setMaximumFractionDigits(3);
           format.setMinimumFractionDigits(2);
           format.setPositivePrefix("");
           format.setMinimumIntegerDigits(1);
       }
        /** Caution not synchronised */
           
       protected void paintComponent( Graphics g){
           super.paintComponent(g);
           g.setFont(font);
         //  Color c;
           g.setColor(Color.LIGHT_GRAY);
           g.fillRect(0,0,IMAGE_SIZEX,IMAGE_SIZEY);
           g.setColor(Color.GRAY);
           int x =IMAGE_SIZEX/2;int y =0;
           for (int i=0;i< nbSensors;i++){
                y += lineSize;
                if (y > IMAGE_SIZEY){
                    y = lineSize; x+= 50;
                }
               if(type == TYPE_BUMPER)
                   g.drawString("["+i+"] "+hits[i],x,y);
               else
                   g.drawString("["+i+"] "+format.format(measurements[i]),x,y);
           }     	
           int cx,cy;
           cx=cy=IMAGE_SIZEX/4;
           // display factor
           float f = (float)cx/(float)(maxRange+radius);
           int r = (int)(radius*f);
           g.setColor(Color.BLUE);
           g.drawOval(cx-r,cy-r,2*r,2*r);
           g.drawLine(cx,cy,cx+r,cy);
           // draw each ray
            for (int i=0;i< nbSensors;i++){
              
                int x1=	(int)(positions[i].x *f);
                int y1= (int)(positions[i].z *f);
                if( type == TYPE_BUMPER){
                    g.setColor(Color.BLUE);	
                    g.drawRect(cx+x1-1,cy+y1-1,2,2);
                }   
                g.setColor(Color.RED);
                if (hits[i]){
                   double ratio =  measurements[i]/directions[i].length();
                   
                    int x2=	x1 +(int)(directions[i].x *ratio*f);
                    int y2= y1+(int)(directions[i].z *ratio*f);
                   if( type == TYPE_BUMPER){
                       g.fillOval(cx+x1-1,cy+y1-1,3,3);
                   }else
                   g.drawLine( cx+x1, cy+y1,cx+x2,cy+y2);
               }
           }
       }       
   }
}