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!
}
Exemple #2
0
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!
}