小车相关的

分手后的思念是犯贱 2022-06-12 23:22 145阅读 0赞

1.C相关的

就是那serial1 改成serial2

还有一个就是serialEvent1 改成serialEvent2

dealy\_time 调节速度的,越大越快

serial2表示新小车；

serial1表示旧小车

2.导航相关的

com.openrobot.app.navi.NagivatorChannelTask

public static NavigatorTask create(IRobot robot,IExecutor executor,NagivateDestination destination){
    		if(destination == null)
    			return null;
    		NavigatorTask task = null;
    		if(destination.navigatorMethod == null || destination.navigatorMethod.equals("")){
    			task = new NagivatorGridTask(robot,executor,destination);
    		}else if(destination.navigatorMethod.equalsIgnoreCase("grid")){
    			task = new NagivatorGridTask(robot,executor,destination);
    		}else if(destination.navigatorMethod.equalsIgnoreCase("channel")){
    			task = new NagivatorChannelTask(robot,executor,destination);
    		}else{
    			//task = new NagivatorGridTask(robot,executor,destination);
    			task = new NagivatorChannelTask(robot,executor,destination);
    		}
    		task.init();
    		return task;
    	}

3.关于Croass的加载

4.关于NagivatorChannelTask.java中的private Coordinate\[\] getRoadDirection(Road curRoad, Road nextRoad,boolean cur) 方法的修改

private Coordinate[] getRoadDirection(Road curRoad, Road nextRoad,boolean cur) {
    		// TODO Auto-generated method stub
    		Coordinate[] r1 = curRoad.getCenters().getCoordinates();
    		Coordinate[] r2 = nextRoad.getCenters().getCoordinates();
    		
    		double mindis = -1;
    		int r1Index = -1,r2Index = -1;
    		//找到r1和r2距离最近的两个点
    		for(int i=0;i<r1.length;i++){
    			for(int j=0;j<r2.length;j++){				
    				if(i!=0 && i != r1.length-1 && j!=0 && j != r2.length-1)continue;
    				
    				double dx = r1[i].x - r2[j].x;
    				double dy = r1[i].y - r2[j].y;
    				double dis = Math.sqrt(dx*dx+dy*dy);
    				
    				if(mindis == -1){
    					mindis = dis;
    					r1Index = i;
    					r2Index = j;
    				}else if(mindis > dis){
    					mindis = dis;
    					r1Index = i;
    					r2Index = j;
    				}
    			}
    		}
    		
    		
    		List<Coordinate> results = new ArrayList<Coordinate>(Arrays.asList(cur?r1:r2));
    		if(cur && r1Index == 0){		
    			Collections.reverse(results);			
    		}else if(!cur && r2Index !=0){			
    			Collections.reverse(results);						
    		}
    		return results.toArray(new Coordinate[results.size()]);
    		
    	}

1.假如当前道路R1的坐标序列是P1,P2,P3；下一条道路R2的坐标序列式P4,P5,P6  
2.由于配置文件中记录的坐标点顺序是任意的，所以R1和R2两个道路相接的点有四种可能：P1和P4相接，P1和P6相接，P3和P4相接，P3和P6相接  
getRoadDirection()函数的作用是先找到四种相接方式到底是哪一种：对于R1来说，如果P1是接头点，则返回P3,P2,P1，否则返回P1,P2,P3  
这样返回的坐标序列能确保是行进方向一致  
对于R2来说，如果P4是接头点，则返回P4,P5,P6，否则将坐标序列颠倒下

5.关于X轴Y轴方向的确定

if(mainDirection == 0 || mainDirection == 1 || mainDirection == 2)\{//X为主方向

6.坐标轴方向的确定

目前这块总是假设Road是平行坐标轴的，且车子总是在Road附近

private int getMainDirectoin(){
    		//将道路方向简单规划为与坐标轴平行的方向(只有室内，且障碍物比较规则才可能)
    		//1 x轴正向，2 x轴反向，3 y轴正向，y 轴反向
    		int mainDirection = 0;		
    		if(curRoadAngle <= Math.PI/4 && curRoadAngle >= -1*Math.PI/4){//X正向
    			mainDirection = 1;					
    		}else if(curRoadAngle > Math.PI/4 && curRoadAngle <= 3*Math.PI/4){//y轴正向
    			mainDirection = 3;	
    		}else if(curRoadAngle > 3*Math.PI/4 && 3*Math.PI/4 <= Math.PI){//X反向
    			mainDirection = 2;
    		}else if(curRoadAngle > 3*Math.PI/4 ||  curRoadAngle <= -1*3*Math.PI/4){//x轴反向
    			mainDirection = 2;		
    		}else{//y 轴反向
    			mainDirection = 4;	
    		}
    		return mainDirection;
    	}

7.是否到达目的点的判断

public boolean isArrive(Polygon destinationRange) {
    		if(destinationRange == null)
    			return false;
    		StatePosition curPos = robot.getStateCollection().getValue(StateCollection.POSITION);
    		//Point pt = GeometryUtil.createPoint(curPos.getCoord());
    		//判断当前点和目的点的距离，距离小于10cm，就认为到达目的点了
    		boolean isArriveDestination = false;
    		//目的点的坐标点
    		Coordinate[] coordinates = destinationRange.getCoordinates();
    		for (int i = 0; i < coordinates.length; i++) {
    			if( twoCoordinatesIsAlike(curPos.getCoord(),coordinates[i]) ){
    				isArriveDestination = true;
    				break;
    			}
    		}
    		return isArriveDestination;
    	}
    	/**
    	 * 判断两个点，是不是相似。
    	 * 两个点的距离在15cm之内，就认为到达目的点了
    	 * @param a
    	 * @param b
    	 * @return
    	 */
    	private boolean twoCoordinatesIsAlike(Coordinate a,Coordinate b){
    		double juli = Math.sqrt( Math.pow((a.x - b.x), 2) + Math.pow((a.y - b.y), 2) );
    		return juli > 25 ? false : true;
    		
    	}

8.旋转之前，睡眠一会

//进行旋转
    			if(rotateAction!=null && rotateAction.angle>=10 && rotateAction.direction!=WalkCode.STOP){			
    				//旋转之前先睡眠一会
    				Common.sleep(2000);
    				rotateAction.execute();
    				//旋转完成也睡眠一会
    				Common.sleep(2000);
    			}

for(int i=0;i<actionQueue.size();i++){
    				IAction action = actionQueue.get(i);
    				RetCode retCode = null;
    				//旋转的话，先睡眠一会
    				if(action.getClass().isAssignableFrom(RotateAction.class)){
    					Common.sleep(2000);
    					retCode = action.execute();
    					Common.sleep(2000);
    				}else{ //不是旋转动作，直接执行
    					retCode = action.execute();
    				}
    				
    }

9.通过道路的时候，前进一半的距离，这样可以进行角度的修正

//移动,这个应该就是避障逻辑的修改了（注意是前进一半的距离）

MoveAction moveAction = new MoveAction(robot,WalkCode.FORWARD,mainDistance);

前进一半距离的时候，可能会造成前进的距离越来越短。所以进行简单的判断

要是前进的距离小于60cm，就直接前进60cm。不要前进一半的距离了。

StatePosition curPosition = robot.getStateCollection().getValue(StateCollection.POSITION);//当前位置信息
    			double curAngle = curPosition.getAngle(); //当前角度信息
    		
    			//将道路方向简单规划为与坐标轴平行的方向(只有室内，且障碍物比较规则才可能)
    			//1 x轴正向，2 x轴反向，3 y轴正向，4 y轴反向 ，这个地方不是很理解
    			int mainDirection = this.getMainDirectoin(); //得到当前小车的坐标轴方向
    			RotateAction rotateAction = computeRotateAction(mainDirection,curAngle); //根据当前角度和mainDirection，确定需要旋转的角度
    		
    			//计算该方向上当前点到当前道路终点的距离		
    			double mainDistance  = 0;
    			if(mainDirection == 0 || mainDirection == 1 || mainDirection == 2){//X为主方向 
    				mainDistance = Math.abs(curPosition.getCoord().x-curRoadPoints[curRoadPoints.length-1].x);
    			}else{
    				mainDistance = Math.abs(curPosition.getCoord().y-curRoadPoints[curRoadPoints.length-1].y);
    			}
    			
    			//尝试通过该条道路的条件
    			//当前目标点的坐标距离道路最后一个坐标点的距离小于5cm，就认为通过这条路径了
    			if(mainDistance <= 10){ //小于5厘米认为已经到了
    				return RetCode.COMPLETED;
    			}
    			
    			//进行旋转
    			if(rotateAction!=null && rotateAction.angle>=10 && rotateAction.direction!=WalkCode.STOP){			
    				//旋转之前先睡眠一会
    				Common.sleep(2000);
    				rotateAction.execute();
    				//旋转完成也睡眠一会
    				Common.sleep(2000);
    			}
    			//移动,这个应该就是避障逻辑的修改了
    			MoveAction moveAction = new MoveAction(robot,WalkCode.FORWARD,mainDistance/2);
    			RetCode retCode = moveAction.execute();

10.旋转角度

旋转角度的实现，跟电量和地面有很大的关系。这个只能依靠通过，提前确定好参数，然后通过配置文件来写入到C端的代码里面。

提前确定，旋转角度的C参数。