DummyArMarkerLocalizer.cpp

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/>.
 *
 * @package    VisionX::ArmarXObjects::DummyArMarkerLocalizer
 * @author     Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
 * @date       2018
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "DummyArMarkerLocalizer.h"
 
#include <VirtualRobot/math/Helpers.h>
#include <VirtualRobot/math/Line.h>
#include <VirtualRobot/math/LinearInterpolatedOrientation.h>
 
#include <RobotAPI/libraries/core/FramedPose.h>
 
 
using namespace armarx;
using namespace ::visionx;
 
class MyTrajectory : public math::SimpleAbstractFunctionR1R6
{
public:
    MyTrajectory(std::vector<Eigen::Matrix4f> poses, float duration) : duration(duration)
    {
        for (size_t i = 0; i < poses.size(); i++)
        {
            posFuncts.emplace_back(
                math::Line::FromPoses(poses.at(i), poses.at((i + 1) % poses.size())));
            oriFuncts.emplace_back(math::LinearInterpolatedOrientation(
                math::Helpers::GetOrientation(poses.at(i)),
                math::Helpers::GetOrientation(poses.at((i + 1) % poses.size())),
                0,
                1,
                true));
        }
    }
 
    Eigen::Vector3f
    GetPosition(float t) override
    {
        t = fmod(t / duration, 1.0f) * posFuncts.size();
        int i = std::min((int)t, (int)posFuncts.size() - 1);
        float f = t - i;
        return posFuncts.at(i).Get(f);
    }
 
    Eigen::Quaternionf
    GetOrientation(float t) override
    {
        t = fmod(t / duration, 1.0f) * posFuncts.size();
        int i = std::min((int)t, (int)posFuncts.size() - 1);
        float f = t - i;
        return oriFuncts.at(i).Get(f);
    }
 
private:
    std::vector<math::Line> posFuncts;
    std::vector<math::LinearInterpolatedOrientation> oriFuncts;
    float duration;
};
 
void
DummyArMarkerLocalizer::onInitComponent()
{
    agentName = getProperty<std::string>("AgentName").getValue();
    frameName = getProperty<std::string>("FrameName").getValue();
 
    Eigen::Vector3f p1(470, 723, 991);
    Eigen::Vector3f p2(100, 0, 0);
    Eigen::Vector3f p3(100, 0, 100);
    Eigen::Vector3f p4(0, 0, 100);
    float duration = 10.0f;
    std::vector<Eigen::Matrix4f> poses;
 
    poses.emplace_back(math::Helpers::CreatePose(p1, Eigen::Matrix3f::Identity()));
    poses.emplace_back(math::Helpers::CreatePose(
        p1 + p2,
        Eigen::Matrix3f::Identity() * Eigen::AngleAxisf(-0.25 * M_PI, Eigen::Vector3f::UnitY())));
    poses.emplace_back(math::Helpers::CreatePose(
        p1 + p3,
        Eigen::Matrix3f::Identity() * Eigen::AngleAxisf(-0.5 * M_PI, Eigen::Vector3f::UnitY())));
    poses.emplace_back(math::Helpers::CreatePose(
        p1 + p4,
        Eigen::Matrix3f::Identity() * Eigen::AngleAxisf(-0.75 * M_PI, Eigen::Vector3f::UnitY())));
 
    trajectory.reset(new MyTrajectory(poses, duration));
}
 
void
DummyArMarkerLocalizer::onConnectComponent()
{
    startTime = TimeUtil::GetTime();
}
 
void
DummyArMarkerLocalizer::onDisconnectComponent()
{
}
 
void
DummyArMarkerLocalizer::onExitComponent()
{
}
 
armarx::PropertyDefinitionsPtr
DummyArMarkerLocalizer::createPropertyDefinitions()
{
    return armarx::PropertyDefinitionsPtr(
        new DummyArMarkerLocalizerPropertyDefinitions(getConfigIdentifier()));
}
 
void
visionx::DummyArMarkerLocalizer::reportImageAvailable(const std::string&, const Ice::Current&)
{
    ARMARX_WARNING << "This is not a real image processor. Ignoring any input image!";
}
 
ArMarkerLocalizationResultList
visionx::DummyArMarkerLocalizer::LocalizeAllMarkersNow(const Ice::Current&)
{
    ArMarkerLocalizationResultList result;
    ArMarkerLocalizationResult marker;
    marker.id = 1234;
    float t = (TimeUtil::GetTime() - startTime).toSecondsDouble();
    marker.pose = new FramedPose(
        math::Helpers::CreatePose(trajectory->GetPosition(t), trajectory->GetOrientation(t)),
        frameName,
        agentName);
    result.push_back(marker);
    return result;
}
 
ArMarkerLocalizationResultList
DummyArMarkerLocalizer::GetLatestLocalizationResult(const Ice::Current&)
{
    return LocalizeAllMarkersNow(Ice::emptyCurrent);
}