LaserBasedProximity.cpp

Go to the documentation of this file.

#include "LaserBasedProximity.h"
 
#include <algorithm>
#include <cmath>
#include <cstddef>
#include <cstdlib>
#include <functional>
#include <limits>
#include <optional>
#include <string>
#include <utility>
#include <vector>
 
#include <boost/geometry.hpp>
#include <boost/geometry/algorithms/append.hpp>
#include <boost/geometry/geometries/multi_point.hpp>
 
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/src/Geometry/Transform.h>
 
#include <range/v3/algorithm/all_of.hpp>
#include <range/v3/algorithm/min.hpp>
#include <range/v3/range/conversion.hpp>
#include <range/v3/view/enumerate.hpp>
#include <range/v3/view/filter.hpp>
#include <range/v3/view/reverse.hpp>
#include <range/v3/view/transform.hpp>
#include <range/v3/view/zip_with.hpp>
 
#include <SimoxUtility/color/Color.h>
#include <SimoxUtility/color/cmaps/colormaps.h>
#include <SimoxUtility/shapes/OrientedBox.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/Robot.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/util/StringHelpers.h>
 
#include <RobotAPI/components/ArViz/Client/Client.h>
#include <RobotAPI/components/ArViz/Client/Elements.h>
#include <RobotAPI/components/ArViz/Client/Layer.h>
#include <RobotAPI/components/ArViz/Client/elements/Color.h>
#include <RobotAPI/components/ArViz/Client/elements/Line.h>
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
 
#include <armarx/navigation/conversions/eigen.h>
#include <armarx/navigation/core/NavigationStackGeneralConfig.h>
#include <armarx/navigation/core/basic_types.h>
#include <armarx/navigation/core/types.h>
#include <armarx/navigation/human/types.h>
#include <armarx/navigation/memory/types.h>
#include <armarx/navigation/safety_guard/SafetyGuard.h>
#include <armarx/navigation/safety_guard/aron/LaserBasedProximityParams.aron.generated.h>
#include <armarx/navigation/safety_guard/aron_conversions.h>
#include <armarx/navigation/safety_guard/core.h>
#include <armarx/navigation/util/geometry.h>
 
namespace rv = ::ranges::views;
 
namespace armarx::navigation::safety_guard
{
 
    namespace
    {
 
        inline core::TwistLimits
        mergeSafetyLimits(const std::vector<std::optional<core::TwistLimits>>& resultsAll)
        {
            const auto isValidFn =
                [](const std::optional<core::TwistLimits>& result) noexcept -> bool
            { return result.has_value(); };
 
            const std::vector<core::TwistLimits> validResults =
                resultsAll | rv::filter(isValidFn) |
                rv::transform(
                    [](const std::optional<core::TwistLimits>& result) noexcept -> core::TwistLimits
                    { return result.value(); }) |
                ranges::to_vector;
 
 
            if (validResults.empty())
            {
                return core::TwistLimits::NoLimits();
            }
 
            const std::vector<float> linearLimits =
                validResults |
                rv::transform([](const core::TwistLimits& result) noexcept -> float
                              { return result.linear; }) |
                ranges::to_vector;
 
            const std::vector<float> angularLimits =
                validResults |
                rv::transform([](const core::TwistLimits& result) noexcept -> float
                              { return result.angular; }) |
                ranges::to_vector;
 
            const core::TwistLimits combinedResult{.linear = ranges::min(linearLimits),
                                                   .angular = ranges::min(angularLimits)};
 
            return combinedResult;
        }
 
 
    } // namespace
 
    Algorithms
    LaserBasedProximityParams::algorithm() const
    {
        return Algorithms::LaserBasedProximity;
    }
 
    aron::data::DictPtr
    LaserBasedProximityParams::toAron() const
    {
        arondto::LaserBasedProximityParams dto;
 
        armarx::navigation::safety_guard::toAron(dto, *this);
 
        return dto.toAron();
    }
 
    LaserBasedProximityParams
    LaserBasedProximityParams::FromAron(const aron::data::DictPtr& dict)
    {
        arondto::LaserBasedProximityParams dto;
        dto.fromAron(dict);
 
        LaserBasedProximityParams bo;
        fromAron(dto, bo);
 
        return bo;
    }
 
    LaserBasedProximity::LaserBasedProximity(const Params& params,
                                             const core::GeneralConfig& generalConfig,
                                             const core::Scene& scene,
                                             const Context& ctx) :
        SafetyGuard(scene, ctx),
        params(params),
        generalConfig(generalConfig),
        humanColorMap_(simox::color::cmaps::BuPu().reversed()),
        laserColorMap_(simox::color::cmaps::OrRd().reversed())
    {
    }
 
    SafetyGuardResult
    LaserBasedProximity::computeSafetyLimits(const Eigen::Vector2f& global_V_movement)
    {
        ARMARX_CHECK(scene.dynamicScene.has_value());
 
        auto layer = viz.layer("safety_guard");
 
        const std::optional<core::TwistLimits> resultHumans = safetyLimitsHumans(layer);
        const std::optional<core::TwistLimits> resultLaserScanners =
            safetyLimitsLaserScanners(layer, global_V_movement);
 
        // Commit always. This ensures that objects eventually will be cleared.
        viz.commit(layer);
 
        const core::TwistLimits combinedResult =
            mergeSafetyLimits({resultHumans, resultLaserScanners});
        ARMARX_VERBOSE << "Safety limits: " << VAROUT(combinedResult.linear)
                       << VAROUT(combinedResult.angular);
 
        return SafetyGuardResult{.twistLimits = combinedResult};
    }
 
    std::optional<core::TwistLimits>
    LaserBasedProximity::safetyLimitsLaserScanners(viz::Layer& layer,
                                                   const Eigen::Vector2f& global_V_movement) const
    {
        if (not params.enableLaserScanners)
        {
            return std::nullopt;
        }
 
        if (scene.dynamicScene->laserScannerFeatures.empty())
        {
            ARMARX_INFO << deactivateSpam(5) << "No laser scanner features for SafetyGuard";
 
            return std::nullopt;
        }
 
        ARMARX_CHECK_NOT_NULL(context.debugObserver);
        auto& debugObserver = *context.debugObserver;
 
        debugObserver.setDebugObserverDatafield("numLaserScannerFeatures",
                                                scene.dynamicScene->laserScannerFeatures.size());
        debugObserver.setDebugObserverDatafield(
            "numFirstLaserScannerFeatures",
            scene.dynamicScene->laserScannerFeatures.front().features.size());
 
        ARMARX_VERBOSE << VAROUT(scene.dynamicScene->laserScannerFeatures.size());
        ARMARX_VERBOSE << VAROUT(scene.dynamicScene->laserScannerFeatures.front().features.size());
 
        const core::Pose global_T_robot{scene.robot->getGlobalPose()};
        const core::Pose robot_T_global{global_T_robot.inverse()};
 
        // compute distance based on convex hull of robot
        util::geometry::polygon_type convexHull;
        {
            boost::geometry::model::multi_point<util::geometry::point_type> robotNodes;
            for (const auto& node : scene.robot->getRobotNodes())
            {
                Eigen::Vector2f pos2d = conv::to2D(core::Pose(node->getGlobalPose())).translation();
                boost::geometry::append(robotNodes,
                                        util::geometry::point_type(pos2d.x(), pos2d.y()));
            }
            boost::geometry::convex_hull(robotNodes, convexHull);
        }
 
        // Compute the minimal distance to the robot for a set of features (point clusters)
        const auto minDistanceFn =
            [this, &convexHull, &robot_T_global](
                const memory::LaserScannerFeatures& features) -> DistanceAndClosestPoint
        {
            const core::Pose& global_T_sensor = features.frameGlobalPose;
 
            // Compute the minimal distance to the robot for a single feature (point cluster)
            const auto minDistanceFnInner =
                [this, &convexHull, &global_T_sensor, &robot_T_global](
                    const memory::LaserScannerFeature& feature) -> DistanceAndClosestPoint
            {
                // transform points to 3D global frame
                const std::vector<Eigen::Vector3f> points3dGlobal =
                    feature.points |
                    rv::transform([&global_T_sensor](const Eigen::Vector2f& pt) -> Eigen::Vector3f
                                  { return global_T_sensor * conv::to3D(pt); }) |
                    ranges::to_vector;
 
                return calculateMinDistance(convexHull, robot_T_global, points3dGlobal);
            };
 
            // Compute the minimal distance to the robot for all features (point clusters)
            const std::vector<DistanceAndClosestPoint> distances =
                features.features | rv::transform(minDistanceFnInner) | ranges::to_vector;
 
            if (distances.empty())
            {
                return DistanceAndClosestPoint{.distance = std::numeric_limits<float>::max(),
                                               .closestPoint = Eigen::Vector2f::Zero()};
            }
 
            return ranges::min(distances, std::less{}, &DistanceAndClosestPoint::distance);
        };
 
        const std::vector<DistanceAndClosestPoint> minDistanceToObstacles =
            scene.dynamicScene->laserScannerFeatures | rv::transform(minDistanceFn) |
            ranges::to_vector;
 
        const auto result = [&]() -> std::optional<InternalVelocityLimitResult>
        {
            switch (params.laserScannerProximityField.mode)
            {
                case ProximityFieldParams::Mode::Disabled:
                    return std::nullopt;
                case ProximityFieldParams::Mode::DirectionDependent:
                    return velocityLimitsDirectionDependent(
                        global_V_movement, minDistanceToObstacles, global_T_robot);
                case ProximityFieldParams::Mode::DirectionIndependent:
                    return velocityLimitsDirectionIndependent(minDistanceToObstacles);
            }
 
            ARMARX_ERROR << "Unknown ProximityFieldParams::Mode: "
                         << static_cast<int>(params.laserScannerProximityField.mode);
            return std::nullopt;
        }();
 
        layer.clear();
 
        // visualize robot and obstacle-independent information (always)
        {
            // visualize convex hull of robot
            simox::color::Color color = simox::Color::blue();
            viz::Polygon vizPoly("robot_convex_hull");
            for (const auto& p : rv::reverse(convexHull.outer()))
            {
                Eigen::Vector2f pt(p.x(), p.y());
                vizPoly.addPoint(conv::to3D(pt));
            }
            layer.add(vizPoly.color(color).lineColor(color));
 
            // visualize movement direction
            layer.add(viz::Line("robot_movement_Dir")
                          .fromTo(global_T_robot.translation(),
                                  global_T_robot.translation() +
                                      conv::to3D(global_V_movement).normalized() * 1000)
                          .lineWidth(10.F)
                          .color(simox::Color::blue()));
 
            // visualize ignored regions
            for (const auto& [i, region] : ranges::views::enumerate(params.ignoredRegions))
            {
                viz::Polygon vizRegion("ignored_region_" + std::to_string(i));
                vizRegion.addPoint(Eigen::Vector3f(region.min().x(), region.min().y(), 15));
                vizRegion.addPoint(Eigen::Vector3f(region.max().x(), region.min().y(), 15));
                vizRegion.addPoint(Eigen::Vector3f(region.max().x(), region.max().y(), 15));
                vizRegion.addPoint(Eigen::Vector3f(region.min().x(), region.max().y(), 15));
                layer.add(vizRegion.color(simox::Color::magenta()));
            }
 
            // visualize attached objects
            for (const auto& [i, obj] :
                 ranges::views::enumerate(scene.dynamicScene->attachedObjects))
            {
                const auto& oobb = obj->oobbGlobal();
                ARMARX_CHECK(oobb.has_value());
 
                simox::OrientedBoxf inflatedOobb{
                    oobb->transformation_centered(),
                    oobb->dimensions() + Eigen::Vector3f::Ones() * params.attachedObjectsInflation};
 
                viz::Box vizObj("attached_object_" + std::to_string(i));
                vizObj.set(inflatedOobb);
                layer.add(vizObj.color(simox::Color::magenta()));
            }
        }
 
        if (not result.has_value())
        {
            return std::nullopt;
        }
 
        // visualize the obstacle that leads to the calculated safety limits and the corresponding velocity limits
        {
            // increase transparency with increased distance, but never make fully transparent
            const auto color = laserColorMap_.at(
                std::abs(result->twistLimits.linear), 0, generalConfig.maxVel.linear);
 
            layer.add(viz::Line("nearest_obstacle")
                          .fromTo(global_T_robot.translation(), conv::to3D(result->closestPoint))
                          .lineWidth(50.F)
                          .color(color));
        }
 
        return result->twistLimits;
    }
 
    std::optional<core::TwistLimits>
    LaserBasedProximity::safetyLimitsHumans(viz::Layer& layer) const
    {
        if (not params.enableHumans)
        {
            return std::nullopt;
        }
 
        const core::Pose global_T_robot{scene.robot->getGlobalPose()};
 
        // Compute the minimal distance to the robot for a human
        // TODO(utetg): is minimum distance = distance to robot center or robot edge
        const auto minimalSegmentDistanceToRobot =
            [&global_T_robot](const human::Human& human) -> float
        {
            const Eigen::Isometry3f global_T_human = conv::to3D(human.pose);
            const Eigen::Isometry3f robot_T_human = global_T_robot.inverse() * global_T_human;
 
            // TODO extension: if also human keypoints are available, use them to compute the minimal distance
            return robot_T_human.translation().norm();
        };
 
        if (scene.dynamicScene->humans.empty())
        {
            ARMARX_VERBOSE << "No humans";
            return std::nullopt;
        }
 
        const auto distanceRobotToHumans = scene.dynamicScene->humans |
                                           rv::transform(minimalSegmentDistanceToRobot) |
                                           ranges::to_vector;
 
        const std::optional<float> minDistanceToHumans = scene.dynamicScene->humans.empty()
                                                             ? std::optional<float>{std::nullopt}
                                                             : ranges::min(distanceRobotToHumans);
 
        // handle if no humans are detected
        if (not minDistanceToHumans.has_value())
        {
            return core::TwistLimits::NoLimits();
        }
 
        const auto result =
            evaluateProximityField(params.humanProximityField, minDistanceToHumans.value());
 
        {
            // increase transparency with increased distance, but never make fully transparent
            const auto color = humanColorMap_(result.second).with_alpha(1 - result.second + 0.1);
 
            layer.add(viz::Cylinder("distance_humans")
                          .position(global_T_robot.translation() + Eigen::Vector3f{0, 0, 10})
                          .direction(Eigen::Vector3f::UnitZ())
                          .radius(minDistanceToHumans.value())
                          .height(10)
                          .color(color));
        }
 
        return result.first;
    }
 
    LaserBasedProximity::DistanceAndClosestPoint
    LaserBasedProximity::calculateMinDistance(
        const util::geometry::polygon_type& convexHull,
        const core::Pose& robot_T_global,
        const std::vector<Eigen::Vector3f>& globalPoints) const
    {
        // Compute the minimal distance to the robot for a list of points
 
        // transform points to 2D global frame
        const std::vector<Eigen::Vector2f> points2dGlobal =
            globalPoints |
            rv::transform([](const Eigen::Vector3f& pt) -> Eigen::Vector2f
                          { return conv::to2D(pt); }) |
            ranges::to_vector;
 
        if (/*isFeatureIgnored(globalPoints, points2dGlobal) or*/ globalPoints.empty())
        {
            ARMARX_VERBOSE << "Global points empty";
            return {.distance = std::numeric_limits<float>::max(),
                    .closestPoint = Eigen::Vector2f::Zero()};
        }
 
        // calculate distance to robot center
        const std::vector<float> pointsDistanceRobotCenter =
            globalPoints |
            rv::transform([&robot_T_global](const Eigen::Vector3f& pt) -> float
                          { return conv::to2D(robot_T_global * pt).norm(); }) |
            ranges::to_vector;
 
        if (params.enableLaserScannerFiltering &&
            globalPoints.size() <= static_cast<std::size_t>(params.laserScannerFilteringThreshold))
        {
            // this is a small feature
            ARMARX_VERBOSE << "Small feature with " << globalPoints.size()
                           << " points. Distances to robot center: " << pointsDistanceRobotCenter;
 
            if (ranges::all_of(pointsDistanceRobotCenter,
                               [&](float distanceToCenter) noexcept
                               {
                                   return std::max(distanceToCenter - params.robotRadius, 0.F) <
                                          params.laserScannerMaxFilteringDistance;
                               }))
            {
                // the feature is close to the robot
                // -> it should be filtered out
                return {.distance = std::numeric_limits<float>::max(),
                        .closestPoint = Eigen::Vector2f::Zero()};
            }
        }
 
        const auto distancesAndClosestPoint =
            rv::zip_with(
                [this, &convexHull](const Eigen::Vector2f& pt,
                                    float distanceToCenter) -> DistanceAndClosestPoint
                {
                    const auto distanceToConvexHull = static_cast<float>(boost::geometry::distance(
                        util::geometry::point_type(pt.x(), pt.y()), convexHull));
 
                    const float distanceUsingRobotRadius =
                        std::max(distanceToCenter - params.robotRadius, 0.F);
 
                    return {.distance = std::min(distanceToConvexHull, distanceUsingRobotRadius),
                            .closestPoint = pt};
                },
                points2dGlobal,
                pointsDistanceRobotCenter) |
            ranges::to_vector;
 
        return ranges::min(
            distancesAndClosestPoint, std::less{}, &DistanceAndClosestPoint::distance);
    }
 
    std::pair<core::TwistLimits, float>
    LaserBasedProximity::evaluateProximityField(const ProximityFieldParams& proximityField,
                                                float minDistance) const
    {
        if (minDistance < proximityField.safetyDistance)
        {
            return {core::TwistLimits::ZeroLimits(), 0.F};
        }
 
        if (minDistance > proximityField.influenceDistance)
        {
            return {core::TwistLimits::NoLimits(), 1.F};
        }
 
 
        const float proximityRange =
            proximityField.influenceDistance - proximityField.safetyDistance;
        const float clippedDistance =
            std::min(minDistance, proximityField.influenceDistance) - proximityField.safetyDistance;
 
        const float fractionalDistance = std::clamp(clippedDistance / proximityRange, 0.F, 1.F);
 
        if (not proximityField.reduceVelocity)
        {
            return {core::TwistLimits::NoLimits(), fractionalDistance};
        }
 
        const float d_s = std::pow(1 - fractionalDistance, proximityField.k);
 
        const auto permissibleVelocity = [&proximityField](const float d_s,
                                                           const float v_max) -> float
        { return v_max / (1 + proximityField.lambda * d_s); };
 
        const core::TwistLimits result{
            .linear = permissibleVelocity(d_s, generalConfig.maxVel.linear),
            .angular = permissibleVelocity(d_s, generalConfig.maxVel.angular)};
 
        return {result, fractionalDistance};
    }
 
    template <class T, class Func>
    static bool
    allPointsInside(const std::vector<T>& points, Func isInsideFunc)
    {
        for (const auto& p : points)
        {
            if (not isInsideFunc(p))
            {
                // this point is not inside
                return false;
            }
        }
        return true;
    }
 
    bool
    LaserBasedProximity::isFeatureIgnored(
        const std::vector<Eigen::Vector3f>& featurePoints3DGlobal,
        const std::vector<Eigen::Vector2f>& featurePoints2DGlobal) const
    {
        return false; // FIXME: enable this again after testing, currently disabled to not filter out any features while testing the new safety guard implementation
 
        // first check against ignored regions
        for (const auto& region : params.ignoredRegions)
        {
            ARMARX_CHECK(not region.isEmpty());
 
            if (allPointsInside(featurePoints2DGlobal,
                                [&region](const Eigen::Vector2f& p) { return region.contains(p); }))
            {
                // the feature lies fully inside this region -> it is ignored
                return true;
            }
        }
 
        // then check against attached objects
        for (const auto* o : scene.dynamicScene->attachedObjects)
        {
            const auto& oobbGlobal = o->oobbGlobal();
            ARMARX_CHECK(oobbGlobal.has_value());
 
            simox::OrientedBoxf inflatedOobb{oobbGlobal->transformation_centered(),
                                             oobbGlobal->dimensions() +
                                                 Eigen::Vector3f::Ones() *
                                                     params.attachedObjectsInflation};
 
            if (allPointsInside(featurePoints3DGlobal,
                                [&inflatedOobb](const Eigen::Vector3f& p)
                                { return inflatedOobb.contains(p); }))
            {
                // the feature lies fully inside the objects oobb -> it is ignored
                return true;
            }
        }
 
        return false;
    }
 
    LaserBasedProximity::InternalVelocityLimitResult
    LaserBasedProximity::velocityLimitsDirectionDependent(
        const Eigen::Vector2f& global_V_movement,
        const std::vector<DistanceAndClosestPoint>& minDistanceToObstacles,
        const Eigen::Isometry3f& global_T_robot) const
    {
 
        float maxPermissibleRelativeVelocityLinear = generalConfig.maxVel.linear;
 
        Eigen::Vector2f minPoint = Eigen::Vector2f::Zero();
        float minDistance = std::numeric_limits<float>::max();
 
        const Eigen::Isometry3f robot_T_global = global_T_robot.inverse();
 
        ARMARX_CHECK_NOT_NULL(context.debugObserver);
        auto& debugObserver = *context.debugObserver;
 
        debugObserver.setDebugObserverDatafield("numObstacles", minDistanceToObstacles.size());
 
        // ARMARX_VERBOSE << VAROUT(minDistanceToObstacles.size());
 
        for (const auto& [distance, global_P_obstacle_pt] : minDistanceToObstacles)
        {
            const float ds = params.laserScannerProximityField.safetyDistance;
            const float di = params.laserScannerProximityField.influenceDistance;
            const float d = distance;
 
            float velocity_damper = generalConfig.maxVel.linear * (distance - ds) / (di - ds);
 
            ARMARX_VERBOSE << VAROUT(velocity_damper);
 
            const Eigen::Vector2f minPointGlobal = global_P_obstacle_pt;
 
            ARMARX_VERBOSE << VAROUT(minPointGlobal);
            ARMARX_VERBOSE << VAROUT(robot_T_global.translation().head<2>());
 
            const Eigen::Vector2f directionRobotToObstacle =
                (minPointGlobal - global_T_robot.translation().head<2>()).normalized();
 
            ARMARX_VERBOSE << VAROUT(directionRobotToObstacle);
 
            // Cases:
            // 1) > 0: approaching object
            // 2) < 0: moving away from object
            const float relativeVelocityScaling =
                directionRobotToObstacle.dot(global_V_movement.normalized());
 
            ARMARX_VERBOSE << VAROUT(distance);
            ARMARX_VERBOSE << VAROUT(relativeVelocityScaling);
 
            // moving away from obstacle?
            if (relativeVelocityScaling < 0)
            {
                continue;
            }
 
            if (d <= ds) // we must stop, if an obstacle is too close
            {
                maxPermissibleRelativeVelocityLinear = 0;
                minPoint = minPointGlobal;
                minDistance = d;
                continue;
            }
 
            // if not exactly tangential
            if (std::abs(relativeVelocityScaling) > 1e-4)
            {
                const float thisMaxPermissibileRelVelLinear =
                    velocity_damper / relativeVelocityScaling;
                ARMARX_VERBOSE << VAROUT(thisMaxPermissibileRelVelLinear);
 
                if (thisMaxPermissibileRelVelLinear < maxPermissibleRelativeVelocityLinear)
                {
                    maxPermissibleRelativeVelocityLinear = thisMaxPermissibileRelVelLinear;
                    minPoint = minPointGlobal;
                }
            }
        }
 
        debugObserver.setDebugObserverDatafield("maxPermissibleRelativeVelocityLinear",
                                                maxPermissibleRelativeVelocityLinear);
        debugObserver.setDebugObserverDatafield("minDistance", minDistance);
 
        core::TwistLimits result{.linear = std::max<float>(0, maxPermissibleRelativeVelocityLinear),
                                 .angular = generalConfig.maxVel.angular};
        return {.twistLimits = result, .minDistance = minDistance, .closestPoint = minPoint};
    }
 
    LaserBasedProximity::InternalVelocityLimitResult
    LaserBasedProximity::velocityLimitsDirectionIndependent(
        const std::vector<DistanceAndClosestPoint>& minDistanceToObstacles) const
    {
        const DistanceAndClosestPoint minDistance =
            ranges::min(minDistanceToObstacles, std::less{}, &DistanceAndClosestPoint::distance);
 
        const auto result =
            evaluateProximityField(params.laserScannerProximityField, minDistance.distance);
 
        return {.twistLimits = result.first,
                .minDistance = minDistance.distance,
                .closestPoint = minDistance.closestPoint};
    }
} // namespace armarx::navigation::safety_guard