GoalReachedMonitor.cpp

Go to the documentation of this file.

#include "GoalReachedMonitor.h"
 
#include <VirtualRobot/Robot.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.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
    {
        // return false;
        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();
 
        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::GoalReachedMonitor(GoalReachedMonitor&& other) noexcept :
        goal_{other.goal_}, scene{other.scene}, config(other.config)
    {
    }
 
    GoalReachedMonitor&
    GoalReachedMonitor::operator=(GoalReachedMonitor&& /*unused*/) noexcept
    {
        return *this;
    }
} // namespace armarx::navigation::server