Visu.cpp

Go to the documentation of this file.

#include "Visu.h"
 
#include <algorithm>
#include <cmath>
#include <exception>
#include <string>
 
#include <Eigen/Geometry>
 
#include <SimoxUtility/algorithm/apply.hpp>
#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <SimoxUtility/color/Color.h>
#include <SimoxUtility/color/ColorMap.h>
#include <SimoxUtility/color/hsv.h>
#include <SimoxUtility/math/pose.h>
 
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/CycleUtil.h>
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/interface/core/PackagePath.h>
 
#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
 
#include "RobotAPI/components/ArViz/Client/elements/Color.h"
#include "RobotAPI/components/ArViz/Client/elements/PointCloud.h"
#include "RobotAPI/libraries/armem/util/util.h"
#include "RobotAPI/libraries/armem_laser_scans/types.h"
#include "RobotAPI/libraries/core/FramedPose.h"
#include <RobotAPI/components/ArViz/Client/Elements.h>
#include <RobotAPI/interface/components/TrajectoryPlayerInterface.h>
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem_laser_scans/aron/LaserScan.aron.generated.h>
#include <RobotAPI/libraries/armem_laser_scans/aron_conversions.h>
#include <RobotAPI/libraries/armem_laser_scans/util/laser_scanner_conversion.h>
 
namespace armarx::armem::server::laser_scans
{
 
 
    Visu::Visu()
    {
        Logging::setTag("Visu");
    }
 
    void
    Visu::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
    {
        defs->optional(
            p.enabled, prefix + "enabled", "Enable or disable visualization of objects.");
        defs->optional(p.frequencyHz, prefix + "frequenzyHz", "Frequency of visualization.");
        defs->optional(
            p.uniformColor, prefix + "uniformColor", "If enabled, points will be drawn in red.");
        defs->optional(p.maxRobotAgeMs,
                       prefix + "maxRobotAgeMs",
                       "Maximum age of robot state before a new one is retrieved in milliseconds.");
        defs->optional(p.colorByIntensity, "colorByIntensity", "");
        defs->optional(p.pointSizeInPixels, prefix + "pointSizeInPixels", "Point size in pixels.");
    }
 
    void
    Visu::init(const wm::CoreSegment* coreSegment,
               armem::robot_state::VirtualRobotReader* virtualRobotReader)
    {
        this->coreSegment = coreSegment;
        this->virtualRobotReader = virtualRobotReader;
    }
 
    void
    Visu::connect(const viz::Client& arviz, DebugObserverInterfacePrx debugObserver)
    {
        this->arviz = arviz;
        if (debugObserver)
        {
            bool batchMode = true;
            this->debugObserver = DebugObserverHelper("LaserScansMemory", debugObserver, batchMode);
        }
 
        if (updateTask)
        {
            updateTask->stop();
            updateTask->join();
            updateTask = nullptr;
        }
        updateTask = new SimpleRunningTask<>([this]() { this->visualizeRun(); });
        updateTask->start();
    }
 
    void
    Visu::connectRemoteGui(armarx::LightweightRemoteGuiComponentPluginUser* guiUser,
                           const std::string& tabName)
    {
        guiUser_ = guiUser;
        remoteGuiTabName_ = tabName;
    }
 
    void
    Visu::remoteGuiUpdate()
    {
        if (needsTabRebuild_.exchange(false))
        {
            createOrUpdateCalibrationTab();
        }
 
        // Sync widget values back to calibrationData_ whenever any value changed
        bool anyChanged = false;
        for (const auto& [frame, spinboxes] : calibrationTab_.sensors)
        {
            for (const auto& sb : spinboxes)
            {
                anyChanged = anyChanged || sb.hasValueChanged();
            }
        }
 
        if (anyChanged)
        {
            std::lock_guard<std::mutex> lock(sensorMutex_);
            for (const auto& [frame, spinboxes] : calibrationTab_.sensors)
            {
                calibrationData_[frame] = {
                    spinboxes[0].getValue(), spinboxes[1].getValue(), spinboxes[2].getValue()};
            }
        }
    }
 
    void
    Visu::createOrUpdateCalibrationTab()
    {
        using namespace armarx::RemoteGui::Client;
 
        // Snapshot current frames and their calibration data under lock
        std::set<std::string> frames;
        std::map<std::string, SensorCalibrationData> currentData;
        {
            std::lock_guard<std::mutex> lock(sensorMutex_);
            frames = knownSensorFrames_;
            currentData = calibrationData_;
        }
 
        // Rebuild spinboxes from scratch, pre-populated with current values
        calibrationTab_.sensors.clear();
 
        GridLayout grid;
        int row = 0;
 
        grid.add(Label("Sensor"), {.row = row, .column = 0})
            .add(Label("x [mm]"), {.row = row, .column = 1})
            .add(Label("y [mm]"), {.row = row, .column = 2})
            .add(Label("yaw [deg]"), {.row = row, .column = 3});
        ++row;
 
        for (const auto& frame : frames)
        {
            const auto& d = currentData[frame];
            auto& sb = calibrationTab_.sensors[frame];
 
            sb[0].setRange(-10.f, 10.f);
            sb[0].setDecimals(1);
            sb[0].setValue(d.x);
 
            sb[1].setRange(-10.f, 10.f);
            sb[1].setDecimals(1);
            sb[1].setValue(d.y);
 
            sb[2].setRange(-5.f, 5.f);
            sb[2].setDecimals(2);
            sb[2].setValue(d.yaw);
 
            grid.add(Label(frame), {.row = row, .column = 0})
                .add(sb[0], {.row = row, .column = 1})
                .add(sb[1], {.row = row, .column = 2})
                .add(sb[2], {.row = row, .column = 3});
            ++row;
        }
 
        VBoxLayout root = {grid, VSpacer()};
        guiUser_->RemoteGui_createTab(remoteGuiTabName_, root, &calibrationTab_);
    }
 
    Eigen::Isometry3f
    Visu::getCalibrationOffset(const std::string& sensorFrame) const
    {
        std::lock_guard<std::mutex> lock(sensorMutex_);
        const auto it = calibrationData_.find(sensorFrame);
        if (it == calibrationData_.end())
        {
            return Eigen::Isometry3f::Identity();
        }
 
        const auto& d = it->second;
        constexpr float kDegToRad = static_cast<float>(M_PI) / 180.f;
 
        Eigen::Isometry3f offset = Eigen::Isometry3f::Identity();
        offset.translation().x() = d.x;
        offset.translation().y() = d.y;
        offset.linear() =
            Eigen::AngleAxisf(d.yaw * kDegToRad, Eigen::Vector3f::UnitZ()).toRotationMatrix();
        return offset;
    }
 
    void
    Visu::visualizeRun()
    {
        CycleUtil cycle(static_cast<int>(1000 / p.frequencyHz));
        while (updateTask and not updateTask->isStopped())
        {
            if (p.enabled)
            {
                const Time timestamp = Time::Now();
                ARMARX_DEBUG << "Visu task at " << armem::toStringMilliSeconds(timestamp);
 
                try
                {
                    visualizeOnce(timestamp);
                }
                catch (const std::exception& e)
                {
                    ARMARX_WARNING << "Caught exception while visualizing robots: \n" << e.what();
                }
                catch (...)
                {
                    ARMARX_WARNING << "Caught unknown exception while visualizing robots.";
                }
 
                if (debugObserver.has_value())
                {
                    debugObserver->sendDebugObserverBatch();
                }
            }
            cycle.waitForCycleDuration();
        }
    }
 
    void
    Visu::visualizeScan(const std::vector<ScanPoint>& points,
                        const std::string& sensorName,
                        const std::string& agentName,
                        const viz::Color& color)
    {
        viz::PointCloud pointCloud("laser_scan");
 
        ARMARX_VERBOSE << "Point cloud with " << points.size() << " points";
 
        for (const auto& point : points)
        {
 
            // ARMARX_INFO << point.intensity;
            const viz::Color specificColor = [&point, &color, this]() -> viz::Color
            {
                if (p.colorByIntensity)
                {
                    Eigen::Vector3f hsv = simox::color::rgb_to_hsv(
                        Eigen::Vector3f(static_cast<float>(color.r) / 255.f,
                                        static_cast<float>(color.g) / 255.f,
                                        static_cast<float>(color.b) / 255.f));
 
                    // ARMARX_INFO << point.intensity;
 
                    hsv(2) = std::clamp<float>(point.intensity, 0., 1.);
 
                    const Eigen::Vector3f rgb = simox::color::hsv_to_rgb(hsv);
 
                    return viz::Color{rgb(0), rgb(1), rgb(2)};
                }
 
                return color;
            }();
 
            pointCloud.addPoint(point.point.x(), point.point.y(), point.point.z(), specificColor);
        }
 
        pointCloud.pointSizeInPixels(p.pointSizeInPixels);
 
        viz::Layer l = arviz.layer(agentName + "/" + sensorName);
        l.add(pointCloud);
 
        arviz.commit(l);
    }
 
    std::vector<ScanPoint>
    convertScanToGlobal(const armem::laser_scans::LaserScanStamped& scan,
                        const Eigen::Isometry3f& global_T_sensor)
    {
        const auto scanCartesian =
            armarx::armem::laser_scans::util::toCartesian<Eigen::Vector3f>(scan.data);
 
        std::vector<ScanPoint> points;
        points.reserve(scan.data.size());
 
        for (std::size_t i = 0; i < scan.data.size(); i++)
        {
            const auto& point = scanCartesian.at(i);
            const auto& raw = scan.data.at(i);
 
            const Eigen::Vector3f pointGlobal = global_T_sensor * point;
            points.push_back(ScanPoint{.point = pointGlobal, .intensity = raw.intensity});
        }
 
        return points;
    }
 
    // void Segment::getLatestObjectPoses(const wm::CoreSegment& coreSeg, ObjectPoseMap& out)
    // {
    //     coreSeg.forEachProviderSegment([&out](const wm::ProviderSegment & provSegment)
    //     {
    //         getLatestObjectPoses(provSegment, out);
    //     });
    // }
 
 
    // void Segment::getLatestObjectPoses(const wm::ProviderSegment& provSegment, ObjectPoseMap& out)
    // {
    //     provSegment.forEachEntity([&out](const wm::Entity & entity)
    //     {
    //         if (!entity.empty())
    //         {
    //             ObjectPose pose = getLatestObjectPose(entity);
    //             // Try to insert. Fails and returns false if an entry already exists.
    //             const auto [it, success] = out.insert({pose.objectID, pose});
    //             if (!success)
    //             {
    //                 // An entry with that ID already exists. We keep the newest.
    //                 if (it->second.timestamp < pose.timestamp)
    //                 {
    //                     it->second = pose;
    //                 }
    //             }
    //         }
    //     });
    // }
 
 
    // void Segment::getLatestObjectPose(const wm::Entity& entity, ObjectPose& out)
    // {
    //     entity.getLatestSnapshot().forEachInstance([&out](const wm::EntityInstance & instance)
    //     {
    //         arondto::ObjectInstance dto;
    //         dto.fromAron(instance.data());
 
    //         fromAron(dto, out);
    //     });
    // }
 
    std::map<std::string, armem::laser_scans::LaserScanStamped>
    Visu::getCurrentLaserScans()
    {
        ARMARX_CHECK_NOT_NULL(coreSegment);
 
        const auto convert = [this](const wm::EntityInstance& entityInstance)
            -> ::armarx::armem::laser_scans::LaserScanStamped
        {
            const std::optional<armarx::armem::laser_scans::arondto::LaserScanStamped> dto =
                tryCast<armarx::armem::laser_scans::arondto::LaserScanStamped>(entityInstance);
 
            ARMARX_CHECK(dto.has_value());
 
            ::armarx::armem::laser_scans::LaserScanStamped laserScanStamped;
            fromAron(dto.value(), laserScanStamped);
 
            const auto ndArrayNavigator =
                aron::data::NDArray::DynamicCast(entityInstance.data()->getElement("scan"));
 
            ARMARX_CHECK_NOT_NULL(ndArrayNavigator);
 
            laserScanStamped.data =
                armarx::armem::laser_scans::fromAron<LaserScanStep>(ndArrayNavigator);
 
            ARMARX_VERBOSE << "Number of steps: " << laserScanStamped.data.size();
 
            return laserScanStamped;
        };
 
        std::map<std::string, armem::laser_scans::LaserScanStamped> scans;
 
        const auto applyToInstance = [&](const wm::EntityInstance& instance)
        {
            const auto scan = convert(instance);
            scans[instance.id().providerSegmentName + "/" + instance.id().entityName] = scan;
        };
 
        const auto applyToEntity = [&](const wm::Entity& entity)
        {
            if (entity.empty())
            {
                return;
            }
 
            const auto& snapshot = entity.getLatestSnapshot();
 
            snapshot.forEachInstance(applyToInstance);
        };
 
        const auto applyToProviderSegment = [&](const auto& providerSegment)
        { providerSegment.forEachEntity(applyToEntity); };
 
        coreSegment->forEachProviderSegment(applyToProviderSegment);
 
        ARMARX_VERBOSE << scans.size() << " scans";
        return scans;
    }
 
    void
    Visu::visualizeOnce(const Time& timestamp)
    {
        std::map<std::string, armem::laser_scans::LaserScanStamped> currentLaserScans =
            getCurrentLaserScans();
 
        // Register newly discovered sensor frames and schedule a GUI rebuild
        if (guiUser_)
        {
            bool newSensors = false;
            {
                std::lock_guard<std::mutex> lock(sensorMutex_);
                for (const auto& [provider, scan] : currentLaserScans)
                {
                    if (knownSensorFrames_.insert(scan.header.frame).second)
                    {
                        calibrationData_[scan.header.frame] = {};
                        newSensors = true;
                    }
                }
            }
            if (newSensors)
            {
                needsTabRebuild_ = true;
            }
        }
 
        int i = 0;
 
        for (const auto& [provider, scan] : currentLaserScans)
        {
            ARMARX_VERBOSE << "Visualizing `" << provider << "`";
 
            const auto global_T_sensor = [&]() -> Eigen::Isometry3f
            {
                const auto robot = getSynchronizedRobot(scan.header.agent, scan.header.timestamp);
                if (not robot)
                {
                    ARMARX_VERBOSE << deactivateSpam(1) << "Robot `" << scan.header.agent << "`"
                                   << "not available";
                    return Eigen::Isometry3f::Identity();
                }
 
                const auto sensorNode = robot->getRobotNode(scan.header.frame);
                ARMARX_CHECK_NOT_NULL(sensorNode) << "No robot node `" << scan.header.frame
                                                  << "` for robot `" << scan.header.agent << "`";
 
                ARMARX_VERBOSE << "Sensor position for sensor `" << scan.header.frame << "` is "
                               << sensorNode->getGlobalPosition();
                return Eigen::Isometry3f{sensorNode->getGlobalPose()};
            }();
 
            const Eigen::Isometry3f global_T_sensorCalibrated =
                global_T_sensor * getCalibrationOffset(scan.header.frame);
 
            const std::vector<ScanPoint> points =
                convertScanToGlobal(scan, global_T_sensorCalibrated);
 
            const auto color = [&]() -> simox::Color
            {
                if (p.uniformColor)
                {
                    return simox::Color::red();
                }
 
                return simox::color::GlasbeyLUT::at(i++);
            }();
 
            visualizeScan(points, scan.header.frame, scan.header.agent, color);
        }
    }
 
    VirtualRobot::RobotPtr
    Visu::getSynchronizedRobot(const std::string& name, const DateTime& timestamp)
    {
        if (robots.count(name) == 0)
        {
            ARMARX_CHECK_NOT_NULL(virtualRobotReader);
            const auto robot = virtualRobotReader->getRobot(name);
 
            if (robot)
            {
                robots[name] = {robot, DateTime::Invalid()};
            }
            else
            {
                return nullptr;
            }
        }
 
        auto& entry = robots.at(name);
        if (entry.second.isInvalid() ||
            (timestamp - entry.second) > Duration::MilliSeconds(p.maxRobotAgeMs))
        {
            if (virtualRobotReader->synchronizeRobotPose(*entry.first, timestamp))
            {
                entry.second = timestamp;
            }
            else
            {
                ARMARX_INFO << deactivateSpam(10) << "Failed to synchronize robot `" << name << "`";
            }
        }
        return entry.first;
    }
 
} // namespace armarx::armem::server::laser_scans