Component.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    navigation::ArmarXObjects::dynamic_scene_provider
 * @author     Fabian Reister ( fabian dot reister at kit dot edu )
 * @date       2022
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
 
#include "Component.h"
 
#include <string>
#include <vector>
 
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/time/Clock.h>
#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/core/time/Duration.h>
#include <ArmarXCore/core/time/ScopedStopWatch.h>
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include <armarx/navigation/components/dynamic_scene_provider/ArVizDrawer.h>
#include <armarx/navigation/core/basic_types.h>
#include <armarx/navigation/human/HumanTracker.h>
#include <armarx/navigation/human/types.h>
#include <armarx/navigation/memory/client/laser_scanner_features/Reader.h>
#include <armarx/navigation/memory/types.h>
 
#include <VisionX/libraries/armem_human/client/HumanPoseReader.h>
#include <VisionX/libraries/armem_human/types.h>
 
namespace armarx::navigation::components::dynamic_scene_provider
{
 
    const std::string Component::defaultName = "dynamic_scene_provider";
 
    Component::Component()
    {
        addPlugin(humanPoseReaderPlugin);
        addPlugin(laserScannerFeaturesReaderPlugin);
        addPlugin(virtualRobotReaderPlugin);
        addPlugin(costmapReaderPlugin);
        addPlugin(occupancyGridReaderPlugin);
        addPlugin(humanWriterPlugin);
        addPlugin(laserScannerFeaturesWriterPlugin);
    }
 
    armarx::PropertyDefinitionsPtr
    Component::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr def =
            new armarx::ComponentPropertyDefinitions(getConfigIdentifier());
 
        // Publish to a topic (passing the TopicListenerPrx).
        // def->topic(myTopicListener);
 
        // Subscribe to a topic (passing the topic name).
        // def->topic<PlatformUnitListener>("MyTopic");
 
        // Use (and depend on) another component (passing the ComponentInterfacePrx).
        // def->component(myComponentProxy)
 
 
        // Add a required property. (The component won't start without a value being set.)
        // def->required(properties.boxLayerName, "p.box.LayerName", "Name of the box layer in ArViz.");
 
        // Add an optionalproperty.
        def->optional(
            properties.taskPeriodMs, "p.taskPeriodMs", "Update rate of the running task.");
 
        def->optional(properties.laserScannerFeatures.enabled,
                      "p.laserScannerFeatures.enabled",
                      "Whether laser scanner features are used.");
        def->optional(properties.laserScannerFeatures.providerName,
                      "p.laserScannerFeatures.providerName",
                      "");
        def->optional(properties.laserScannerFeatures.name, "p.laserScannerFeatures.name", "");
 
        def->required(properties.robot.name, "p.robot.name", "");
 
        def->optional(properties.occupancyGrid.providerName, "p.occupancyGrid.providerName", "");
        def->optional(properties.occupancyGrid.name, "p.occupancyGrid.name", "");
        def->optional(
            properties.occupancyGrid.freespaceThreshold, "p.occupancyGrid.freespaceThreshold", "");
        def->optional(
            properties.occupancyGrid.occupiedThreshold, "p.occupancyGrid.occupiedThreshold", "");
 
        def->optional(
            properties.humanPoses.enabled, "p.humanPoses.enabled", "Whether human poses are used.");
        def->optional(properties.humanPoses.providerName, "p.humanPoses.providerName", "");
 
        return def;
    }
 
    void
    Component::onInitComponent()
    {
        // Topics and properties defined above are automagically registered.
 
        // Keep debug observer data until calling `sendDebugObserverBatch()`.
        // (Requies the armarx::DebugObserverComponentPluginUser.)
        setDebugObserverBatchModeEnabled(true);
 
        usingProxy("RobotStateMemory");
    }
 
    void
    Component::onConnectComponent()
    {
        arvizDrawer.emplace(ArVizDrawer(getArvizClient()));
 
        // Do things after connecting to topics and components.
 
        /* (Requies the armarx::DebugObserverComponentPluginUser.)
        // Use the debug observer to log data over time.
        // The data can be viewed in the ObserverView and the LivePlotter.
        // (Before starting any threads, we don't need to lock mutexes.)
        {
            setDebugObserverDatafield("numBoxes", properties.numBoxes);
            setDebugObserverDatafield("boxLayerName", properties.boxLayerName);
            sendDebugObserverBatch();
        }
        */
 
        /* (Requires the armarx::ArVizComponentPluginUser.)
        // Draw boxes in ArViz.
        // (Before starting any threads, we don't need to lock mutexes.)
        drawBoxes(properties, arviz);
        */
 
        /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
        // Setup the remote GUI.
        {
            createRemoteGuiTab();
            RemoteGui_startRunningTask();
        }
        */
 
        while (true)
        {
            robot = virtualRobotReaderPlugin->get().getRobotWaiting(properties.robot.name);
            if (robot != nullptr)
            {
                break;
            }
 
            Clock::WaitFor(Duration::MilliSeconds(100));
        }
        ARMARX_CHECK_NOT_NULL(robot);
 
        humanTracker.reset();
 
        task = new PeriodicTask<Component>(
            this, &Component::runPeriodically, properties.taskPeriodMs, false, "runningTask");
        task->start();
    }
 
    void
    Component::onDisconnectComponent()
    {
        task->stop();
    }
 
    void
    Component::onExitComponent()
    {
    }
 
    std::string
    Component::getDefaultName() const
    {
        return Component::defaultName;
    }
 
    std::string
    Component::GetDefaultName()
    {
        return Component::defaultName;
    }
 
    void
    Component::runPeriodically()
    {
        const auto logDuration = [this](const std::string& name, const Duration& duration)
        { setDebugObserverDatafield("timing." + name + " [ms]", duration.toMilliSeconds()); };
 
        const auto makeSWlog = [&](const std::string& name)
        { return [=](const Duration& duration) { logDuration(name, duration); }; };
 
        armarx::core::time::ScopedStopWatch sw(makeSWlog("dynamic_scene.full"));
 
        // obtain data from perception
        const DateTime timestamp = Clock::Now();
 
        //
        // Robot
        //
        ARMARX_TRACE;
        {
            armarx::core::time::ScopedStopWatch sw(makeSWlog("dynamic_scene.read_robot"));
 
            if (not virtualRobotReaderPlugin->get().synchronizeRobotPose(*robot, timestamp))
            {
                ARMARX_INFO << "Failed to synchronize robot to timestamp " << QUOTED(timestamp)  << ".";
                return;
            }
 
            const core::Pose global_T_robot(robot->getGlobalPose());
 
            ARMARX_VERBOSE << "Robot position: " << global_T_robot.translation().head<2>();
        }
 
        //
        // Human
        //
        ARMARX_TRACE;
        const std::vector<armem::human::HumanPose> humanPoses = [&]
        {
            if (!properties.humanPoses.enabled)
            {
                return std::vector<armem::human::HumanPose>{};
            }
 
            armarx::core::time::ScopedStopWatch sw(makeSWlog("dynamic_scene.read_human"));
 
            ARMARX_VERBOSE << "Querying humans";
            const armem::human::client::Reader::Query humanPoseQuery{
                .providerName = properties.humanPoses.providerName,
                .timestamp = timestamp,
                .maxAge = Duration::MilliSeconds(500)};
 
            const armem::human::client::Reader::Result humanPoseResult =
                humanPoseReaderPlugin->get().query(humanPoseQuery);
            ARMARX_CHECK(humanPoseResult.status !=
                         armem::human::client::Reader::Result::Status::Error)
                << humanPoseResult.errorMessage;
 
            ARMARX_VERBOSE << humanPoseResult.humanPoses.size() << " humans in the scene.";
 
            return humanPoseResult.humanPoses;
        }();
 
        //
        // Laser scanner features
        //
        ARMARX_TRACE;
        const memory::LaserScannerFeatures laserFeatures = [&]() -> memory::LaserScannerFeatures
        {
            if (!properties.laserScannerFeatures.enabled)
            {
                return memory::LaserScannerFeatures{};
            }
 
            armarx::core::time::ScopedStopWatch sw(makeSWlog("dynamic_scene.read_laserscanner"));
 
            ARMARX_VERBOSE << "Querying laser scanner features";
            const memory::client::laser_scanner_features::Reader::Query laserFeaturesQuery{
                .providerName = properties.laserScannerFeatures.providerName,
                .name = properties.laserScannerFeatures.name,
                .timestamp = timestamp};
 
            const memory::client::laser_scanner_features::Reader::Result laserFeaturesResult =
                laserScannerFeaturesReaderPlugin->get().queryData(laserFeaturesQuery);
 
            if (laserFeaturesResult.status !=
                memory::client::laser_scanner_features::Reader::Result::Success)
            {
                ARMARX_VERBOSE << "Querying laser scanner features failed. Reason: "
                               << laserFeaturesResult.errorMessage;
                return {};
            }
 
            // "Happy" case: data is available
            ARMARX_CHECK_EQUAL(laserFeaturesResult.status,
                               memory::client::laser_scanner_features::Reader::Result::Success)
                << laserFeaturesResult.errorMessage;
 
            ARMARX_VERBOSE << laserFeaturesResult.features.size() << " clusters/features";
 
            if (laserFeaturesResult.features.empty())
            {
                ARMARX_VERBOSE << "No laser scanner features available";
                return {};
            }
 
            ARMARX_CHECK_EQUAL(laserFeaturesResult.features.size(), 1)
                << "We request the merged instance with only one result";
 
            return laserFeaturesResult.features.front();
        }();
 
 
        /* we don't need this at the moment
 
        //
        //  Objects in the scene (both static and dynamic)
        //
        ARMARX_INFO << "Querying object poses";
 
        const objpose::ObjectPoseSeq objectPoses = ObjectPoseClientPluginUser::getObjectPoses();
 
        // remove those objects that belong to an object dataset. the manipulation object / distance computation is broken
        const auto objectPosesStatic =
            armarx::navigation::util::filterObjects(objectPoses, {"KIT", "HOPE", "MDB", "YCB"});
 
        const auto objects = armarx::navigation::util::asSceneObjects(objectPosesStatic);
        ARMARX_INFO << objects->getSize() << " objects in the scene";
 
        // ARMARX_INFO << "Creating costmap";
        // ARMARX_CHECK_NOT_NULL(scene.robot);
 
        // algorithms::CostmapBuilder costmapBuilder(
        //     scene.robot,
        //     objects,
        //     algorithms::Costmap::Parameters{.binaryGrid = false, .cellSize = 100},
        //     "Platform-navigation-colmodel");
 
        // // const auto costmap = costmapBuilder.create();
 
        //
        //  Costmaps
        //
 
        ARMARX_INFO << "Querying costmap";
 
        const memory::client::costmap::Reader::Query costmapQuery{
            .providerName = "distance_to_obstacle_costmap_provider", // TODO check
            .name = "distance_to_obstacles",
            .timestamp = timestamp};
 
        const memory::client::costmap::Reader::Result costmapResult =
            costmapReaderPlugin->get().query(costmapQuery);
 
        ARMARX_CHECK_EQUAL(costmapResult.status, memory::client::costmap::Reader::Result::Success);
 
        ARMARX_TRACE;
        ARMARX_CHECK(costmapResult.costmap.has_value());
        const auto grid = costmapResult.costmap->getGrid();
 
 
        //
        // Occupancy grid: from SLAM component
        //
 
        const armem::vision::occupancy_grid::client::Reader::Result result =
            occupancyGridReaderPlugin->get().query(
                armem::vision::occupancy_grid::client::Reader::Query{
                    .providerName = properties.occupancyGrid.providerName,
                    .timestamp = armem::Time::Now()});
 
        if (result and result.occupancyGrid.has_value())
        {
            ARMARX_INFO << "Occupancy grid available!";
 
            const auto occupancyGridSceneElements = util::asSceneObjects(
                result.occupancyGrid.value(),
                OccupancyGridHelper::Params{
                    .freespaceThreshold = properties.occupancyGrid.freespaceThreshold,
                    .occupiedThreshold = properties.occupancyGrid.occupiedThreshold});
            ARMARX_INFO << occupancyGridSceneElements->getSize()
                        << " scene elements from occupancy grid";
 
            auto occupancyGridObstacles =
                std::make_shared<VirtualRobot::SceneObjectSet>("OccupancyGridObstacles");
            occupancyGridObstacles->addSceneObjects(occupancyGridSceneElements);
 
            // draw
            // auto layer = arviz.layer("occupancy_grid");
 
            // for (const auto& sceneObject : occupancyGridSceneElements->getSceneObjects())
            // {
            //     const Eigen::Isometry3f world_T_obj(sceneObject->getGlobalPose());
            //     ARMARX_INFO << world_T_obj.translation();
            //     ARMARX_INFO << layer.size();
            //     layer.add(viz::Box("box_" + std::to_string(layer.size()))
            //                   .pose(world_T_obj)
            //                   .size(result.occupancyGrid->resolution)
            //                   .color(viz::Color::orange()));
            // }
 
            // ARMARX_INFO << "Creating costmap";
 
            // algorithms::CostmapBuilder costmapBuilder(
            //     getRobot(),
            //     scene.objects,
            //     algorithms::Costmap::Parameters{.binaryGrid = false, .cellSize = 100},
            //     "Platform-navigation-colmodel");
 
            // const auto costmap = costmapBuilder.create();
 
            // ARMARX_INFO << "Done";
 
            // ARMARX_TRACE;
            // ARMARX_INFO << "Saving costmap.";
            // algorithms::save(costmap, "/tmp/navigation-costmap");
 
            // arviz.commit({layer});
        }
        */
 
        // here ends: data fetching
 
 
        // process data from perception and write information back to memory
 
        //
        // Human tracking
        //
        ARMARX_TRACE;
        if (properties.humanPoses.enabled)
        {
            armarx::core::time::ScopedStopWatch sw(makeSWlog("dynamic_scene.human_tracker.camera"));
 
            ARMARX_VERBOSE << "Running human tracker with camera measurements";
            humanTracker.update(human::HumanTracker::CameraMeasurement{.detectionTime = timestamp,
                                                                       .humanPoses = humanPoses});
        }
 
 
        ARMARX_TRACE;
        const std::vector<memory::LaserScannerFeature> unusedFeatures = [&]
        {
            if (!properties.laserScannerFeatures.enabled)
            {
                return std::vector<memory::LaserScannerFeature>{};
            }
 
            armarx::core::time::ScopedStopWatch sw(
                makeSWlog("dynamic_scene.human_tracker.laserscanner"));
 
            ARMARX_VERBOSE << "Running human tracker with lasersensor measurements";
 
            return humanTracker.update(human::HumanTracker::LaserMeasurement{
                .detectionTime = timestamp, .clusters = laserFeatures.features});
        }();
 
 
        ARMARX_VERBOSE << "Human tracking done";
 
 
        //
        // Write dynamic scene back to memory
        //
        ARMARX_TRACE;
        {
            armarx::core::time::ScopedStopWatch sw(makeSWlog("dynamic_scene.write_back_human"));
 
            //TODO use clusters for obstacle creation
            std::vector<human::Human> humans = humanTracker.getTrackedHumans();
 
            if (not humans.empty())
            {
                ARMARX_VERBOSE << "Detected " << humans.size() << " humans";
                humanWriterPlugin->get().store(humans, getName(), timestamp);
            }
 
            if (not unusedFeatures.empty())
            {
                ARMARX_VERBOSE << "Detected " << unusedFeatures.size()
                               << " laser scanner features not associated with humans";
                //TODO(groeger): check frame, do we need it?
                laserScannerFeaturesWriterPlugin->get().store(
                    memory::LaserScannerFeatures{.frame = "global",
                                                 .frameGlobalPose = Eigen::Isometry3f::Identity(),
                                                 .features = unusedFeatures},
                    getName(),
                    timestamp);
            }
        }
 
        sendDebugObserverBatch();
    }
 
    /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
    void
    Component::createRemoteGuiTab()
    {
        using namespace armarx::RemoteGui::Client;
 
        // Setup the widgets.
 
        tab.boxLayerName.setValue(properties.boxLayerName);
 
        tab.numBoxes.setValue(properties.numBoxes);
        tab.numBoxes.setRange(0, 100);
 
        tab.drawBoxes.setLabel("Draw Boxes");
 
        // Setup the layout.
 
        GridLayout grid;
        int row = 0;
        {
            grid.add(Label("Box Layer"), {row, 0}).add(tab.boxLayerName, {row, 1});
            ++row;
 
            grid.add(Label("Num Boxes"), {row, 0}).add(tab.numBoxes, {row, 1});
            ++row;
 
            grid.add(tab.drawBoxes, {row, 0}, {2, 1});
            ++row;
        }
 
        VBoxLayout root = {grid, VSpacer()};
        RemoteGui_createTab(getName(), root, &tab);
    }
 
 
    void
    Component::RemoteGui_update()
    {
        if (tab.boxLayerName.hasValueChanged() || tab.numBoxes.hasValueChanged())
        {
            std::scoped_lock lock(propertiesMutex);
            properties.boxLayerName = tab.boxLayerName.getValue();
            properties.numBoxes = tab.numBoxes.getValue();
 
            {
                setDebugObserverDatafield("numBoxes", properties.numBoxes);
                setDebugObserverDatafield("boxLayerName", properties.boxLayerName);
                sendDebugObserverBatch();
            }
        }
        if (tab.drawBoxes.wasClicked())
        {
            // Lock shared variables in methods running in seperate threads
            // and pass them to functions. This way, the called functions do
            // not need to think about locking.
            std::scoped_lock lock(propertiesMutex, arvizMutex);
            drawBoxes(properties, arviz);
        }
    }
    */
 
 
    /* (Requires the armarx::ArVizComponentPluginUser.)
    void
    Component::drawBoxes(const Component::Properties& p, viz::Client& arviz)
    {
        // Draw something in ArViz (requires the armarx::ArVizComponentPluginUser.
        // See the ArVizExample in RobotAPI for more examples.
 
        viz::Layer layer = arviz.layer(p.boxLayerName);
        for (int i = 0; i < p.numBoxes; ++i)
        {
            layer.add(viz::Box("box_" + std::to_string(i))
                      .position(Eigen::Vector3f(i * 100, 0, 0))
                      .size(20).color(simox::Color::blue()));
        }
        arviz.commit(layer);
    }
    */
 
 
    ARMARX_REGISTER_COMPONENT_EXECUTABLE(Component, Component::GetDefaultName());
 
} // namespace armarx::navigation::components::dynamic_scene_provider