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/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
        };
 
        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;
        };
 
        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