GuideHumanToRoom.cpp

Go to the documentation of this file.

#include "GuideHumanToRoom.h"
 
#include <math.h>
 
#include <cmath>
#include <string>
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#include <VirtualRobot/VirtualRobot.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/Clock.h>
#include <ArmarXCore/core/util/StringHelpers.h>
 
#include <RobotAPI/libraries/skills/core/Skill.h>
#include <RobotAPI/libraries/skills/core/SkillStatusUpdate.h>
 
#include <armarx/navigation/algorithms/Costmap.h>
#include <armarx/navigation/algorithms/Room.h>
#include <armarx/navigation/client/NavigationStackConfig.h>
#include <armarx/navigation/client/Navigator.h>
#include <armarx/navigation/client/services/IceNavigator.h>
#include <armarx/navigation/core/basic_types.h>
#include <armarx/navigation/core/types.h>
#include <armarx/navigation/global_planning/SPFA.h>
#include <armarx/navigation/memory/client/costmap/Reader.h>
#include <armarx/navigation/memory/client/rooms/Reader.h>
#include <armarx/navigation/rooms/RoomNavigationTargetCreator.h>
#include <armarx/navigation/trajectory_control/local/TrajectoryFollowingController.h>
 
namespace armarx::navigation::skills
{
    GuideHumanToRoom::GuideHumanToRoom(const Services& srv) : Base(DefaultSkillDescription())
    {
        srv_.emplace(srv);
    }
 
    ::armarx::skills::Skill::InitResult
    GuideHumanToRoom::init(const Base::SpecializedInitInput& in)
    {
        ARMARX_CHECK(srv_.has_value());
 
        // parameterize the navigation stack
        client::NavigationStackConfig cfg;
        cfg.general(client::GeneralConfig{});
        cfg.globalPlanner(armarx::navigation::global_planning::SPFAParams{});
        cfg.trajectoryController(
            armarx::navigation::traj_ctrl::local::TrajectoryFollowingControllerParams{});
 
        const std::string configId = DefaultSkillDescription().skillId.skillName;
 
        // configure the `navigator` which provides a simplified and typed interface to the navigation server
        memorySubscriber.emplace(configId, srv_->memoryNameSystem);
 
        // register our config
        ARMARX_INFO << "Registering config";
        srv_->iceNavigator.createConfig(cfg, configId);
 
        navigator.emplace(client::Navigator::InjectedServices{
            .navigator = &srv_->iceNavigator, .subscriber = &memorySubscriber.value()});
 
        return ::armarx::skills::Skill::InitResult{
            .status = ::armarx::skills::TerminatedSkillStatus::Succeeded};
    }
 
    ::armarx::skills::Skill::MainResult
    GuideHumanToRoom::main(const Base::SpecializedMainInput& in)
    {
        const core::Pose globalTarget = getTargetInFrontOfRoom(in.parameters.room);
 
        ARMARX_INFO << "moving to target " << VAROUT(globalTarget.matrix());
 
        // execute
        ARMARX_INFO << "Sending navigation request";
        navigator->moveTo(globalTarget, core::NavigationFrame::Absolute);
 
        // Wait until goal is reached
        ARMARX_INFO << "Waiting until goal is reached.";
        client::StopEvent se = navigator->waitForStop();
        if (se)
        {
            ARMARX_INFO << "Goal `" << globalTarget.translation() << "`reached.";
        }
        else
        {
            if (se.isSafetyStopTriggeredEvent())
            {
                ARMARX_ERROR << "Safety stop was triggered!";
 
                return ::armarx::skills::Skill::MainResult{
                    .status = ::armarx::skills::TerminatedSkillStatus::Failed};
            }
            if (se.isUserAbortTriggeredEvent())
            {
                ARMARX_ERROR << "Aborted by user!";
 
                return ::armarx::skills::Skill::MainResult{
                    .status = ::armarx::skills::TerminatedSkillStatus::Failed};
            }
            if (se.isInternalErrorEvent())
            {
                ARMARX_ERROR << "Unknown internal error occured! "
                             << se.toInternalErrorEvent().message;
 
                return ::armarx::skills::Skill::MainResult{
                    .status = ::armarx::skills::TerminatedSkillStatus::Failed};
            }
        }
 
 
        return ::armarx::skills::Skill::MainResult{
            .status = ::armarx::skills::TerminatedSkillStatus::Succeeded};
    }
 
    void
    GuideHumanToRoom::onStopRequested()
    {
        if (navigator.has_value())
        {
            ARMARX_CHECK(navigator.has_value());
            navigator->stop();
        }
    }
 
    core::Pose
    GuideHumanToRoom::getTargetInFrontOfRoom(const std::string& roomName) const
    {
        const algorithms::Costmap costmap = [&]
        {
            const memory::client::costmap::Reader::Query costmapQuery{
                .providerName = properties.distanceToObstacleCostmapProvider,
                .name = "distance_to_obstacles",
                .timestamp = Clock::Now()};
 
            const auto result = srv_->costmapReader.query(costmapQuery);
            ARMARX_CHECK(result) << result.errorMessage;
            ARMARX_CHECK(result.costmap.has_value());
            return result.costmap.value();
        }();
 
        const armarx::navigation::algorithms::Room room = [&]
        {
            const memory::client::rooms::Reader::Query roomQuery{.providerName =
                                                                     properties.roomsProvider,
                                                                 .name = roomName,
                                                                 .timestamp = Clock::Now()};
 
            const auto result = srv_->roomsReader.query(roomQuery);
            ARMARX_CHECK(result) << "Failed to query room `" << roomName << "` from provider `"
                                 << properties.roomsProvider
                                 << "`. Reason: " << result.errorMessage;
            ARMARX_CHECK_NOT_EMPTY(result.rooms);
            ARMARX_CHECK_EQUAL(result.rooms.size(), 1);
 
            return result.rooms.front();
        }();
 
        const VirtualRobot::RobotPtr robot =
            srv_->virtualRobotReader.getRobotWaiting(properties.robotName);
        ARMARX_CHECK_NOT_NULL(robot);
        ARMARX_CHECK(srv_->virtualRobotReader.synchronizeRobot(*robot, Clock::Now()));
 
        const core::Pose global_T_robot(robot->getGlobalPose());
 
        rooms::RoomNavigationTargetCreator::Params algoParams;
        rooms::RoomNavigationTargetCreator algo(algoParams);
        auto result = algo.getClosestPositionOutsideOfRoom(
            costmap, room, global_T_robot.translation(), &srv_->arviz);
 
        const core::Direction dirToRoom = result.global_P_roomEntry - result.global_P_robot;
 
        // the robot should point to the room entry
        const float yaw = std::atan2(dirToRoom.y(), dirToRoom.x()) - M_PI_2f32;
 
        // TODO maybe rotate robot a bit towards the human?
 
        core::Pose global_T_robot_target = core::Pose::Identity();
        global_T_robot_target.translation() = result.global_P_robot;
        global_T_robot_target.linear() =
            Eigen::AngleAxisf(yaw, Eigen::Vector3f::UnitZ()).toRotationMatrix();
 
        return global_T_robot_target;
    }
 
 
} // namespace armarx::navigation::skills