本文整理汇总了C++中WayPoints::begin方法的典型用法代码示例。如果您正苦于以下问题:C++ WayPoints::begin方法的具体用法?C++ WayPoints::begin怎么用?C++ WayPoints::begin使用的例子?那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类WayPoints
的用法示例。
在下文中一共展示了WayPoints::begin方法的3个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: shortCut
/**
* @brief Check if some point ahead on the road is closer (L2) than along the road, to take a shortcut
*
* @param innermodel ...
* @param road ...
* @param laserData ...
* @return bool
*/
bool ElasticBand::shortCut(InnerModel *innermodel, WayPoints &road, const RoboCompLaser::TLaserData &laserData)
{
//Compute distances from robot to all points ahead. If any of them is laser-visible and significantly shorter than de distance along de road, try it!
WayPoints::iterator robot = road.getIterToClosestPointToRobot();
WayPoints::iterator best = road.begin();
for (WayPoints::iterator it = robot + 1; it != road.end(); ++it)
{
//qDebug() << __FUNCTION__ << it->isVisible << (it->pos - robot->pos).norm2() << road.computeDistanceBetweenPointsAlongRoad(robot, it);
if ( it->isVisible )
{
if (road.computeDistanceBetweenPointsAlongRoad(robot, it) - (it->pos - robot->pos).norm2() > 300) //Half robot SACARRRR
{
qDebug() << __FUNCTION__ << "Candidato";
//Check if the robot passes through the straight line
if (checkCollisionAlongRoad(innermodel, laserData, road, robot, it, ROBOT_RADIUS))
{
//Is so remove all intermadiate points between robot and new subtarget
qDebug() << __FUNCTION__ << "Confirmado";
best = it;
}
}
}
else
break;
}
if (best != road.begin() and (robot + 1) != road.end())
road.erase(robot + 1, best);
return false;
}
示例2: checkIfNAN
/**
* @brief Check if any of the waypoints has nan coordinates
*
* @param road ...
* @return bool
*/
bool ElasticBand::checkIfNAN(const WayPoints &road)
{
for (auto it = road.begin(); it != road.end(); ++it)
if (std::isnan(it->pos.x()) or std::isnan(it->pos.y()) or std::isnan(it->pos.z()))
{
road.print();
return true;
}
return false;
}
示例3: update
bool ElasticBand::update(InnerModel *innermodel, WayPoints &road, const RoboCompLaser::TLaserData &laserData,
const CurrentTarget ¤tTarget, uint iter)
{
//qDebug() << __FILE__ << __FUNCTION__ << "road size"<< road.size();
if (road.isFinished() == true)
return false;
/////////////////////////////////////////////
//Tags all points in the road ar visible or blocked, depending on laser visibility. Only visible points are processed in this iteration
/////////////////////////////////////////////
//checkVisiblePoints(innermodel, road, laserData);
/////////////////////////////////////////////
//Check if there is a sudden shortcut to take
/////////////////////////////////////////////
//shortCut(innermodel, road, laserData);
/////////////////////////////////////////////
//Add points to achieve an homogenoeus chain
/////////////////////////////////////////////
addPoints(road, currentTarget);
/////////////////////////////////////////////
//Remove point too close to each other
/////////////////////////////////////////////
cleanPoints(road);
/////////////////////////////////////////////
//Compute the scalar magnitudes
/////////////////////////////////////////////
computeForces(innermodel, road, laserData);
/////////////////////////////////////////////
//Delete half the tail behind, if greater than 6, to release resources
/////////////////////////////////////////////
if (road.getIndexOfClosestPointToRobot() > 6)
{
for (auto it = road.begin(); it != road.begin() + (road.getIndexOfCurrentPoint() / 2); ++it)
road.backList.append(it->pos);
road.erase(road.begin(), road.begin() + (road.getIndexOfCurrentPoint() / 2));
}
return true;
}