Visualization.cpp

Go to the documentation of this file.

#include "Visualization.h"
 
#include <chrono>
#include <cstddef>
#include <mutex>
#include <thread>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/TimeUtil.h>
 
#include <RobotAPI/components/ArViz/Client/Client.h>
#include <RobotAPI/components/ArViz/Client/Layer.h>
#include <RobotAPI/components/ArViz/Client/elements/Color.h>
#include <RobotAPI/components/ArViz/Client/elements/Robot.h>
 
#include <armarx/navigation/core/Trajectory.h>
#include <armarx/navigation/core/basic_types.h>
 
namespace armarx::navigation::util
{
 
    Visualization::Visualization(armarx::viz::Client& arviz, const Params& params) :
        params(params), arviz(arviz)
    {
        robotVis = viz::Robot("robot").file(params.robotModelFileName.serialize().package,
                                            params.robotModelFileName.serialize().path);
        robotVis.useCollisionModel();
        robotVis.hide();
        robotVis.overrideColor(viz::Color::white(0));
 
 
        arviz.commitLayerContaining("target", robotVis);
    }
 
    void
    Visualization::visualize(const core::GlobalTrajectory& trajectory)
    {
        // TODO(fabian.reister): more finegrained locking
        std::lock_guard g{paramsMutex};
 
        if (!params.enableVisualization)
        {
            return;
        }
 
        if (trajectory.poses().size() < 2)
        {
            ARMARX_WARNING << "Unable to visualize path with less than two segments";
            return;
        }
 
        // viz::Layer pathLayer = arviz.layer("path");
        // for (size_t i = 1; i < path.size(); i++)
        // {
        //     Eigen::Vector3f prevPose = Eigen::Vector3f::Zero();
        //     prevPose.head<2>() = path.at(i - 1).head<2>();
        //     Eigen::Vector3f currentPose = Eigen::Vector3f::Zero();
        //     currentPose.head<2>() = path.at(i).head<2>();
        //     viz::Arrow a = viz::Arrow("path_segment_" + std::to_string(i));
        //     a.color(viz::Color::orange());
        //     a.width(25);
        //     a.fromTo(prevPose, currentPose);
        //     pathLayer.add(a);
        // }
 
        // arviz.commit({pathLayer});
 
        visualizeTrajectory(trajectory);
    }
 
    void
    Visualization::visualizeTrajectory(const core::GlobalTrajectory& trajectory)
    {
        if (!params.enableTrajectoryVisualization)
        {
            return;
        }
 
        viz::Layer robotLayer = arviz.layer("trajectory");
 
        robotVis.overrideColor(viz::Color::white(params.alphaValue));
        robotVis.show();
        robotLayer.add(robotVis);
 
        const auto resampledTrajectory = trajectory.resample(10);
        // const auto duration = trajectory.duration(core::VelocityInterpolation::LastWaypoint);
 
        const float speedup = 1.0;
 
        ARMARX_INFO << "Visualizing trajectory";
 
        const auto points = resampledTrajectory.points();
        for (size_t i = 0; i < (points.size() - 1); i++)
        {
            const auto& wp = points.at(i);
            robotVis.pose(wp.waypoint.pose.matrix());
 
            std::lock_guard<std::mutex> lock(mutex);
            arviz.commit({robotLayer});
 
            const float distance =
                (wp.waypoint.pose.translation() - points.at(i + 1).waypoint.pose.translation())
                    .norm();
 
            ARMARX_CHECK(wp.velocity > 0.F);
            const float velocity = 200;
            const int dt = static_cast<int>(speedup * distance / velocity); // [ms]
 
            std::this_thread::sleep_for(std::chrono::milliseconds(dt));
        }
 
        ARMARX_DEBUG << "Done visualizing trajectory";
 
        robotVis.hide();
 
        std::lock_guard<std::mutex> lock(mutex);
        arviz.commit({robotLayer});
    }
 
    void
    Visualization::asyncHideDelayed()
    {
        std::thread{
            [this]
            {
                std::this_thread::sleep_for(std::chrono::seconds(params.hideDelaySeconds));
 
                std::lock_guard<std::mutex> lock(mutex);
 
                if (lastUpdate <= (TimeUtil::GetTime().toSeconds() - params.hideDelaySeconds))
                {
                    robotVis.hide();
                    // arviz.commitLayerContaining("target", robotVis);
                    arviz.commit(arviz.layer("target"));
                    arviz.commit(arviz.layer("path"));
                }
            }}
            .detach();
    }
 
    void
    Visualization::success()
    {
        // TODO(fabian.reister): more finegrained locking
        std::lock_guard g{paramsMutex};
 
        if (!params.enableVisualization)
        {
            return;
        }
 
        robotVis.overrideColor(viz::Color::green(255, params.alphaValue));
        {
            std::lock_guard<std::mutex> lock(mutex);
            arviz.commitLayerContaining("target", robotVis);
        }
        asyncHideDelayed();
    }
 
    void
    Visualization::failed()
    {
        // TODO(fabian.reister): more finegrained locking
        std::lock_guard g{paramsMutex};
 
        if (!params.enableVisualization)
        {
            return;
        }
        robotVis.overrideColor(viz::Color::red(255, params.alphaValue));
        {
            std::lock_guard<std::mutex> lock(mutex);
            arviz.commitLayerContaining("target", robotVis);
        }
 
        asyncHideDelayed();
    }
 
    void
    Visualization::setTarget(const core::Pose& m)
    {
        // TODO(fabian.reister): more finegrained locking
        std::lock_guard g{paramsMutex};
 
        if (!params.enableVisualization)
        {
            return;
        }
 
        robotVis.show();
        robotVis.overrideColor(viz::Color::azure(128, params.alphaValue));
        robotVis.pose(m.matrix());
 
        std::lock_guard<std::mutex> lock(mutex);
        arviz.commitLayerContaining("target", robotVis);
 
        lastUpdate = TimeUtil::GetTime().toSeconds();
    }
 
 
} // namespace armarx::navigation::util