TrajectoryChecker.h

Go to the documentation of this file.

#pragma once
 
#include <vector>
 
#include <armarx/navigation/algorithms/orientation_aware/Costmap3D.h>
#include <armarx/navigation/core/Trajectory.h>
 
namespace armarx::navigation::algorithms::orientation_aware
{
 
    struct TrajectoryCollisionCheckResult
    {
        int collisionCount = 0;
        std::vector<core::GlobalTrajectoryPoint> collisionPoints;
        bool isCollisionFree() const { return collisionCount == 0; }
    };
 
    class TrajectoryChecker
    {
    public:
        explicit TrajectoryChecker(const Costmap3D& costmap);
        TrajectoryCollisionCheckResult check(const core::GlobalTrajectory& trajectory, bool logDetails = false) const;
 
    private:
        const Costmap3D& costmap_;
        bool isInCollisionBilinear(float x, float y, float theta_rad) const;
    };
 
} // namespace armarx::navigation::algorithms::orientation_aware