HandoverCostmapBuilder.h

Go to the documentation of this file.

/**
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @author      (  )
 * @date       2026
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <optional>
#include <vector>
 
#include <armarx/navigation/algorithms/Costmap.h>
#include <armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.h>
#include <armarx/navigation/core/basic_types.h>
 
#include <VisionX/libraries/armem_human/types.h>
 
namespace armarx::navigation::algorithms::costmap
{
 
    class HandoverCostmapBuilder
    {
    public:
        struct Parameters
        {
            // radial distance from human to handover location
            float handoverDistanceMin = 700.0f; // in mm
            float handoverDistanceMax = 1500.0f; // in mm
            float handoverDistancePreference = 1000.0f; // in mm
 
            // angular sector in front of human where handover location is preferred
            float handoverAngleMin = -30.0f; // in degrees
            float handoverAngleMax = 30.0f; // in degrees
            float handoverAnglePreference = 0; // in degrees
 
            // scene bound params from human position
            float humanSceneBoundOffsetBehind = 1000.0f; // in mm
            float humanSceneBoundOffsetFront = 2000.0f; // in mm
            float humanSceneBoundOffsetSideLeft = 1500.0f; // in mm
            float humanSceneBoundOffsetSideRight = 1500.0f; // in mm
 
            // additive cost penalty applied to candidates whose distance to the
            // nearest obstacle is below `obstacleProximityThreshold` (instead of
            // scaling the whole cost by the distance to the nearest obstacle)
            float obstacleProximityThreshold = 50.0f; // in mm
            float obstacleProximityPenalty = 1.0f;
 
            // If robotPosition is provided and weight > 0, the SPFA path cost from
            // the robot to each candidate is added to the handover cost. This favors
            // placements that are closer to the robot and reachable without passing
            // through narrow spaces (the SPFA edge cost penalizes proximity to
            // obstacles).
            float robotPathCostWeight = 0.5f; // 0 disables the term
            float robotPathMaxCostMM = 5000.0f; // saturation distance
 
            // Parameters for the internal SPFA run used to compute robotPathCost.
            spfa::ShortestPathFasterAlgorithm::Parameters robotPathParams;
        };
 
        struct SceneInformation
        {
            //! information about the human for handover
            armarx::armem::human::HumanPose humanPose;
 
            //! information about other humans in the scene
            std::vector<armarx::armem::human::HumanPose> otherHumans;
 
            //! distance to obstacles in the environment
            Costmap distanceMap;
 
            //! Optional override for the human's 2-D pose. When set, the builder uses this
            //! instead of deriving the pose from the skeleton in \c humanPose.
            std::optional<armarx::navigation::core::Pose2D> humanPose2DOverride;
 
            //! Optional global 2-D position of the robot. When set, the builder runs an
            //! SPFA from this position on the distance map and adds the path cost to
            //! every handover candidate, while masking out unreachable cells.
            std::optional<Eigen::Vector2f> robotPosition;
        };
 
        HandoverCostmapBuilder(const Costmap::Parameters& costmapParameters,
                               const Parameters& handoverParameters);
 
        [[nodiscard]] std::optional<Costmap> create(const SceneInformation& sceneInfo);
 
        std::optional<core::Pose2D> getOptimalHandoverPose(const SceneInformation& sceneInfo);
 
    protected:
    private:
        const navigation::algorithms::Costmap::Parameters costmapParameters_;
        const Parameters handoverParameters_;
    };
 
} // namespace armarx::navigation::algorithms::costmap