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::human_simulator
 * @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 <algorithm>
#include <cstddef>
#include <cstdlib>
#include <string>
#include <vector>
 
#include <SimoxUtility/color/Color.h>
#include <VirtualRobot/BoundingBox.h>
#include <VirtualRobot/CollisionDetection/CollisionModel.h> // IWYU pragma: keep
#include <VirtualRobot/SceneObjectSet.h> // IWYU pragma: keep
 
#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/libraries/DecoupledSingleComponent/Decoupled.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include "RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h"
#include <RobotAPI/components/ArViz/Client/Elements.h>
#include <RobotAPI/components/ArViz/Client/elements/Path.h>
#include <RobotAPI/libraries/ArmarXObjects/forward_declarations.h>
#include <RobotAPI/libraries/ArmarXObjects/util.h>
 
#include <armarx/navigation/algorithms/Costmap.h>
#include <armarx/navigation/core/basic_types.h>
#include <armarx/navigation/human/types.h>
#include <armarx/navigation/memory/client/costmap/Reader.h>
#include <armarx/navigation/simulation/SimulatedHuman.h>
 
#include <RVOSimulator.h>
#include <Vector2.h>
 
// Include headers you only need in function definitions in the .cpp.
 
// #include <Eigen/Core>
 
// #include <SimoxUtility/color/Color.h>
 
 
namespace armarx::navigation::components::human_simulator
{
    Component::Component()
    {
        addPlugin(virtualRobotReaderPlugin);
        addPlugin(costmapReaderPlugin);
        addPlugin(humanWriterPlugin);
    }
 
    const std::string Component::defaultName = "human_simulator";
 
    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", "");
        def->optional(properties.nHumans, "p.nHumans", "Number of humans to spawn.");
 
        def->optional(
            properties.humanParams.goalDistanceThreshold, "p.human.goalDistanceThreshold", "");
        def->optional(properties.humanParams.minLinearVelocity, "p.human.maxLinearVelocity", "");
        def->optional(properties.humanParams.maxLinearVelocity, "p.human.maxLinearVelocity", "");
 
        def->optional(properties.objectPoseProviderName, "p.objectPoseProviderName", "");
 
        def->required(properties.robotName, "p.robotName", "");
 
        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);
    }
 
    void
    Component::setPreferredVelocities(RVO::RVOSimulator* simulator,
                                      const std::vector<RVO::Vector2>& goals)
    {
        /* Set the preferred velocity to be a vector of unit magnitude (speed) in the
   * direction of the goal. */
        // #ifdef _OPENMP
        // #pragma omp parallel for
        // #endif /* _OPENMP */
 
        for (int i = 0; i < static_cast<int>(properties.nHumans); ++i)
        {
            // RVO::Vector2 goalVector = goals[i] - simulator->getAgentPosition(i);
 
            // if (RVO::absSq(goalVector) > 1.0F)
            // {
            //     goalVector = RVO::normalize(goalVector);
            // }
 
            const auto agentPosition = simulator->getAgentPosition(i);
            auto& simulatedHuman = simulatedHumans.at(i);
 
            core::Pose2D pose = core::Pose2D::Identity();
            pose.translation().x() = agentPosition.x() * 1000; // [m] -> [mm]
            pose.translation().y() = agentPosition.y() * 1000; // [m] -> [mm]
 
            simulatedHuman.reinitialize(pose);
 
            const std::vector<core::Position> trajectoryPoints =
                simulatedHuman.trajectory().positions();
 
            const std::size_t lookAheadGoalIdx = std::min(trajectoryPoints.size() - 1, 5ul);
            const auto& intermediateGoal = trajectoryPoints.at(lookAheadGoalIdx);
 
            const Eigen::Vector2f rvoGoalHelper = intermediateGoal.head<2>() / 1000; // [mm] -> [m]
 
            const RVO::Vector2 rvoGoal(rvoGoalHelper.x(), rvoGoalHelper.y());
 
            RVO::Vector2 goalVector = rvoGoal - agentPosition;
 
            if (RVO::absSq(goalVector) > 1.0F)
            {
                goalVector = RVO::normalize(goalVector);
            }
 
            simulator->setAgentPrefVelocity(i, goalVector);
        }
    }
 
    void
    Component::onConnectComponent()
    {
        //
        //  Costmaps
        //
 
        ARMARX_INFO << "Querying costmap";
 
        const algorithms::Costmap costmap = [&]() -> algorithms::Costmap
        {
            while (true)
            {
                const auto timestamp = Clock::Now();
 
                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);
 
                // if costmap is not available yet, wait
                if (costmapResult.status != memory::client::costmap::Reader::Result::Success)
                {
                    Clock::WaitFor(Duration::MilliSeconds(100));
                    continue;
                }
 
                ARMARX_CHECK_EQUAL(costmapResult.status,
                                   memory::client::costmap::Reader::Result::Success);
 
                ARMARX_TRACE;
                ARMARX_CHECK(costmapResult.costmap.has_value());
                return costmapResult.costmap.value();
            };
        }();
 
        /// create simulation environment
        {
            ARMARX_INFO << "Creating sim";
 
            simulator.setTimeStep(static_cast<float>(properties.taskPeriodMs) / 1000);
            simulator.setAgentDefaults(15.0F, 10U, 1.5F, .1F, .4F, .5F);
 
            const objpose::ObjectPoseSeq obstacles =
                ObjectPoseClientPluginUser::getObjectPosesByProvider(
                    properties.objectPoseProviderName);
 
            const auto objects = armarx::objpose::util::asSceneObjects(obstacles);
 
 
            ARMARX_VERBOSE << obstacles.size() << " obstacles";
 
            {
                auto layer = arviz.layer("obstacles");
 
 
                for (const auto& obst : objects->getCollisionModels())
                {
 
                    ARMARX_INFO << "adding obstacle ...";
                    const VirtualRobot::BoundingBox bbox = obst->getGlobalBoundingBox();
 
                    Eigen::Vector2f bbMin = bbox.getMin().head<2>();
                    Eigen::Vector2f bbMax = bbox.getMax().head<2>();
 
                    // add some margin to the obstacles
                    const float margin = 100; // [mm]
 
                    bbMin.x() -= margin;
                    bbMin.y() -= margin;
                    bbMax.x() += margin;
                    bbMax.y() += margin;
 
                    ARMARX_VERBOSE << "Bounding box: (" << bbMin.transpose() << "), ("
                                   << bbMax.transpose() << ")";
 
                    {
                        constexpr float zHeight = 3; // [mm]
 
                        viz::Polygon polygon(std::to_string(layer.size()));
                        polygon.addPoint(Eigen::Vector3f{bbMin.x(), bbMin.y(), zHeight});
                        polygon.addPoint(Eigen::Vector3f{bbMax.x(), bbMin.y(), zHeight});
                        polygon.addPoint(Eigen::Vector3f{bbMax.x(), bbMax.y(), zHeight});
                        polygon.addPoint(Eigen::Vector3f{bbMin.x(), bbMax.y(), zHeight});
 
                        polygon.color(simox::Color::black());
                        polygon.color(simox::Color::black());
 
                        layer.add(polygon);
                    }
 
 
                    const Eigen::Vector2d min = bbMin.cast<double>() / 1000; // [mm] -> [m]
                    const Eigen::Vector2d max = bbMax.cast<double>() / 1000; // [mm] -> [m]
 
                    std::vector<RVO::Vector2> obstacle;
                    obstacle.emplace_back(min.x(), min.y());
                    obstacle.emplace_back(max.x(), min.y());
                    obstacle.emplace_back(max.x(), max.y());
                    obstacle.emplace_back(min.x(), max.y());
 
                    simulator.addObstacle(obstacle);
                }
 
                arviz.commit(layer);
            }
 
            simulator.processObstacles();
        }
 
        /// create humans in simulation
        {
            simulatedHumans.clear();
 
            for (std::size_t i = 0U; i < properties.nHumans; ++i)
            {
                armarx::navigation::human::simulation::SimulatedHuman& human =
                    simulatedHumans.emplace_back(costmap);
 
                const RVO::Vector2 position =
                    RVO::Vector2(human.human().pose.translation().x() / 1000,
                                 human.human().pose.translation().y() / 1000);
                auto goal = human.trajectory().positions().back() / 1000; // [mm] -> [m]
 
                ARMARX_VERBOSE << "Adding agent";
                // simulator.addAgent(position, simulator.addGoal(hrvo::Vector2(goal.x(), goal.y())));
                simulator.addAgent(
                    position); // , simulator.addGoal(hrvo::Vector2(goal.x(), goal.y())));
 
                goals.emplace_back(goal.x(), goal.y());
            }
        }
 
        /// load robot and add it to the simulator
        {
            robot = virtualRobotReaderPlugin->get().getRobotWaiting(properties.robotName);
            ARMARX_CHECK(
                virtualRobotReaderPlugin->get().synchronizeRobot(*robot, armarx::Clock::Now()));
 
            const auto robotPosH = robot->getGlobalPosition();
            const RVO::Vector2 robotPos(robotPosH.x() / 1000, robotPosH.y() / 1000);
 
            robotAgentId = simulator.addAgent(robotPos);
            simulator.setAgentMaxSpeed(robotAgentId,
                                       0); // we do not allow the robot to be perturbed
            simulator.setAgentPrefVelocity(robotAgentId, RVO::Vector2(0, 0));
            simulator.setAgentRadius(robotAgentId, 0.6);
        }
 
        /// start task
        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 DateTime timestamp = Clock::Now();
 
        setPreferredVelocities(&simulator, goals);
 
        // TODO(fabian.reister): check how much the agents interact => this can be an evaluation criterion
 
        // update robot
        {
            ARMARX_CHECK(
                virtualRobotReaderPlugin->get().synchronizeRobot(*robot, armarx::Clock::Now()));
 
            const auto robotPosH = robot->getGlobalPosition();
            const RVO::Vector2 robotPos(robotPosH.x() / 1000, robotPosH.y() / 1000);
 
            simulator.setAgentPosition(robotAgentId, robotPos);
        }
 
        simulator.doStep();
 
        // visualize paths
        {
            auto layer = arviz.layer("trajectories");
            for (const auto& simulatedHuman : simulatedHumans)
            {
                viz::Path path(std::to_string(layer.size()));
 
                for (const auto& pt : simulatedHuman.trajectory().points())
                {
                    path.addPoint(pt.waypoint.pose.translation());
                }
 
                // unique and distinct color for this human
                path.colorGlasbeyLUT(layer.size());
 
                layer.add(path);
            }
 
            arviz.commit(layer);
        }
 
 
        human::Humans humans;
        for (std::size_t i = 0; i < properties.nHumans; i++)
        {
            const auto pos = simulator.getAgentPosition(i);
 
            human::Human human;
            human.pose.setIdentity();
            human.pose.translation().x() = pos.x() * 1000; // [m] -> [mm]
            human.pose.translation().y() = pos.y() * 1000; // [m] -> [mm]
 
            humans.push_back(human);
        }
 
        ARMARX_VERBOSE << "Updating humans.";
        humanWriterPlugin->get().store(humans, getName(), timestamp);
    }
 
    /* (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::human_simulator