Visu.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/>.
 *
 * @author     Rainer Kartmann ( rainer dot kartmann at kit dot edu )
 * @date       2021
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "Visu.h"
 
#include <string>
 
#include <SimoxUtility/color/Color.h>
 
#include <RobotAPI/components/ArViz/Client/Elements.h>
#include <RobotAPI/components/ArViz/Client/Layer.h>
 
#include <armarx/navigation/core/LocationUtils.h>
#include <armarx/navigation/core/basic_types.h>
#include <armarx/navigation/graph/Visu.h>
#include <armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.h>
 
namespace armarx::navigation::qt_plugins::location_graph_editor
{
 
    const simox::Color visu::defaultColorHighlighted = simox::Color::orange();
 
    viz::Pose
    VertexVisu::Pose::draw(const std::string& name,
                           const core::Pose& pose,
                           const VertexData& attribs) const
    {
        viz::Pose p = navigation::graph::VertexVisu::Pose::draw(attribs.getLocationName(), pose);
        if (attribs.highlighted)
        {
            p.scale(scale * scaleFactorHighlighted);
        }
        return p;
    }
 
    viz::Arrow
    VertexVisu::ForwardArrow::draw(const std::string& name,
                                   const core::Pose& pose,
                                   const VertexData& attribs) const
    {
        viz::Arrow arrow =
            navigation::graph::VertexVisu::ForwardArrow::draw(attribs.getLocationName(), pose);
        if (attribs.highlighted)
        {
            arrow.color(colorHighlighted);
            arrow.width(width * 1.5f);
        }
        return arrow;
    }
 
    void
    VertexVisu::draw(viz::Layer& layer,
                     GuiGraph::ConstVertex vertex,
                     graph::visu::ObjectParserInfo info) const
    {
        const auto p = core::resolveLocation(info.objects, info.info, vertex.attrib().getPose());
        if (p.pose.has_value())
        {
            draw(layer, vertex.attrib().getLocationName(), p.pose.value(), vertex.attrib());
        }
    }
 
    void
    VertexVisu::draw(viz::Layer& layer,
                     const std::string& name,
                     const core::Pose& pose,
                     const VertexData& attribs) const
    {
        if (this->pose.has_value())
        {
            layer.add(this->pose->draw(name, pose, attribs));
        }
        if (forwardArrow.has_value())
        {
            layer.add(forwardArrow->draw(name, pose, attribs));
        }
    }
 
    viz::Arrow
    EdgeVisu::Arrow::draw(const EdgeData& edge,
                          const std::string& sourceName,
                          const std::string& targetName,
                          const core::Pose& source,
                          const core::Pose& target) const
    {
        viz::Arrow arrow =
            navigation::graph::EdgeVisu::Arrow::draw(sourceName, targetName, source, target);
        if (edge.highlighted)
        {
            arrow.color(colorHighlighted);
            arrow.width(width * 1.5f);
        }
        return arrow;
    }
 
    void
    EdgeVisu::draw(viz::Layer& layer,
                   GuiGraph::ConstEdge edge,
                   graph::visu::ObjectParserInfo info) const
    {
        const auto src =
            core::resolveLocation(info.objects, info.info, edge.source().attrib().getPose());
        const auto tar =
            core::resolveLocation(info.objects, info.info, edge.target().attrib().getPose());
        if (src.pose.has_value() && tar.pose.has_value() && arrow.has_value())
        {
            layer.add(arrow->draw(edge.attrib(),
                                  edge.source().attrib().getLocationName(),
                                  edge.target().attrib().getLocationName(),
                                  src.pose.value(),
                                  tar.pose.value()));
        }
    }
 
    void
    GraphVisu::draw(viz::Layer& layer,
                    const GuiGraph& graph,
                    graph::visu::ObjectParserInfo info) const
    {
        if (vertex.has_value())
        {
            for (GuiGraph::ConstVertex v : graph.vertices())
            {
                vertex->draw(layer, v, info);
            }
        }
        if (edge.has_value())
        {
            for (GuiGraph::ConstEdge e : graph.edges())
            {
                edge->draw(layer, e, info);
            }
        }
    }
 
 
} // namespace armarx::navigation::qt_plugins::location_graph_editor