CreateHumanPoseWithFaceRecognition.cpp

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/>.
 *
 * @package    VisionX::COMPONENT_NAME_SNAKE::skills
 * @author     Peter Albrecht ( usnlf at student dot kit dot edu)
 * @date       2024
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "CreateHumanPoseWithFaceRecognition.h"
 
#include <VisionX/libraries/person_simulator/core/PersonSimulator.h>
#include <VisionX/libraries/person_simulator/skills/constants.h>
#include <VisionX/libraries/person_simulator/skills/skill_ids.h>
 
namespace VisionX::person_simulator::skills
{
 
    ::armarx::skills::SkillDescription
    CreateHumanPoseWithFaceRecognition::GetSkillDescription()
    {
        ParamType defaultParams;
        defaultParams.profileEntityName = "John Handover";
        defaultParams.humanId = "0";
        defaultParams.orientationDegreesClockwise = 0.F;
        defaultParams.orientationFacesRobot = true;
 
        const std::string skillName =
            VisionX::person_simulator::skills::constants::CreateHumanPoseWithFaceRecognition;
        const armarx::skills::SkillID skillId =
            VisionX::person_simulator::skills::skill_ids::CreateHumanPoseWithFaceRecognition;
 
        return ::armarx::skills::SkillDescription{
            .skillId = skillId,
            .description = "Creates and commits a human pose with an attached face recognition. "
                           "Optionally, the pose can be set to face the robot.",
            .rootProfileDefaults = defaultParams.toAron(),
            .timeout = ::armarx::Duration::MilliSeconds(1000),
            .parametersType = ParamType::ToAronType()};
    }
 
    CreateHumanPoseWithFaceRecognition::CreateHumanPoseWithFaceRecognition(
        const core::PersonSimulator::Remote& r,
        const core::PersonSimulator::Properties& p) :
        Base(GetSkillDescription()), remote(r), properties(p)
    {
    }
 
    CreateHumanPoseWithFaceRecognition::MainResult
    CreateHumanPoseWithFaceRecognition::main(const SpecializedMainInput& in)
    {
        core::PersonSimulator simulator(remote, properties);
 
        // create a pose, and then create a face recognition at the face position
 
        armarx::armem::human::HumanPose committedPose;
        if (in.parameters.orientationFacesRobot)
        {
            committedPose = simulator.createHumanPoseFacingRobot(in.parameters.humanId,
                                                                 in.parameters.positionCartesian);
        }
        else
        {
            committedPose = simulator.createHumanPose(in.parameters.humanId,
                                                      in.parameters.positionCartesian,
                                                      in.parameters.orientationDegreesClockwise);
        }
 
        // we gain the committed pose from the simulator
        // take position of face; create face recognition at that location
        simulator.createFaceRecognition(committedPose.keypoints["Head"].positionGlobal->toEigen(),
                                        in.parameters.profileEntityName);
 
        return Skill::MakeSucceededResult();
    }
 
} // namespace VisionX::person_simulator::skills