Exemplo n.º 1
0
	bool tryRecover()
	{
		//检查自动恢复路点标记
		if(Recover != gpState)
			return false;
		if(true != triggerRecover)
			return false;
		MBUG("GP@%d : gpState = {%s} (robixStatus = %s)\n", myFrameid, "Recover", "N/A");
	//	gp.intit(static_cast<unsigned int>(gpInput->reco_point.recovery),gps);
		
		if(RNDF_ERROR == gp.stat())
				{
					MBUG("\t<tryRecover> GP recover faild at %d.\n", gpInput->reco_point.recovery);
					gpEvent->event = E_FAILURE;//发送错误事件
					return 1;
				}
		
		MBUG("\t<tryRecover> GP has recovered from %d.\n", gpInput->reco_point.recovery);
		//如果恢复点位不是1号点,则发送直接切换到道路跟踪
		if(0 == gpInput->reco_point.recovery)
		{
			gpEvent->event = E_WAIT_LIGHT;
			MBUG("\t<tryRecover> GP Send Event = E_WAIT_LIGHT, No Recovery Information(start from first)!\n");
		}
		else if(1 == gpInput->reco_point.recovery)
		{
			gpEvent->event = E_WAIT_LIGHT;
			MBUG("\t<tryRecover> GP Send Event = E_WAIT_LIGHT, Specific Recovery Information(first point)!\n");
		}
		else
		{
			gpEvent->event = E_RUN_ROAD;
			MBUG("\t<tryRecover> GP Send Event = E_RUN_ROAD, Specific Recovery Infomation!\n");
		}
		//执行完毕后切回Run,并返回
		triggerRecover = false;
		gpState = Run;
		return true;
	}
Exemplo n.º 2
0
	int Global_Plan(GP_FUNC_INPUT * pInput, GP_INFO * pData, GP_LOCAL_DATA * pLocal)
	{
		
		/* 0 全局规划预处理 */
		/* 0.1 调用计数器自增 */
		if(myFrameid==1)
			MBUG("WARNING:this shell is for simulate only, no use any more\n");
		++myFrameid;
		pData->header.id = myFrameid;
		MBUG("\n*************************FRAME_ID %d***********************************\n", myFrameid);
		/* 0.2 检查规划器状态 */
		
		if(RNDF_ERROR == gp.stat())
			{
				MBUG("GP@%d : GP is INVALID(RNDF STATUS ERROR, CHECK FILE task OR RECOVERY POINT)!\n", myFrameid);
				MBUG("**********************************************************************\n");
				return 1;
			}
		
		/* 0.3 预置全局规划信息,更新动态数据 */
		
		gpInput = pInput;
		gpOutput = pData;
		gpEvent = pLocal;
		memcpy(&pData->state, &pInput->state, sizeof(STATE));
		gps[0] = pInput->state.pos.com_coord.x;
		gps[1] = pInput->state.pos.com_coord.y;
		gps[2] = pInput->state.pos.yaw*(180.0*1e-8)/3.1415926535898;

#ifdef USING_ROBIX4
		rbxReadStatus(&robixStatus);
#else
		if(myFrameid==1)
			robixStatus=S_INVALID;
		else
			robixStatus=S_ROAD_NAV;
#endif
		/* 1 执行全局规划 */
		switch(robixStatus)
		{
		case S_INVALID:
			MBUG("GP@%d : gpState = {%s} (robixStatus = %s)\n", myFrameid, "N/A", "S_INVALID");
			tryRecover();
			MBUG("**********************************************************************\n");
			break;
		case S_WAIT:
			MBUG("GP@%d : gpState = {%s} (robixStatus = %s)\n", myFrameid, "N/A", "S_WAIT");
			tryRecover();
			MBUG("**********************************************************************\n");
			break;
		case S_WAIT_LIGHT:
			MBUG("GP@%d : gpState = {%s} (robixStatus = %s)\n", myFrameid, "Run", "S_WAIT_LIGHT");
			gp.context(gps,gpOutput);
			MBUG("**********************************************************************\n");
			break;
		case S_ROAD_NAV:
			MBUG("GP@%d : gpState = {%s} (robixStatus = %s)\n", myFrameid, "Run", "S_ROAD_NAV");
			trySwitch();
			MBUG("**********************************************************************\n");
			break;
		case S_CROSS_UND:
			MBUG("GP@%d : gpState = {%s} (robixStatus = %s)\n", myFrameid, "Run", "S_CROSS_UND");
			trySwitch();
			MBUG("**********************************************************************\n");
			break;
		case S_STRAIGHT:
			MBUG("GP@%d : gpState = {%s} (robixStatus = %s)\n", myFrameid, "Run", "S_STRAIGHT");
			trySwitch();
			MBUG("**********************************************************************\n");
			break;
		case S_LEFT:
			MBUG("GP@%d : gpState = {%s} (robixStatus = %s)\n", myFrameid, "Run", "S_LEFT");
			trySwitch();
			MBUG("**********************************************************************\n");
			break;
		case S_RIGHT:
			MBUG("GP@%d : gpState = {%s} (robixStatus = %s)\n", myFrameid, "Run", "S_RIGHT");
			trySwitch();
			MBUG("**********************************************************************\n");
			break;
		case S_UTURN:
			MBUG("GP@%d : gpState = {%s} (robixStatus = %s)\n", myFrameid, "Run", "S_UTURN");
			trySwitch();
			MBUG("**********************************************************************\n");
			break;
		case S_PARKING:
			MBUG("GP@%d : gpState = {%s} (robixStatus = %s)\n", myFrameid, "Run", "S_PARKING");
			trySwitch();
			MBUG("**********************************************************************\n");
			break;
		case S_FREE_RUN:
			MBUG("GP@%d : gpState = {%s} (robixStatus = %s)\n", myFrameid, "Run", "S_FREE_RUN");
			trySwitch();
			MBUG("**********************************************************************\n");
			break;
		case S_TASK_OVER:
			MBUG("GP@%d: gpState = {%s} (robixStatus = %s)\n", myFrameid, "Off", "S_TASK_OVER");
			gpState = Off;
			gp.context(gps,gpOutput);
			MBUG("**********************************************************************\n");
			break;
		default:
			break;
		}
		return 0;
	}