VisualServoTowardsTargetPose.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::GraspObjectGroup
 * @author     David ( david dot schiebener at kit dot edu )
 * @date       2014
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "VisualServoTowardsTargetPose.h"
 
#include <IceUtil/UUID.h>
#include <VirtualRobot/RobotNodeSet.h>
 
#include "VisualServoGroupStatechartContextBase.generated.h"
 
using namespace armarx;
using namespace VisualServoGroup;
 
 
// DO NOT EDIT NEXT LINE
VisualServoTowardsTargetPose::SubClassRegistry
    VisualServoTowardsTargetPose::Registry(VisualServoTowardsTargetPose::GetName(),
                                           &VisualServoTowardsTargetPose::CreateInstance);
 
VisualServoTowardsTargetPose::VisualServoTowardsTargetPose(
    const XMLStateConstructorParams& stateData) :
    XMLStateTemplate<VisualServoTowardsTargetPose>(stateData),
    VisualServoTowardsTargetPoseGeneratedBase<VisualServoTowardsTargetPose>(stateData)
{
}
 
void
VisualServoTowardsTargetPose::onEnter()
{
    if (!in.isTcpTargetPoseSet() && !in.isTcpTargetPositionSet())
    {
        ARMARX_ERROR << "Either TcpTargetPose or TcpTargetPosition has to be set for VisualServo, "
                        "but none is set.";
        cancelSubstates();
        emitFailure();
        return;
    }
 
 
    VisualServoGroupStatechartContextBase* context =
        getContext<VisualServoGroupStatechartContextBase>();
 
    std::string kinematicChainName = in.getKinematicChainName();
    if (!in.isTCPNameInRobotModelSet())
    {
        std::string tcpName =
            context->getLocalRobot()->getRobotNodeSet(kinematicChainName)->getTCP()->getName();
        local.setTcpNameInRobotModel(tcpName);
    }
    else
    {
        local.setTcpNameInRobotModel(in.getTCPNameInRobotModel());
    }
 
    local.setSpeedModificationFactorHandNotLocalized(0.3f);
    local.setSpeedModificationFactorHandPoseUncertain(0.5f);
 
    if (in.isStartTimeRefSet())
    {
        ARMARX_INFO << "Using existing StartTime for Visual Servo.";
        local.setTimerCreated(false);
    }
    else
    {
        ARMARX_INFO << "Creating new StartTime for Visual Servo.";
        in.setStartTimeRef(
            getSystemObserver()->startTimer("VisualServoTimer_" + IceUtil::generateUUID()));
        local.setTimerCreated(true);
    }
 
    if (context->getTCPControlUnit()->isRequested())
    {
        ARMARX_INFO << "TCPControlUnit is already requested.";
        local.setTCPControlUnitRequested(false);
    }
    else
    {
        ARMARX_INFO << "Requesting TCPControlUnit.";
        context->getTCPControlUnit()->request();
        local.setTCPControlUnitRequested(true);
    }
 
    // TCP visu
    std::string chainName = in.getKinematicChainName();
    memoryx::PersistentObjectClassSegmentBasePrx classesSegmentPrx =
        context->getPriorKnowledge()->getObjectClassesSegment();
    auto tcpObjectClass = classesSegmentPrx->getObjectClassByName(
        in.getHandMemoryChannel()->getDataField("className")->getString());
    if (tcpObjectClass)
    {
        context->getEntityDrawerTopic()->setObjectVisu(
            "VisualServoHandVisu", "tcpTarget_" + chainName, tcpObjectClass, new Pose());
        context->getEntityDrawerTopic()->updateObjectTransparency(
            "VisualServoHandVisu", "tcpTarget_" + chainName, 0.3);
    }
}
 
void
VisualServoTowardsTargetPose::onExit()
{
    if (local.getTimerCreated())
    {
        ARMARX_INFO << "Removing timer.";
        getSystemObserver()->removeTimer(in.getStartTimeRef());
    }
    if (local.getTCPControlUnitRequested())
    {
        ARMARX_INFO << "Releasing TCPControlUnit.";
        getTcpControlUnit()->release();
    }
 
    VisualServoGroupStatechartContextBase* context =
        getContext<VisualServoGroupStatechartContextBase>();
    context->getDebugDrawerTopic()->removeLayer("VisualServo");
    context->getEntityDrawerTopic()->removeLayer("VisualServoHandVisu");
}
 
// DO NOT EDIT NEXT FUNCTION
std::string
VisualServoTowardsTargetPose::GetName()
{
    return "VisualServoTowardsTargetPose";
}
 
// DO NOT EDIT NEXT FUNCTION
XMLStateFactoryBasePtr
VisualServoTowardsTargetPose::CreateInstance(XMLStateConstructorParams stateData)
{
    return XMLStateFactoryBasePtr(new VisualServoTowardsTargetPose(stateData));
}