MoveToNext.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2014-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * 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    RobotSkillTemplates::MovePlatformToLandmarkGroup
 * @author     Valerij Wittenbeck ( valerij dot wittenbeck at student dot kit dot edu )
 * @date       2014
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "MoveToNext.h"
 
using namespace armarx;
using namespace PlatformGroup;
 
#include <VirtualRobot/MathTools.h>
 
// DO NOT EDIT NEXT LINE
MoveToNext::SubClassRegistry MoveToNext::Registry(MoveToNext::GetName(), &MoveToNext::CreateInstance);
 
 
 
MoveToNext::MoveToNext(XMLStateConstructorParams stateData) :
    XMLStateTemplate < MoveToNext > (stateData),
    MoveToNextGeneratedBase<MoveToNext>(stateData)
{
}
 
void MoveToNext::visualizeTargetPose(PlatformContext* context, Vector3Ptr& currentTarget, float positionalAccuracy)
{
    Eigen::Matrix3f rot;
    rot = Eigen::AngleAxisf(0, Eigen::Vector3f::UnitX())
          * Eigen::AngleAxisf(0,  Eigen::Vector3f::UnitY())
          * Eigen::AngleAxisf(currentTarget->z, Eigen::Vector3f::UnitZ());
    //    PosePtr targetPose = new Pose(rot, Eigen::Vector3f(currentTarget->x, currentTarget->y, 0));
    //    context->debugDrawer->setPoseVisu("Platform", "PlatformTargetPose", targetPose);
    DrawColor color {0, 0, 1, 0.3};
    const Eigen::Vector3f eigPos
    {
        currentTarget->x, currentTarget->y, 0
    };
    Vector3Ptr pos = new Vector3(eigPos);
    //debug layer
    Eigen::Vector3f dir {0, 1, 0};
    dir = rot * dir;
    //    PosePtr pose = ::armarx::PosePtr{new ::armarx::Pose{rot, eigPos}};
    context->debugDrawer->setArrowVisu("Platform", "PlatformTargetPose", pos,
                                       new armarx::Vector3(dir),
                                       armarx::DrawColor {0, 0, 1, 1},
                                       100,
                                       10);
    context->debugDrawer->setSphereVisu("Platform", "PlatformTargetPoseAccuracy", pos, color, positionalAccuracy);
 
}
 
void MoveToNext::onEnter()
{
 
    PlatformContext* context = getContext<PlatformContext>();
    ChannelRefPtr counter = getInput<ChannelRef>("positionCounter");
    int positionIndex = counter->getDataField("value")->getInt();
    ARMARX_DEBUG << "Entering positionIndex:" << positionIndex << flush;
    SingleTypeVariantListPtr points = getInput<SingleTypeVariantList>("targetPositions");
 
    local.setAdjustedPositionalAccuracy(in.getpositionalAccuracy());
    local.setAdjustedOrientationalAccuracy(in.getorientationalAccuracy());
 
    ARMARX_DEBUG << "points->getSize:" << points->getSize() << flush;
 
    if (positionIndex < points->getSize())
    {
        Vector3Ptr currentTarget = points->getVariant(positionIndex)->get<Vector3>();
        ARMARX_VERBOSE << "going to Platform Target:" << currentTarget->toEigen().transpose() << flush;
        float positionalAccuracy = in.getpositionalAccuracy();
        //context->platformUnitPrx->moveTo(currentTarget->x, currentTarget->y, currentTarget->z, positionalAccuracy, orientationalAccuracy);
        local.setAbsoluteTargetPosition(currentTarget);
        visualizeTargetPose(context, currentTarget, positionalAccuracy);
 
        if (positionIndex < points->getSize() - 1)
        {
            //waypoints
            ARMARX_INFO << "NOT Stopping after this waypoint";
            local.setStopOnWaypoints(false);
            ARMARX_INFO << "Accuracy increase position: " << in.getIntermediateWaypointPositionAccuracyFactor();
            ARMARX_INFO << "Accuracy increase orientation: " << in.getIntermediateWaypointOrientationAccuracyFactor();
            local.setAdjustedPositionalAccuracy(in.getpositionalAccuracy() * in.getIntermediateWaypointPositionAccuracyFactor());
            local.setAdjustedOrientationalAccuracy(in.getorientationalAccuracy() * in.getIntermediateWaypointOrientationAccuracyFactor());
            local.setVerifyTargetPose(false);
        }
        else // last waypoint
        {
            ARMARX_INFO << "Stopping after this waypoint";
            local.setStopOnWaypoints(true);
            local.setVerifyTargetPose(true);
        }
 
    }
    else
    {
        // this is AFTER the last waypoint
 
        Vector3Ptr currentTarget = points->getVariant(points->getSize() - 1)->get<Vector3>();
        local.setAbsoluteTargetPosition(currentTarget);
        cancelSubstates();
        setTimeoutEvent(getInput<int>("waitAfterLast"), createEvent<EvEndpointReached>());
    }
 
    ARMARX_INFO << "Positional accuracy: " << local.getAdjustedPositionalAccuracy();
    ARMARX_INFO << "Orientational accuracy: " << local.getAdjustedOrientationalAccuracy();
 
    context->systemObserverPrx->incrementCounter(counter);
}
 
void MoveToNext::onExit()
{
    PlatformContext* context = getContext<PlatformContext>();
    //context->debugDrawer->removePoseVisu("Platform", "PlatformTargetPose");
    context->debugDrawer->removeSphereVisu("Platform", "PlatformTargetPoseAccuracy");
    context->debugDrawer->removeArrowVisu("Platform", "PlatformTargetPose");
}
 
 
// DO NOT EDIT NEXT FUNCTION
std::string MoveToNext::GetName()
{
    return "MoveToNext";
}
 
// DO NOT EDIT NEXT FUNCTION
XMLStateFactoryBasePtr MoveToNext::CreateInstance(XMLStateConstructorParams stateData)
{
    return XMLStateFactoryBasePtr(new MoveToNext(stateData));
}