NavigateToChargingStation.cpp

Go to the documentation of this file.

#include "NavigateToChargingStation.h"
 
#include <sstream>
 
#include <ArmarXCore/core/time/Duration.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
#include <RobotAPI/libraries/skills/core/Skill.h>
#include <RobotAPI/libraries/skills/core/SkillDescription.h>
#include <RobotAPI/libraries/skills/core/SkillStatusUpdate.h>
 
#include <armarx/navigation/skills/aron/NavigateToNamedLocation.aron.generated.h>
#include <armarx/navigation/skills/skill_ids.h>
 
namespace armarx::navigation::skills
{
 
    armarx::skills::SkillDescription
    NavigateToChargingStation::GetSkillDescription()
    {
        ParamType defaultParameters;
 
        const armarx::ObjectID chargingStationObjectID{"Building/charging-station"};
 
        // The locations are defined linked to the charging station object.
        defaultParameters.inFrontOfChargingStationLocationName =
            chargingStationObjectID.getClassID().str() + "/inFrontOf";
        defaultParameters.chargingLocationName =
            chargingStationObjectID.getClassID().str() + "/charging";
 
        std::stringstream description;
        description << "Go to the charging station and initiate charging.";
        description << "\n";
        description << "Optionally the names of the charging-station locations can be changed "
                       "through the skill's parameters.";
        description << "\n\n";
        description
            << " - **inFrontOfChargingStationLocationName**: Name of the location the robot will "
               "first navigate to in order to initiate the movement towards the charging "
               "station.\n\n";
        description
            << " - **chargingLocationName**: Name of the location where the robot should charge. "
               "This is just a theoretical location, since the robot will probably have to find a "
               "very precise placement where the wireless charging will initiate.\n";
 
        return armarx::skills::SkillDescription{
            .skillId = skill_ids::NavigateToChargingStation,
            .description = description.str(),
            .rootProfileDefaults = defaultParameters.toAron(),
            .timeout = armarx::core::time::Duration::Minutes(3),
            .parametersType = Params::ToAronType(),
        };
    }
 
    NavigateToChargingStation::Properties::SubSkillIds::SubSkillIds() :
        navigateToNamedLocation{armarx::navigation::skills::skill_ids::NavigateToNamedLocation}
    {
        ;
    }
 
    NavigateToChargingStation::NavigateToChargingStation(const Properties& properties) :
        Base(GetSkillDescription()), properties(properties)
    {
        ;
    }
 
    ::armarx::skills::Skill::MainResult
    NavigateToChargingStation::main(const Base::SpecializedMainInput& in)
    {
        ARMARX_TRACE;
        throwIfSkillShouldTerminate();
 
        // Navigate to inFrontOf charging station location for approach.
        {
            using ParamsT = arondto::NavigateToNamedLocationParams;
 
            auto update =
                callSubskill<ParamsT>(properties.subSkillIDs.navigateToNamedLocation,
                                      [&in](ParamsT& namedLocationParams) noexcept
                                      {
                                          namedLocationParams.locationName =
                                              in.parameters.inFrontOfChargingStationLocationName;
                                      });
 
            if (armarx::skills::skillExecutionFailed(update))
            {
                return MakeFailedResult();
            }
        }
 
        ARMARX_TRACE;
        throwIfSkillShouldTerminate();
 
        // Navigate to charging location to initiate charging.
        {
            // TODO: Does not work because the location is not reachable.
 
            //            arondto::NavigateToNamedLocationParams namedLocationParams;
            //            namedLocationParams.locationName = in.parameters.chargingLocationName;
 
            //            auto update =
            //                callSubskill(properties.subSkillIDs.navigateToNamedLocation, namedLocationParams);
            //            if (not(update.has_value() and update->hasBeenSucceeded()))
            //            {
            //                return MakeFailedResult();
            //            }
        }
 
        ARMARX_TRACE;
        throwIfSkillShouldTerminate();
 
        return MakeSucceededResult();
    }
 
} // namespace armarx::navigation::skills