LaserBasedProximity.cpp

Go to the documentation of this file.

#include "LaserBasedProximity.h"
 
#include <optional>
#include <vector>
 
#include <boost/geometry.hpp>
#include <boost/geometry/algorithms/append.hpp>
#include <boost/geometry/geometries/multi_point.hpp>
 
#include <Eigen/Geometry>
 
#include <SimoxUtility/color/cmaps/colormaps.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/Robot.h>
 
#include <ArmarXCore/core/logging/Logging.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 <armarx/navigation/conversions/eigen.h>
#include <armarx/navigation/core/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 <range/v3/algorithm/min.hpp>
#include <range/v3/range/conversion.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>
 
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) -> bool
            { return result.has_value(); };
 
            const std::vector<core::TwistLimits> validResults =
                resultsAll | rv::filter(isValidFn) |
                rv::transform(
                    [](const std::optional<core::TwistLimits>& result) -> 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) -> float
                              { return result.linear; }) |
                ranges::to_vector;
 
            const std::vector<float> angularLimits =
                validResults |
                rv::transform([](const core::TwistLimits& result) -> float
                              { return result.angular; }) |
                ranges::to_vector;
 
            const core::TwistLimits combinedResult{.linear = ranges::min(linearLimits),
                                                   .angular = ranges::min(angularLimits)};
 
            return combinedResult;
        }
 
        struct PairCompare
        {
            bool
            operator()(const auto& a, const auto& b)
            {
                return a.first < b.first;
            }
        };
 
    } // 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::Scene& scene,
                                             const Context& ctx) :
        SafetyGuard(scene, ctx),
        params(params),
        humanColorMap_(simox::color::cmaps::BuPu().reversed()),
        laserColorMap_(simox::color::cmaps::OrRd().reversed())
    {
    }
 
    SafetyGuardResult
    LaserBasedProximity::computeSafetyLimits()
    {
        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);
 
        // 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
    {
        if (not params.enableLaserScanners)
        {
            return std::nullopt;
        }
 
        if (scene.dynamicScene->laserScannerFeatures.empty())
        {
            ARMARX_INFO << "No laser scanner features";
 
            return std::nullopt;
        }
 
        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) -> std::pair<float, Eigen::Vector2f>
        {
            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) -> std::pair<float, Eigen::Vector2f>
            {
                // 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<std::pair<float, Eigen::Vector2f>> distances =
                features.features | rv::transform(minDistanceFnInner) | ranges::to_vector;
 
            return ranges::min(distances, PairCompare());
        };
 
        const std::vector<std::pair<float, Eigen::Vector2f>> minDistanceToObstacles =
            scene.dynamicScene->laserScannerFeatures | rv::transform(minDistanceFn) |
            ranges::to_vector;
 
        const std::pair<float, Eigen::Vector2f> minDistance =
            ranges::min(minDistanceToObstacles, PairCompare());
 
        const auto result =
            evaluateProximityField(params.laserScannerProximityField, minDistance.first);
 
 
        {
            //visualize convex hull and nearest obstacle
 
            // increase transparency with increased distance, but never make fully transparent
            const auto color = laserColorMap_(result.second);
 
            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));
 
            layer.add(viz::Line("nearest_obstacle")
                          .fromTo(global_T_robot.translation(), conv::to3D(minDistance.second))
                          .lineWidth(100.F)
                          .color(color));
        }
 
        return result.first;
    }
 
    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;
    }
 
    std::pair<float, Eigen::Vector2f>
    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;
 
        // 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;
 
        return ranges::min(
            rv::zip_with(
                [this, &convexHull](const Eigen::Vector2f& pt,
                                    float distanceToCenter) -> std::pair<float, Eigen::Vector2f>
                {
                    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 {std::min(distanceToConvexHull, distanceUsingRobotRadius), pt};
                },
                points2dGlobal,
                pointsDistanceRobotCenter),
            PairCompare());
    }
 
    std::pair<core::TwistLimits, float>
    LaserBasedProximity::evaluateProximityField(const ProximityFieldParams& proximityField,
                                                float minDistance) const
    {
        if (minDistance < proximityField.minDistance)
        {
            return {core::TwistLimits::ZeroLimits(), 0.F};
        }
 
        if (minDistance > proximityField.maxDistance)
        {
            return {core::TwistLimits::NoLimits(), 1.F};
        }
 
 
        const float proximityRange = proximityField.maxDistance - proximityField.minDistance;
        const float clippedDistance =
            std::min(minDistance, proximityField.maxDistance) - proximityField.minDistance;
 
        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, params.maxVelocity.linear),
            .angular = permissibleVelocity(d_s, params.maxVelocity.angular)};
 
        return {result, fractionalDistance};
    }
 
} // namespace armarx::navigation::safety_guard