TrajectoryChecker.cpp

Go to the documentation of this file.

#include "TrajectoryChecker.h"
 
#include <cmath>
 
#ifndef M_PIf
#define M_PIf 3.14159265358979323846f
#endif
 
#include <ArmarXCore/core/logging/Logging.h>
 
namespace armarx::navigation::algorithms::orientation_aware
{
    inline double normalizeAngle(double a)
    {
        return std::atan2(std::sin(a), std::cos(a));
    }
 
    TrajectoryChecker::TrajectoryChecker(const Costmap3D& costmap) : costmap_(costmap)
    {
    }
 
    bool TrajectoryChecker::isInCollisionBilinear(float x, float y, float theta_rad) const
    {
        // Uses the same bilinear position interpolation as Costmap3DWrapper
        const auto localPosition = costmap_.origin().inverse() * Eigen::Vector2f{x, y};
 
        // Treat points outside the costmap bounds as collision
        const auto& bounds = costmap_.getLocalSceneBounds();
        if (localPosition.x() < bounds.min.x() || localPosition.x() > bounds.max.x() ||
            localPosition.y() < bounds.min.y() || localPosition.y() > bounds.max.y())
        {
            return true;
        }
 
        const float cell = costmap_.params().cellSize;
 
        const float vX =
            (localPosition.x() - cell / 2.0f - costmap_.getLocalSceneBounds().min.x()) / cell;
        const float vY =
            (localPosition.y() - cell / 2.0f - costmap_.getLocalSceneBounds().min.y()) / cell;
 
        // Same -0.01 shift as Costmap3DWrapper::toIndexBilinear
        const float vX_shift = vX - 0.01f;
        const float vY_shift = vY - 0.01f;
 
        int iXlow = static_cast<int>(std::floor(vX_shift));
        int iXhigh = static_cast<int>(std::ceil(vX_shift));
        int iYlow = static_cast<int>(std::floor(vY_shift));
        int iYhigh = static_cast<int>(std::ceil(vY_shift));
 
        const auto size = costmap_.getSize();
        iXlow = std::clamp(iXlow, 0, size.x() - 1);
        iXhigh = std::clamp(iXhigh, 0, size.x() - 1);
        iYlow = std::clamp(iYlow, 0, size.y() - 1);
        iYhigh = std::clamp(iYhigh, 0, size.y() - 1);
 
        const float w00 =
            (static_cast<float>(iXhigh) - vX) * (static_cast<float>(iYhigh) - vY);
        const float w10 =
            (vX - static_cast<float>(iXlow)) * (static_cast<float>(iYhigh) - vY);
        const float w01 =
            (static_cast<float>(iXhigh) - vX) * (vY - static_cast<float>(iYlow));
        const float w11 =
            (vX - static_cast<float>(iXlow)) * (vY - static_cast<float>(iYlow));
 
        const auto rotation = costmap_.closestRotationFromDegrees(theta_rad * 180.0f / M_PIf);
 
        auto gridVal = [&](int ix, int iy) -> float {
            return costmap_.value_ignore_mask({ix, iy}, rotation.index);
        };
 
        float interpolated = w00 * gridVal(iXlow, iYlow) + w10 * gridVal(iXhigh, iYlow) +
                             w01 * gridVal(iXlow, iYhigh) + w11 * gridVal(iXhigh, iYhigh);
 
        return interpolated <= 10.0f;
    }
 
    TrajectoryCollisionCheckResult TrajectoryChecker::check(const core::GlobalTrajectory& trajectory, bool logDetails) const
    {
        TrajectoryCollisionCheckResult result;
 
        struct Point { double x, y, theta; };
        std::vector<Point> points;
        points.reserve(trajectory.points().size());
        for (const auto& gp : trajectory.points())
        {
            const auto& R = gp.waypoint.pose.rotation();
            points.push_back({
                gp.waypoint.pose.translation().x(),
                gp.waypoint.pose.translation().y(),
                std::atan2(R(1, 0), R(0, 0))
            });
        }
 
        // Check waypoints
        for (std::size_t i = 0; i < points.size(); ++i)
        {
            if (isInCollisionBilinear(static_cast<float>(points[i].x),
                                      static_cast<float>(points[i].y),
                                      static_cast<float>(points[i].theta)))
            {
                result.collisionCount++;
                result.collisionPoints.push_back(trajectory.points()[i]);
                if (logDetails)
                {
                    ARMARX_INFO << "Collision detected at waypoint " << i;
                }
            }
        }
 
        // Check segments
        constexpr float segmentResolution = 50.0f;
        for (std::size_t i = 0; i + 1 < points.size(); ++i)
        {
            double dx = points[i + 1].x - points[i].x;
            double dy = points[i + 1].y - points[i].y;
            double dist = std::hypot(dx, dy);
            int numSamples = static_cast<int>(std::ceil(dist / segmentResolution));
            if (numSamples < 2)
                numSamples = 2;
 
            for (int s = 1; s < numSamples; ++s)
            {
                double t = static_cast<double>(s) / numSamples;
                double sampleX = points[i].x + t * dx;
                double sampleY = points[i].y + t * dy;
                double dtheta = normalizeAngle(points[i + 1].theta - points[i].theta);
                double sampleTheta = normalizeAngle(points[i].theta + t * dtheta);
 
                if (isInCollisionBilinear(static_cast<float>(sampleX),
                                          static_cast<float>(sampleY),
                                          static_cast<float>(sampleTheta)))
                {
                    result.collisionCount++;
                    core::GlobalTrajectoryPoint samplePoint = trajectory.points()[i];
                    samplePoint.waypoint.pose =
                        Eigen::Translation3f{Eigen::Vector3f(static_cast<float>(sampleX),
                                                             static_cast<float>(sampleY),
                                                             0.0f)} *
                        Eigen::AngleAxisf{static_cast<float>(sampleTheta), Eigen::Vector3f::UnitZ()};
                    result.collisionPoints.push_back(samplePoint);
                    if (logDetails)
                    {
                        ARMARX_INFO << "Collision detected on segment between waypoints " << i << " and " << i + 1;
                    }
                }
            }
        }
 
        return result;
    }
 
} // namespace armarx::navigation::algorithms::orientation_aware