bool CReactiveNavigationSystem3D::checkCollisionWithLatestObstacles(
const mrpt::math::TPose2D& relative_robot_pose) const
{
const size_t nSlices = m_robotShape.size();
for (size_t idxH = 0; idxH < nSlices; ++idxH)
{
size_t nObs;
const float *xs, *ys, *zs;
m_WS_Obstacles_inlevels[idxH].getPointsBuffer(nObs, xs, ys, zs);
for (size_t i = 0;
i < 1 /* assume all PTGs share the same robot shape! */; i++)
{
const auto ptg = this->m_ptgmultilevel[i].PTGs[idxH];
ASSERT_(ptg != nullptr);
const double R = ptg->getMaxRobotRadius();
for (size_t obs = 0; obs < nObs; obs++)
{
const double gox = xs[obs], goy = ys[obs];
mrpt::math::TPoint2D lo;
relative_robot_pose.inverseComposePoint(
mrpt::math::TPoint2D(gox, goy), lo);
if (lo.x >= -R && lo.x <= R && lo.y >= -R && lo.y <= R &&
ptg->isPointInsideRobotShape(lo.x, lo.y))
{
return true; // collision!
}
}
}
}
return false; // No collision!
}
bool CReactiveNavigationSystem::checkCollisionWithLatestObstacles(
const mrpt::math::TPose2D& relative_robot_pose) const
{
ASSERT_(!PTGs.empty());
size_t nObs;
const float *xs, *ys, *zs;
m_WS_Obstacles.getPointsBuffer(nObs, xs, ys, zs);
for (size_t i = 0; i < 1 /* assume all PTGs share the same robot shape! */;
i++)
{
const auto ptg = PTGs[i];
ASSERT_(ptg != nullptr);
const double R = ptg->getMaxRobotRadius();
for (size_t obs = 0; obs < nObs; obs++)
{
const double gox = xs[obs], goy = ys[obs], oz = zs[obs];
if (oz < params_reactive_nav.min_obstacles_height ||
oz > params_reactive_nav.max_obstacles_height)
{
continue;
}
mrpt::math::TPoint2D lo;
relative_robot_pose.inverseComposePoint(
mrpt::math::TPoint2D(gox, goy), lo);
if (lo.x >= -R && lo.x <= R && lo.y >= -R && lo.y <= R &&
ptg->isPointInsideRobotShape(lo.x, lo.y))
{
return true; // collision!
}
}
}
return false; // No collision!
}
