// 判断路径起点,即当前位置能否找到校正路标 bool CPathAgent::StartCalibCheck(void) { TPose2D tpose = g_pGyro->ReadPose(); // 机器人当前位置的大地坐标 CPose2D robot(tpose); CPose2D center2robot(m_dCalibCenterDistance, 0, 0); // 查找范围的中心点在机器人坐标系内的坐标 CPose2D center = robot + center2robot; // 查找范围的中心点在大地坐标系内的坐标 // 遍历路标,找到距离0.4米内的路标 for (int j=0; j<m_iLandMarkNum; j++) { TPose2D Land = m_pLandMarkPose[j]; double dis = center.distance2DTo(Land.x, Land.y); if (dis <= 0.5) // 0.4m半径的圆范围 { //// 这个路标点满足基本的距离条件,需要进一步判断是否在矩形ROI区域 //CPoint2D Land2Center(Land.x - center.x(), Land.y - center.y()); // 路标在center坐标系的坐标 //CPose2D TurnPose(0,0,-center.phi()); // 旋转到机器人朝向的center坐标系 //CPoint2D newLand2Center = TurnPose + Land2Center; //if (abs(newLand2Center.x()) < 0.2) // 在矩形ROI区域 // return true; //else // return false; return true; } } return false; }