FramedOrientedPoint.h

Go to the documentation of this file.

/*
* This file is part of ArmarX.
*
* 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/>.
*
* @author     Martin Miller (martin dot miller at student dot kit dot edu)
* @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#pragma once
 
 
#include "OrientedPoint.h"
 
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/LinkedCoordinate.h>
#include <VirtualRobot/LinkedCoordinate.h>
#include <VirtualRobot/VirtualRobot.h>
 
#include <RobotAPI/interface/core/RobotState.h>
#include "FramedPose.h"
 
namespace armarx::VariantType
{
    // variant types
    const VariantTypeId FramedOrientedPoint = Variant::addTypeName("::armarx::FramedOrientedPointBase");
}
 
namespace armarx
{
    class FramedOrientedPoint;
    using FramedOrientedPointPtr = IceInternal::Handle<FramedOrientedPoint>;
 
    class FramedOrientedPoint :
        virtual public FramedOrientedPointBase,
        virtual public OrientedPoint
    {
    public:
        FramedOrientedPoint();
        FramedOrientedPoint(const FramedOrientedPoint& source);
        FramedOrientedPoint(const Eigen::Vector3f& position, const Eigen::Vector3f& normal, const std::string& frame, const std::string& agent);
        FramedOrientedPoint(const OrientedPoint& pointWithNormal, const std::string& frame, const std::string& agent);
        FramedOrientedPoint(Ice::Float px, ::Ice::Float py, ::Ice::Float pz, Ice::Float nx, ::Ice::Float ny, ::Ice::Float nz, const std::string& frame, const std::string& agent);
 
        std::string getFrame() const;
        void changeFrame(const VirtualRobot::RobotPtr& robot, const std::string& newFrame);
 
        void changeToGlobal(const SharedRobotInterfacePrx& referenceRobot);
        void changeToGlobal(const VirtualRobot::RobotPtr& referenceRobot);
        FramedOrientedPointPtr toGlobal(const SharedRobotInterfacePrx& referenceRobot) const;
        FramedOrientedPointPtr toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const;
        Eigen::Vector3f positionToGlobalEigen(const SharedRobotInterfacePrx& referenceRobot) const;
        Eigen::Vector3f normalToGlobalEigen(const SharedRobotInterfacePrx& referenceRobot) const;
        Eigen::Vector3f positionToGlobalEigen(const VirtualRobot::RobotPtr& referenceRobot) const;
        Eigen::Vector3f normalToGlobalEigen(const VirtualRobot::RobotPtr& referenceRobot) const;
        FramedOrientedPointPtr toRootFrame(const SharedRobotInterfacePrx& referenceRobot) const;
        FramedOrientedPointPtr toRootFrame(const VirtualRobot::RobotPtr& referenceRobot) const;
        Eigen::Vector3f positionToRootEigen(const SharedRobotInterfacePrx& referenceRobot) const;
        Eigen::Vector3f normalToRootEigen(const SharedRobotInterfacePrx& referenceRobot) const;
        Eigen::Vector3f positionToRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const;
        Eigen::Vector3f normalToRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const;
 
        FramedPosition getFramedPosition() const;
        FramedDirection getFramedNormal() const;
 
        // inherited from VariantDataClass
        Ice::ObjectPtr ice_clone() const override
        {
            return this->clone();
        }
        VariantDataClassPtr clone(const Ice::Current& c = Ice::emptyCurrent) const override
        {
            return new FramedOrientedPoint(*this);
        }
        std::string output(const Ice::Current& c = Ice::emptyCurrent) const override;
        VariantTypeId getType(const Ice::Current& c = Ice::emptyCurrent) const override
        {
            return VariantType::FramedOrientedPoint;
        }
        bool validate(const Ice::Current& c = Ice::emptyCurrent) override
        {
            return true;
        }
 
        friend std::ostream& operator<<(std::ostream& stream, const FramedOrientedPoint& rhs)
        {
            stream << "FramedOrientedPoint: " << std::endl << rhs.output() << std::endl;
            return stream;
        }
 
    public: // serialization
        void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) const override;
        void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) override;
 
    };
}