# Important part of Kibo RPC Astrobee Code
###### tags: `Kibo RPC` `java` `ROS`
## Init

    Kinematics k = new DefaultKinematics();
    Time t = new Time();
    HashMap<Pair<Point,Quaternion>,Integer>HM=new HashMap<>(); //存放QR code拍攝點
    private Point P1= new Point(11.5,-5.7,4.5);
    private Quaternion Q1=new Quaternion(0,0,0,1);
    private Point P2= new Point(11,-6,5.55);
    private Quaternion Q2 = new Quaternion(0,-0.7071068F,0,0.7071068F);
    private Point P3= new Point(11,-5.5,4.33);
    private Quaternion Q3 = new Quaternion(0,0.7071068F,0,0.7071068F);
    private Point P4= new Point(10.30,-7.5,4.7);
    private Quaternion Q4=new Quaternion(0,0,1,0);
    //private Point P5= new Point(11.5,-8,5);
    private Point P5= new Point(11.3586,-8,5); //modified
    private Quaternion Q5 = new Quaternion(0,0,0,1);
    private Point P6= new Point(11,-7.7,5.55);
    private Quaternion Q6 = new Quaternion(0,-0.7071068F,0,0.7071068F);
    private QRcodeScanner qRcodeScanner = new QRcodeScanner();
    private ARtag AR= new ARtag();

## Procedure

    t.getTime(1,1);
    moveToWrapper(11.50,-5.70,4.50,0,0,0,1);
    t.getTime(1,2);
    
    moveToWrapper(11.2012,-9.2729,4.959,0,0,-0.7071068,(float)0.7071068); //modified
    t.getTime(17,2);
    moveToWrapper(p3[0],p3[1],p3[2],p3[3],p3[4],p3[5],W);
       
    String ID = null;
    int cnti = 1;
    while((ID == null || ID.length() == 0) && cnti < 30)
    {
            ID = AR.DetectMarker(api.getMatNavCam());
            try{
                Thread.sleep(500);
            } catch (Exception e){
                Log.d("sleep", e.getMessage());
            }
            if(ID != null)
                break;
            if((k.getPosition().getX()-p3[0])*(k.getPosition().getX()-p3[0])+(k.getPosition().getY()-p3[1])*(k.getPosition().getY()-p3[1])+(k.getPosition().getZ()-p3[2])*(k.getPosition().getZ()-p3[2])>0.07001)
                moveToWrapper(p3[0],p3[1],p3[2],p3[3],p3[4],p3[5],W);
            cnti++;
    }
    api.judgeSendDiscoveredAR(ID);        
    RelativemoveToWrapper(-0.0125,0.04021,0.16992,p3[3],p3[4],p3[5],W);
    double tpx = k.getPosition().getX();
    double tpy = k.getPosition().getY();
    double tpz = k.getPosition().getZ();
    float tqx = k.getOrientation().getX();
    float tqy = k.getOrientation().getY();
    float tqz = k.getOrientation().getZ();
    float tqw = k.getOrientation().getW();
    for(int i=0;i<10;i++) 
    {
            if((k.getPosition().getX() - tpx) * (k.getPosition().getX() - tpx) + (k.getPosition().getY()-tpy) * (k.getPosition().getY()-tpy) + (k.getPosition().getZ() - tpz) * (k.getPosition().getZ() - tpz) > 0.1)
                moveToWrapper(tpx,tpy,tpz,tqx,tqy,tqz,tqw);

            api.laserControl(true);
            try{
                Thread.sleep(500);
            } catch (Exception e){
                Log.d("sleep", e.getMessage());
            }
            api.laserControl(false);
    }
    t.PrintTimeFromVector();

## MultiThreading

class multiDecode implements Runnable {
        private Bitmap bm;
        private int no;
        public multiDecode(Bitmap temp,int idx) {
            bm = temp;
            no = idx;
        }
        private String _value = null;
        public void run() {
            String TAG = "getQR";
            while(_value == null){
                _value = QRcodeScanner.decodeQRImage(bm);

                if (_value != null && _value.length() > 1) {
                    try{
                        Thread.sleep(1000); //modified sleeping time
                    } catch (Exception e){
                        Log.d(TAG, e.getMessage());
                    }
                    api.judgeSendDiscoveredQR(no, _value);
                    try{
                        Thread.sleep(1000); // modified sleeping time
                    } catch (Exception e){
                        Log.d(TAG, e.getMessage());
                    }
                    String str;
                    str = _value.replaceAll("[^-0-9.]", "");
                    double val = Double.parseDouble(str);
                    Log.d(TAG, "\n\nQRno="+no+"string="+_value+"val= "+val+"iterate times="+"\n\n");
                    p3[no] = val;
                    break;
                }
            }
        }
    }

## RelativeMoveToWrapper (Fix with thread.sleep())

public void RelativemoveToWrapper(double pos_x, double pos_y, double pos_z,
                                      double qua_x, double qua_y, double qua_z,
                                      float qua_w){

        final Point point = new Point(pos_x, pos_y, pos_z);
        final Quaternion quaternion = new Quaternion((float)qua_x, (float)qua_y,
                (float)qua_z, (float)qua_w);

        api.relativeMoveTo(point, quaternion, true);
        try{
            Thread.sleep(200);
        }catch(Exception e){
            Log.e("moveToWrapper",e.getMessage());
        }
    }

## Quaternion Orientation: Computing W

public float w()
    {
        return (float)Math.sqrt(1-p3[3]*p3[3]-p3[4]*p3[4]-p3[5]*p3[5]);
    }

## AR Tag

public String DetectMarker(Mat m)
{
        Mat ids = new Mat();
        float markerLength = 0.05F;
        List<Mat> corners = new ArrayList<>();
        int j=-1;
        Mat distCoeffs = new Mat();
        Mat rvecs = new Mat();
        Mat tvecs = new Mat();
        Aruco.detectMarkers(m,Aruco.getPredefinedDictionary(Aruco.DICT_5X5_250),corners,ids);
        String id = Arrays.toString(ids.get(0, 0));
        id = id.replaceAll("[^-0-9.]", "");
        if(id.indexOf(".") > 0)
        {
            id = id.replaceAll("0 ?$", "");//去掉多餘的0
            id = id.replaceAll("[.]$", "");//如最後一位是.則去掉
        }
        Log.d("AR TAG IDS : ", id);
        return id;
}

## QR code

public static String decodeQRImage(Bitmap blackWhite) 
{
        String TAG="decodeQRImage";
        //Log.d(TAG,"Begin");
        int width = blackWhite.getWidth(), height = blackWhite.getHeight();
        int[] pixels = new int[width * height];
        blackWhite.getPixels(pixels, 0, width, 0, 0, width, height);
        RGBLuminanceSource source = new RGBLuminanceSource(width, height, pixels);
        BinaryBitmap bBitmap = new BinaryBitmap(new HybridBinarizer(source));
        MultiFormatReader reader = new MultiFormatReader();
        try {
            com.google.zxing.Result result = reader.decode(bBitmap);
            return result.getText();
        } catch (NotFoundException e) {
            HashMap<DecodeHintType, Object> map = new HashMap<>();
            map.put(DecodeHintType.PURE_BARCODE, Boolean.TRUE);
            com.google.zxing.Result result = null;
            try {
                result = reader.decode(bBitmap, map);
            } catch (NotFoundException ex) {
                ex.printStackTrace();
            }
            Log.d(TAG,"end");
            return result.getText();
        }
}