GetObjectPose.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 "GetObjectPose.h"
 
#include "VisualServoGroupStatechartContext.generated.h"
 
 
using namespace armarx;
using namespace VisualServoGroup;
 
 
// DO NOT EDIT NEXT LINE
GetObjectPose::SubClassRegistry GetObjectPose::Registry(GetObjectPose::GetName(),
                                                        &GetObjectPose::CreateInstance);
 
GetObjectPose::GetObjectPose(XMLStateConstructorParams stateData) :
    XMLStateTemplate<GetObjectPose>(stateData), GetObjectPoseGeneratedBase<GetObjectPose>(stateData)
{
}
 
void
GetObjectPose::onEnter()
{
    VisualServoGroupStatechartContext* context = getContext<VisualServoGroupStatechartContext>();
 
    ChannelRefPtr objectInstance = in.getObjectInstanceChannel();
    ARMARX_VERBOSE << "getting Object pose from localization";
    FramedPositionPtr position = objectInstance->get<FramedPosition>("position");
    FramedOrientationPtr orientation = objectInstance->get<FramedOrientation>("orientation");
    float posUncertainty = objectInstance->get<float>("uncertaintyOfPosition");
    ARMARX_INFO << "position Uncertainty " << posUncertainty;
    FramedPosePtr objectPose = new FramedPose(
        orientation->toEigen(), position->toEigen(), position->getFrame(), position->agent);
 
    ARMARX_VERBOSE << "Object pose from localization: " << objectPose->output();
 
    // convert pose to root frame
    objectPose->changeFrame(context->getRobot(), context->getRobot()->getRootNode()->getName());
 
    context->getDebugDrawerTopic()->setPoseDebugLayerVisu("objectPose",
                                                          objectPose->toGlobal(getRobot()));
    out.setObjectPose(objectPose);
    emitObjectPoseAvailable();
}
 
void
GetObjectPose::onBreak()
{
    // put your user code for the breaking point here
    // execution time should be short (<100ms)
}
 
void
GetObjectPose::onExit()
{
    // put your user code for the exit point here
    // execution time should be short (<100ms)
}
 
// DO NOT EDIT NEXT FUNCTION
std::string
GetObjectPose::GetName()
{
    return "GetObjectPose";
}
 
// DO NOT EDIT NEXT FUNCTION
XMLStateFactoryBasePtr
GetObjectPose::CreateInstance(XMLStateConstructorParams stateData)
{
    return XMLStateFactoryBasePtr(new GetObjectPose(stateData));
}