TrajectoryFamilyVisualization.h

Go to the documentation of this file.

/*
 * Copyright (C) by
 *   MetraLabs GmbH (MLAB), GERMANY
 *  and
 *   Neuroinformatics and Cognitive Robotics Labs (NICR) at TU Ilmenau, GERMANY
 * All rights reserved.
 *
 * Redistribution and modification of this code is strictly prohibited.
 *
 * IN NO EVENT SHALL "MLAB" OR "NICR" BE LIABLE TO ANY PARTY FOR DIRECT,
 * INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES ARISING OUT OF
 * THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF "MLAB" OR
 * "NICR" HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * "MLAB" AND "NICR" SPECIFICALLY DISCLAIM ANY WARRANTIES, INCLUDING,
 * BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
 * FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS
 * ON AN "AS IS" BASIS, AND "MLAB" AND "NICR" HAVE NO OBLIGATION TO
 * PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS OR MODIFICATIONS.
 */

#ifndef _MIRA_PILOT_TRAJECTORYFAMILYVISUALIZATION_H_
#define _MIRA_PILOT_TRAJECTORYFAMILYVISUALIZATION_H_

#include <math/Saturate.h>
#include <image/Colormap.h>

#include <serialization/DefaultInitializer.h>

#include <visualization/3d/LineStripObject.h>
#include <visualization/3d/TextObject.h>

#include <widgets/OgreUtils.h>

#include <pilot/TrajectoryFamily.h>

#include <OGRE/OgreSceneManager.h>
#include <OGRE/OgreSceneNode.h>
#include <OGRE/OgreHardwarePixelBuffer.h>
#include <OGRE/OgreTechnique.h>
#include <OGRE/OgreMaterialManager.h>
#include <OGRE/OgreTextureManager.h>
#include <OGRE/OgreManualObject.h>

#include <gui/HighlightProperty.h>

namespace mira { namespace pilot {


class TrajectoryFamilyVisualizationCommonBase
{
    class HighlightProperty : public mira::HighlightProperty
    {
    public:
        HighlightProperty(TrajectoryFamilyVisualizationCommonBase* obj)
        : mira::HighlightProperty(&TrajectoryFamilyVisualizationCommonBase::redraw, obj),
          mDashed(false), mColor2(0.0f, 0.0f, 0.0f, 1.0f) {}

        template <typename Reflector>
        void reflect(Reflector& r)
        {
            MIRA_REFLECT_BASE(r, mira::HighlightProperty);

            r.property("Dashed", mDashed,
                       setterNotify(mDashed, mNotifyChange),
                       "Show each trajectory sample interval by dashing",
                       false);
    
            r.property("Color2", mColor2,
                       setterNotify(mColor2, mNotifyChange),
                       "The second color for drawing the selected trajectory in dashed mode",
                       Ogre::ColourValue(0.0f, 0.0f, 0.0f, 1.0f));
        }
    
    public:
        bool isDashed() const { return mDashed; }
        const Ogre::ColourValue& color2() const { return mColor2; }
    
    protected:
        bool mDashed;
        Ogre::ColourValue mColor2;
    };

public:
    TrajectoryFamilyVisualizationCommonBase(Visualization3D* visualization)
        : mHideAll(false), mHideNotReachable(true), mHideNotAdmissible(true), mZOffset(0.01f),
          mEndPointMarkerSize(0.01f), mHighlightBest(true),
          mHighlight(this),
          mHighlightedTrajectory(NULL),
          mText(NULL),
          mVisualization(visualization)
    {
    }

    virtual ~TrajectoryFamilyVisualizationCommonBase();

    template <typename Reflector>
    void reflect(Reflector& r)
    {
        r.property("Hide all", mHideAll,
                   setterNotify(mHideAll, &TrajectoryFamilyVisualizationCommonBase::redraw, this),
                   "Hide all trajectories (except highlight)", false);
        r.property("Hide not reachable", mHideNotReachable,
                   setterNotify(mHideNotReachable, &TrajectoryFamilyVisualizationCommonBase::redraw, this),
                   "Hide unreachable trajectories", true);
        r.property("Hide not admissible", mHideNotAdmissible,
                   setterNotify(mHideNotAdmissible, &TrajectoryFamilyVisualizationCommonBase::redraw, this),
                   "Hide not admissible trajectories", true);
        r.property("ZOffset", mZOffset,
                   setterNotify(mZOffset, &TrajectoryFamilyVisualizationCommonBase::redraw, this),
                   "Raise trajectories above map", 0.f);
        r.property("EndPoint Marker Size", mEndPointMarkerSize,
                   setterNotify(mEndPointMarkerSize, &TrajectoryFamilyVisualizationCommonBase::redraw, this),
                   "Size of marker for trajectory end point (improve visibility of trajectory swarm)", 0.01f,
                   PropertyHints::minimum(0.f));
        r.property("HighlightBest", mHighlightBest,
                   setterNotify(mHighlightBest, &TrajectoryFamilyVisualizationCommonBase::redraw, this),
                   "Highlight the best trajectory (manual highlight selection takes preference, if active)", true);
        r.property("Highlight", mHighlight,
                   "Select a single trajectory to highlight", HighlightProperty(this));
    }

protected:
    void setupScene(Ogre::SceneManager* mgr, Ogre::SceneNode* node, std::string name);

    void renderTrajectory(const robot::PoseVelocityTrajectory& t, const Ogre::ColourValue& color);

    void redraw() { drawLast(); }

    virtual void drawLast() = 0;

protected:
    bool mHideAll;
    bool mHideNotReachable;
    bool mHideNotAdmissible;
    float mZOffset;
    float mEndPointMarkerSize;
    bool mHighlightBest;
    Ogre::ManualObject* mManualObject;
    HighlightProperty mHighlight;
    LineStripObject* mHighlightedTrajectory;
    TextObject* mText;
    Visualization3D* mVisualization;
};


namespace detail {

template<typename Trajectory,
         bool NeedDeref = IsPointerOrSharedPointer<Trajectory>::value>
struct TrajectoryAccessHelper
{
    static const TrajectoryInfo& derefIfNeeded(const Trajectory& t) { return t; }
};

template<typename Trajectory>
struct TrajectoryAccessHelper<Trajectory, true>
{
    static const TrajectoryInfo& derefIfNeeded(const Trajectory& t) { return *t; }
};

}


template <typename Trajectory = TrajectoryInfo>
class TrajectoryFamilyVisualizationBase : public TrajectoryFamilyVisualizationCommonBase
{
public:
    TrajectoryFamilyVisualizationBase(Visualization3D* visualization)
        : TrajectoryFamilyVisualizationCommonBase(visualization)
    {
    }

    virtual ~TrajectoryFamilyVisualizationBase() {}

protected:
    void drawFamily(const std::vector<Trajectory>& f);

    virtual Ogre::ColourValue getColor(const Trajectory& t) = 0;

    void markTrajectory(LineStripObject* line, const Trajectory& t,
                        const Ogre::ColourValue& color,
                        const Ogre::ColourValue& color2);

    virtual void displayData(const Trajectory& t);
};


template <typename Trajectory>
void TrajectoryFamilyVisualizationBase<Trajectory>::drawFamily(const std::vector<Trajectory>& f)
{
    double minCost = std::numeric_limits<double>::max();
    const Trajectory* bestTrajectory = nullptr;

    if (!mHighlight.isEnabled() && mHighlightBest) {
        foreach (const Trajectory& t, f)
        {
            double cost = detail::TrajectoryAccessHelper<Trajectory>::derefIfNeeded(t).costs;
            auto permit = detail::TrajectoryAccessHelper<Trajectory>::derefIfNeeded(t).permit;
            if ((permit == Objective::Permit::ADMISSIBLE) && (cost < minCost)) {
                minCost = cost;
                bestTrajectory = &t;
            }
        }
    }

    mManualObject->clear();
    if (!mHideAll) {
        mManualObject->begin("NoLight", Ogre::RenderOperation::OT_LINE_LIST);

        foreach (const Trajectory& t, f)
        {
            Ogre::ColourValue c = getColor(t);

            if (c.a <= 0.f)
                continue;

            renderTrajectory(detail::TrajectoryAccessHelper<Trajectory>::derefIfNeeded(t).trajectory, c);

            if (mEndPointMarkerSize <= 0.f)
                continue;

            const auto& traj = detail::TrajectoryAccessHelper<Trajectory>::derefIfNeeded(t).trajectory;
            if (traj.empty())
                continue;

            const auto& p = traj.back().p.t;
            for (float angle = 0; angle < deg2rad(360.0); angle += deg2rad(10.0))
            {
                mManualObject->position(Ogre::Vector3(p.x() + cos(angle) * mEndPointMarkerSize,
                                                      p.y() + sin(angle) * mEndPointMarkerSize, mZOffset));
                mManualObject->colour(c);
                mManualObject->position(Ogre::Vector3(p.x() + cos(angle + deg2rad(10.0)) * mEndPointMarkerSize,
                                                      p.y() + sin(angle + deg2rad(10.0)) * mEndPointMarkerSize, mZOffset));
                mManualObject->colour(c);
            }
        }

        mManualObject->end();
    }

    mHighlight.setWrapIndex(f.size());

    if (mHighlight.isEnabled() && f.size())
    {
        markTrajectory(mHighlightedTrajectory, f[mHighlight.index()], mHighlight.color(),
                       mHighlight.isDashed() ? mHighlight.color2() : mHighlight.color());
        displayData(f[mHighlight.index()]);
    }
    else if (mHighlightBest && (bestTrajectory != nullptr))
    {
        markTrajectory(mHighlightedTrajectory, *bestTrajectory, mHighlight.color(),
                       mHighlight.isDashed() ? mHighlight.color2() : mHighlight.color());
        displayData(*bestTrajectory);
    }
    else
    {
        mHighlightedTrajectory->setVisible(false);
        mText->setVisible(false);
    }
}

template <typename Trajectory>
void TrajectoryFamilyVisualizationBase<Trajectory>::markTrajectory(LineStripObject* line, const Trajectory& t,
                                                                   const Ogre::ColourValue& color,
                                                                   const Ogre::ColourValue& color2)
{
    if(!line)
        return;

    line->clear();
    line->begin();

    Ogre::Vector3 p1(0.0f, 0.0f, 0.0f);
    foreach (const robot::PoseVelocityTrajectorySample& s,
             detail::TrajectoryAccessHelper<Trajectory>::derefIfNeeded(t).trajectory) {
        Ogre::Vector3 p2(s.p.t.x(), s.p.t.y(), mZOffset);
        line->point(p1, color);
        line->point(p2, color2);
        p1=p2;
    }

    line->end();
    line->setVisible(mVisualization->isEnabled());
}

template <typename Trajectory>
void TrajectoryFamilyVisualizationBase<Trajectory>::displayData(const Trajectory& t)
{
    mText->setCharacterHeight(mHighlight.textsize());
    mText->setColor(mHighlight.color());
    mText->setVisible(mVisualization->isEnabled());

    std::stringstream ss;
    const Velocity2& v = detail::TrajectoryAccessHelper<Trajectory>::derefIfNeeded(t).trajectory.back().v;
    ss << "velocity= " << v.x() << ", " << rad2deg(v.phi()) << "deg" << std::endl;
    ss << "cost=" << detail::TrajectoryAccessHelper<Trajectory>::derefIfNeeded(t).costs;
    switch (detail::TrajectoryAccessHelper<Trajectory>::derefIfNeeded(t).permit)
    {
        case Objective::Permit::ADMISSIBLE     : ss << "\npermit=Admissible";
                                                 break;
        case Objective::Permit::NOT_ADMISSIBLE : ss << "\npermit=NotAdmissible";
                                                 break;
        case Objective::Permit::NOT_REACHABLE  : ss << "\npermit=NotReachable";
                                                 break;
        default                                : ss << "\npermit=Unknown";
                                                 break;
    }

    mText->setCaption(ss.str());
}


}} // namespace

#endif