GoalReachedMonitor.cpp

Go to the documentation of this file.

#include "GoalReachedMonitor.h"
 
#include <VirtualRobot/Robot.h> // IWYU pragma: keep
 
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include <armarx/navigation/core/basic_types.h>
#include <armarx/navigation/core/types.h>
 
namespace armarx::navigation::server
{
 
    GoalReachedMonitor::GoalReachedMonitor(const core::Pose& goal,
                                           const core::Scene& scene,
                                           const GoalReachedMonitorConfig& config) :
        goal_(goal), scene(scene), config(config), reachedCount(0)
    {
        //ARMARX_CHECK(not waypoints.empty());
    }
 
    bool
    GoalReachedMonitor::goalReached(const bool filter) const noexcept
    {
        ARMARX_TRACE;
 
        const core::Pose world_T_robot(scene.robot->getGlobalPose());
        const core::Pose& world_T_goal(goal_);
 
        const core::Pose robot_T_goal = world_T_robot.inverse() * world_T_goal;
 
        ARMARX_VERBOSE << "Position error: " << robot_T_goal.translation().head<2>().norm();
        ARMARX_VERBOSE << "Rotation error: " << Eigen::AngleAxisf(robot_T_goal.linear()).angle();
 
        const bool posReached = robot_T_goal.translation().head<2>().norm() <= config.posTh;
        const bool oriReached = Eigen::AngleAxisf(robot_T_goal.linear()).angle() <= config.oriTh;
 
        if (posReached)
        {
            ARMARX_DEBUG << "Position goal `" << robot_T_goal.translation().head<2>()
                         << "` reached";
        }
 
        if (oriReached)
        {
            ARMARX_DEBUG << "Orientation goal reached";
        }
 
        const bool linearVelocityLow = scene.platformVelocity.linear.norm() < config.linearVelTh;
        const bool angularVelocityLow = scene.platformVelocity.angular.norm() < config.angularVelTh;
 
        const bool velocityLow = linearVelocityLow and angularVelocityLow;
 
        if (velocityLow)
        {
            ARMARX_DEBUG << "Robot has come to rest.";
        }
 
        bool reached = posReached and oriReached and velocityLow;
 
        if (reached)
        {
            reachedCount++;
            ARMARX_DEBUG << "Goal reached! reachedCount: " << reachedCount;
            return !filter || (reachedCount >= config.filterCount);
        }
        else
        {
            reachedCount = 0;
            return false;
        }
    }
 
    const core::Pose&
    GoalReachedMonitor::goal() const noexcept
    {
        return goal_;
    }
    
    GoalReachedMonitor::Status GoalReachedMonitor::status() const
    {
        const core::Pose world_T_robot(scene.robot->getGlobalPose());
        const core::Pose& world_T_goal(goal_);
 
        const core::Pose robot_T_goal = world_T_robot.inverse() * world_T_goal;
 
        const float posError =  robot_T_goal.translation().head<2>().norm();
        const float oriError = Eigen::AngleAxisf(robot_T_goal.linear()).angle();
 
        return Status{.posError = posError, .oriError = oriError};
    }
 
    GoalReachedMonitor::GoalReachedMonitor(GoalReachedMonitor&& other) noexcept :
        goal_{other.goal_}, scene{other.scene}, config(other.config), reachedCount(other.reachedCount)
    {
    }
 
    GoalReachedMonitor&
    GoalReachedMonitor::operator=(GoalReachedMonitor&& other) noexcept
    {
        if (this != &other)
        {
            goal_ = std::move(other.goal_);
            reachedCount = other.reachedCount;
            // scene is a const reference — cannot be rebound; must refer to the same scene object
            // config is const — cannot be reassigned
        }
        return *this;
    }
 
    const GoalReachedMonitorConfig
    GoalReachedMonitor::getConfig() const
    {
        return this->config;
    }
 
    void
    GoalReachedMonitor::updateGoal(const core::Pose& newGoal)
    {
        goal_ = newGoal;
    }
} // namespace armarx::navigation::server